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]*Volptr[i];
00118                 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00119                 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00120                 count++;
00121               }
00122         }
00123 
00124         if (count == 0) {
00125                 LOGERR("Invalid mask");
00126                 throw ImageFormatException( "Invalid mask");
00127         }
00128 
00129        float avg = static_cast<float>(Sum1/count);
00130        float sig2 = static_cast<float>(Sum2 - count*avg*avg)/(count-1);
00131        float sig = sqrt(sig2);
00132 
00133        stats.push_back(avg);
00134        stats.push_back(sig);
00135        stats.push_back(MIN);
00136        stats.push_back(MAX);
00137 
00138        return stats;
00139 }
00140 
00141 
00142 //----------------------------------------------------------------------------------------------------------
00143 
00144 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00145 {
00146         ENTERFUNC;
00147 
00148         if (!EMUtil::is_same_size(V1, V2)) {
00149                 LOGERR("images not same size");
00150                 throw ImageFormatException( "images not same size");
00151         }
00152 
00153         size_t nx = V1->get_xsize();
00154         size_t ny = V1->get_ysize();
00155         size_t nz = V1->get_zsize();
00156         size_t size = nx*ny*nz;
00157 
00158         EMData *BD = new EMData();
00159         BD->set_size(nx, ny, nz);
00160 
00161         float *params = new float[2];
00162 
00163         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00164         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00165         int nvox = 0L;
00166 
00167         V1ptr = V1->get_data();
00168         V2ptr = V2->get_data();
00169         BDptr = BD->get_data();
00170 
00171 
00172         if(!mask){
00173                 EMData * Mask = new EMData();
00174                 Mask->set_size(nx,ny,nz);
00175                 Mask->to_one();
00176                 MASKptr = Mask->get_data();
00177         } else {
00178                 if (!EMUtil::is_same_size(V1, mask)) {
00179                         LOGERR("mask not same size");
00180                         throw ImageFormatException( "mask not same size");
00181                 }
00182 
00183                 MASKptr = mask->get_data();
00184         }
00185 
00186 
00187 
00188 //       calculation of S1,S2,S3,S3,nvox
00189 
00190         for (size_t i = 0L;i < size; i++) {
00191               if (MASKptr[i]>0.5f) {
00192                S1 += V1ptr[i]*V2ptr[i];
00193                S2 += V1ptr[i]*V1ptr[i];
00194                S3 += V2ptr[i];
00195                S4 += V1ptr[i];
00196                nvox ++;
00197               }
00198         }
00199 
00200         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00201                 A =1.0f ;
00202         } else {
00203                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00204         }
00205         B = static_cast<float> (A*S4  -  S3)/nvox;
00206 
00207         // calculation of the difference image
00208 
00209         for (size_t i = 0L;i < size; i++) {
00210              if (MASKptr[i]>0.5f) {
00211                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00212              }  else  {
00213                BDptr[i] = 0.f;
00214              }
00215         }
00216 
00217         BD->update();
00218 
00219         params[0] = A;
00220         params[1] = B;
00221 
00222         Dict BDnParams;
00223         BDnParams["imdiff"] = BD;
00224         BDnParams["A"] = params[0];
00225         BDnParams["B"] = params[1];
00226 
00227         EXITFUNC;
00228         return BDnParams;
00229  }
00230 
00231 //----------------------------------------------------------------------------------------------------------
00232 
00233 
00234 
00235 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00236 {
00237         ENTERFUNC;
00238         int Mid= (Size+1)/2;
00239 
00240         if (flag==0) { // This is the real function
00241                 EMData* ImBW = new EMData();
00242                 ImBW->set_size(Size,Size,1);
00243                 ImBW->to_zero();
00244 
00245                 float tempIm;
00246                 float x,y;
00247 
00248                 for (int ix=(1-Mid);  ix<Mid; ix++){
00249                         for (int iy=(1-Mid);  iy<Mid; iy++){
00250                                 x = (float)ix;
00251                                 y = (float)iy;
00252                         tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00253                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00254                         }
00255                 }
00256                 ImBW->update();
00257                 ImBW->set_complex(false);
00258                 ImBW->set_ri(true);
00259 
00260                 return ImBW;
00261         }
00262         else if (flag==1) {  // This is the Fourier Transform
00263                 EMData* ImBWFFT = new EMData();
00264                 ImBWFFT ->set_size(2*Size,Size,1);
00265                 ImBWFFT ->to_zero();
00266 
00267                 float r,s;
00268 
00269                 for (int ir=(1-Mid);  ir<Mid; ir++){
00270                         for (int is=(1-Mid);  is<Mid; is++){
00271                                 r = (float)ir;
00272                                 s = (float)is;
00273                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00274                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00275                         }
00276                 }
00277                 ImBWFFT->update();
00278                 ImBWFFT->set_complex(true);
00279                 ImBWFFT->set_ri(true);
00280                 ImBWFFT->set_shuffled(true);
00281                 ImBWFFT->set_fftodd(true);
00282 
00283                 return ImBWFFT;
00284         }
00285         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00286                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00287                 float C=cos(alpha);
00288                 float S=sin(alpha);
00289                 float D= sqrt(S*S*b*b + C*C*a*a);
00290                 //float D2 = D*D;   PAP - to get rid of warning
00291 
00292                 float P = p * C *a*a/D ;
00293                 float Q = q * S *b*b/D ;
00294 
00295                 if (flag==2) {
00296                         EMData* pofalpha = new EMData();
00297                         pofalpha ->set_size(Size,1,1);
00298                         pofalpha ->to_zero();
00299 
00300                         float Norm0 =  D*(float)sqrt(2*pi);
00301                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00302                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00303                         float sD;
00304 
00305                         for (int is=(1-Mid);  is<Mid; is++){
00306                                 sD = is/D ;
00307                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00308                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00309                         }
00310                         pofalpha-> update();
00311                         pofalpha-> set_complex(false);
00312                         pofalpha-> set_ri(true);
00313 
00314                         return pofalpha;
00315                 }
00316                 if (flag==3) { // This is the projection in Fourier Space
00317                         float vD;
00318 
00319                         EMData* pofalphak = new EMData();
00320                         pofalphak ->set_size(2*Size,1,1);
00321                         pofalphak ->to_zero();
00322 
00323                         for (int iv=(1-Mid);  iv<Mid; iv++){
00324                                 vD = iv*D ;
00325                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00326                         }
00327                         pofalphak-> update();
00328                         pofalphak-> set_complex(false);
00329                         pofalphak-> set_ri(true);
00330 
00331                         return pofalphak;
00332                 }
00333         }
00334         else if (flag==4) {
00335                 cout <<" FH under construction";
00336                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00337                 EMData* TryFH= OutFT -> real2FH(4.0);
00338                 return TryFH;
00339         } else {
00340                 cout <<" flag must be 0,1,2,3, or 4";
00341         }
00342 
00343         EXITFUNC;
00344         return 0;
00345 }
00346 
00347 
00348 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00349 {
00350 
00351         float x0= x[0];
00352         float x1= x[1];
00353         float x2= x[2];
00354         float y0= y[0];
00355         float y1= y[1];
00356         float y2= y[2];
00357         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00358         float xn  = x[n];
00359         float xnm1= x[n-1];
00360         float xnm2= x[n-2];
00361         float yn  = y[n];
00362         float ynm1= y[n-1];
00363         float ynm2= y[n-2];
00364         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00365         float *y2d = new float[n];
00366         Util::spline(x,y,n,yp1,ypn,y2d);
00367         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00368         delete [] y2d;
00369         return;
00370 }
00371 
00372 
00373 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00374 {
00375         int i,k;
00376         float p, qn, sig, un, *u;
00377         u = new float[n-1];
00378 
00379         if (yp1 > .99e30){
00380                 y2[0]=u[0]=0.0;
00381         } else {
00382                 y2[0]=-.5f;
00383                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00384         }
00385 
00386         for (i=1; i < n-1; i++) {
00387                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00388                 p = sig*y2[i-1] + 2.0f;
00389                 y2[i]  = (sig-1.0f)/p;
00390                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00391                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00392         }
00393 
00394         if (ypn>.99e30){
00395                 qn=0; un=0;
00396         } else {
00397                 qn= .5f;
00398                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00399         }
00400         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00401         for (k=n-2; k>=0; k--){
00402                 y2[k]=y2[k]*y2[k+1]+u[k];
00403         }
00404         delete [] u;
00405 }
00406 
00407 
00408 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00409 {
00410         int klo, khi, k;
00411         float h, b, a;
00412 
00413 //      klo=0; // can try to put here
00414         for (int j=0; j<m;j++){
00415                 klo=0;
00416                 khi=n-1;
00417                 while (khi-klo >1) {
00418                         k=(khi+klo) >>1;
00419                         if  (xa[k]>xq[j]){ khi=k;}
00420                         else { klo=k;}
00421                 }
00422                 h=xa[khi]- xa[klo];
00423                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00424                 a =(xa[khi]-xq[j])/h;
00425                 b=(xq[j]-xa[klo])/h;
00426                 yq[j]=a*ya[klo] + b*ya[khi]
00427                         + ((a*a*a-a)*y2a[klo]
00428                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00429         }
00430 //      printf("h=%f, a = %f, b=%f, ya[klo]=%f, ya[khi]=%f , yq=%f\n",h, a, b, ya[klo], ya[khi],yq[0]);
00431 }
00432 
00433 
00434 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00435                float *weightofkValsSorted, int Size, int *SizeReturned)
00436 {
00437         int iMax = (int) floor( (Size-1.0)/2 +.01);
00438         int CountMax = (iMax+2)*(iMax+1)/2;
00439         int Count=-1;
00440         float *kVals     = new float[CountMax];
00441         float *weightMat = new float[CountMax];
00442         int *PermMat     = new   int[CountMax];
00443         SizeReturned[0] = CountMax;
00444 
00445 //      printf("Aa \n");        fflush(stdout);
00446         for (int jkx=0; jkx< iMax+1; jkx++) {
00447                 for (int jky=0; jky< jkx+1; jky++) {
00448                         Count++;
00449                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00450                         weightMat[Count]=  1.0;
00451                         if (jkx!=0)  { weightMat[Count] *=2;}
00452                         if (jky!=0)  { weightMat[Count] *=2;}
00453                         if (jkx!=jky){ weightMat[Count] *=2;}
00454                         PermMat[Count]=Count+1;
00455                 }
00456         }
00457 
00458         int lkVals = Count+1;
00459 //      printf("Cc \n");fflush(stdout);
00460 
00461         sort_mat(&kVals[0],&kVals[Count],
00462              &PermMat[0],  &PermMat[Count]);  //PermMat is
00463                                 //also returned as well as kValsSorted
00464         fflush(stdout);
00465 
00466         int newInd;
00467 
00468         for (int iP=0; iP < lkVals ; iP++ ) {
00469                 newInd =  PermMat[iP];
00470                 PermMatTr[newInd-1] = iP+1;
00471         }
00472 
00473 //      printf("Ee \n"); fflush(stdout);
00474 
00475         int CountA=-1;
00476         int CountB=-1;
00477 
00478         while (CountB< (CountMax-1)) {
00479                 CountA++;
00480                 CountB++;
00481 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00482                 kValsSorted[CountA] = kVals[CountB] ;
00483                 if (CountB<(CountMax-1) ) {
00484                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00485                                 SizeReturned[0]--;
00486                                 for (int iP=0; iP < lkVals; iP++){
00487 //                                      printf("iP=%d \n", iP);fflush(stdout);
00488                                         if  (PermMatTr[iP]>CountA+1) {
00489                                                 PermMatTr[iP]--;
00490                                         }
00491                                 }
00492                                 CountB++;
00493                         }
00494                 }
00495         }
00496 
00497 
00498         for (int CountD=0; CountD < CountMax; CountD++) {
00499             newInd = PermMatTr[CountD];
00500             weightofkValsSorted[newInd-1] += weightMat[CountD];
00501         }
00502 
00503 }
00504 
00505 
00506 vector<float>
00507 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00508 {
00509         vector<float> angles;
00510         float psi = 0.0;
00511         if ((0.0 == t1)&&(0.0 == t2)||(t1 >= t2)) {
00512                 t1 = 0.0f;
00513                 t2 = 90.0f;
00514         }
00515         if ((0.0 == p1)&&(0.0 == p2)||(p1 >= p2)) {
00516                 p1 = 0.0f;
00517                 p2 = 359.9f;
00518         }
00519         bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00520         for (float theta = t1; theta <= t2; theta += delta) {
00521                 float detphi;
00522                 int lt;
00523                 if ((0.0 == theta)||(180.0 == theta)) {
00524                         detphi = 360.0f;
00525                         lt = 1;
00526                 } else {
00527                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00528                         lt = int((p2 - p1)/detphi)-1;
00529                         if (lt < 1) lt = 1;
00530                         detphi = (p2 - p1)/lt;
00531                 }
00532                 for (int i = 0; i < lt; i++) {
00533                         float phi = p1 + i*detphi;
00534                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00535                         angles.push_back(phi);
00536                         angles.push_back(theta);
00537                         angles.push_back(psi);
00538                 }
00539         }
00540         return angles;
00541 }
00542 
00543 
00544 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00545 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00546 {
00547 
00548 //  purpose: quadratic interpolation
00549 //
00550 //  parameters:       xx,yy treated as circularly closed.
00551 //                    fdata - image 1..nxdata, 1..nydata
00552 //
00553 //                    f3    fc       f0, f1, f2, f3 are the values
00554 //                     +             at the grid points.  x is the
00555 //                     + x           point at which the function
00556 //              f2++++f0++++f1       is to be estimated. (it need
00557 //                     +             not be in the first quadrant).
00558 //                     +             fc - the outer corner point
00559 //                    f4             nearest x.
00560 c
00561 //                                   f0 is the value of the fdata at
00562 //                                   fdata(i,j), it is the interior mesh
00563 //                                   point nearest  x.
00564 //                                   the coordinates of f0 are (x0,y0),
00565 //                                   the coordinates of f1 are (xb,y0),
00566 //                                   the coordinates of f2 are (xa,y0),
00567 //                                   the coordinates of f3 are (x0,yb),
00568 //                                   the coordinates of f4 are (x0,ya),
00569 //                                   the coordinates of fc are (xc,yc),
00570 c
00571 //                   o               hxa, hxb are the mesh spacings
00572 //                   +               in the x-direction to the left
00573 //                  hyb              and right of the center point.
00574 //                   +
00575 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00576 //                   +               in the y-direction.
00577 //                  hya
00578 //                   +               hxc equals either  hxb  or  hxa
00579 //                   o               depending on where the corner
00580 //                                   point is located.
00581 c
00582 //                                   construct the interpolant
00583 //                                   f = f0 + c1*(x-x0) +
00584 //                                       c2*(x-x0)*(x-x1) +
00585 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00586 //                                       + c5*(x-x0)*(y-y0)
00587 //
00588 //
00589 
00590     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00591     float quadri;
00592     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00593 
00594     x = xx;
00595     y = yy;
00596 
00597     // circular closure
00598         while ( x < 1.0 ) x += nxdata;
00599         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00600         while ( y < 1.0 ) y += nydata;
00601         while ( y >= (float)(nydata+1) )  y -= nydata;
00602 
00603 
00604     i   = (int) x;
00605     j   = (int) y;
00606 
00607     dx0 = x - i;
00608     dy0 = y - j;
00609 
00610     ip1 = i + 1;
00611     im1 = i - 1;
00612     jp1 = j + 1;
00613     jm1 = j - 1;
00614 
00615     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00616     if (im1 < 1)      im1 = im1 + nxdata;
00617     if (jp1 > nydata) jp1 = jp1 - nydata;
00618     if (jm1 < 1)      jm1 = jm1 + nydata;
00619 
00620     f0  = fdata(i,j);
00621     c1  = fdata(ip1,j) - f0;
00622     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00623     c3  = fdata(i,jp1) - f0;
00624     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00625 
00626     dxb = dx0 - 1;
00627     dyb = dy0 - 1;
00628 
00629     // hxc & hyc are either 1 or -1
00630     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00631     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00632 
00633     ic  = i + hxc;
00634     jc  = j + hyc;
00635 
00636     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00637     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00638 
00639     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00640             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00641 
00642     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00643 
00644     return quadri;
00645 }*/
00646 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00647 {
00648 //  purpose: quadratic interpolation
00649 //  Optimized for speed, circular closer removed, checking of ranges removed
00650         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00651         float  quadri;
00652         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00653 
00654         x = xx;
00655         y = yy;
00656 
00657         //     any xx and yy
00658         while ( x < 1.0 )                 x += nxdata;
00659         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00660         while ( y < 1.0 )                 y += nydata;
00661         while ( y >= (float)(nydata+1) )  y -= nydata;
00662 
00663         i   = (int) x;
00664         j   = (int) y;
00665 
00666         dx0 = x - i;
00667         dy0 = y - j;
00668 
00669         ip1 = i + 1;
00670         im1 = i - 1;
00671         jp1 = j + 1;
00672         jm1 = j - 1;
00673 
00674         if (ip1 > nxdata) ip1 -= nxdata;
00675         if (im1 < 1)      im1 += nxdata;
00676         if (jp1 > nydata) jp1 -= nydata;
00677         if (jm1 < 1)      jm1 += nydata;
00678 
00679         f0  = fdata(i,j);
00680         c1  = fdata(ip1,j) - f0;
00681         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00682         c3  = fdata(i,jp1) - f0;
00683         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00684 
00685         dxb = dx0 - 1;
00686         dyb = dy0 - 1;
00687 
00688         // hxc & hyc are either 1 or -1
00689         if (dx0 >= 0) hxc = 1; else hxc = -1;
00690         if (dy0 >= 0) hyc = 1; else hyc = -1;
00691 
00692         ic  = i + hxc;
00693         jc  = j + hyc;
00694 
00695         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00696         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00697 
00698         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00699                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00700 
00701 
00702         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00703 
00704         return quadri;
00705 }
00706 
00707 #undef fdata
00708 
00709 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00710 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00711 {
00712 //  purpose: quadratic interpolation
00713 //  Optimized for speed, circular closer removed, checking of ranges removed
00714         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00715         float  quadri;
00716         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00717 
00718         x = xx;
00719         y = yy;
00720 
00721         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00722         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00723               x = (float)xnew;
00724                   y = (float)ynew;
00725      }
00726 
00727 
00728         i   = (int) x;
00729         j   = (int) y;
00730 
00731         dx0 = x - i;
00732         dy0 = y - j;
00733 
00734         ip1 = i + 1;
00735         im1 = i - 1;
00736         jp1 = j + 1;
00737         jm1 = j - 1;
00738 
00739         if (ip1 > nxdata) ip1 -= nxdata;
00740         if (im1 < 1)      im1 += nxdata;
00741         if (jp1 > nydata) jp1 -= nydata;
00742         if (jm1 < 1)      jm1 += nydata;
00743 
00744         f0  = fdata(i,j);
00745         c1  = fdata(ip1,j) - f0;
00746         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00747         c3  = fdata(i,jp1) - f0;
00748         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00749 
00750         dxb = dx0 - 1;
00751         dyb = dy0 - 1;
00752 
00753         // hxc & hyc are either 1 or -1
00754         if (dx0 >= 0) hxc = 1; else hxc = -1;
00755         if (dy0 >= 0) hyc = 1; else hyc = -1;
00756 
00757         ic  = i + hxc;
00758         jc  = j + hyc;
00759 
00760         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00761         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00762 
00763         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00764                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00765 
00766 
00767         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00768 
00769         return quadri;
00770 }
00771 
00772 #undef fdata
00773 
00774 
00775 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00776         int K = kb.get_window_size();
00777         int kbmin = -K/2;
00778         int kbmax = -kbmin;
00779         int kbc = kbmax+1;
00780 
00781         float pixel =0.0f;
00782         float w=0.0f;
00783 
00784         delx = restrict1(delx, nx);
00785         int inxold = int(round(delx));
00786         if ( ny < 2 ) {  //1D
00787                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00788                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00789                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00790                 float tablex4 = kb.i0win_tab(delx-inxold);
00791                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00792                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00793                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00794 
00795                 int x1, x2, x3, x4, x5, x6, x7;
00796 
00797                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00798                         x1 = (inxold-3+nx)%nx;
00799                         x2 = (inxold-2+nx)%nx;
00800                         x3 = (inxold-1+nx)%nx;
00801                         x4 = (inxold  +nx)%nx;
00802                         x5 = (inxold+1+nx)%nx;
00803                         x6 = (inxold+2+nx)%nx;
00804                         x7 = (inxold+3+nx)%nx;
00805                 } else {
00806                         x1 = inxold-3;
00807                         x2 = inxold-2;
00808                         x3 = inxold-1;
00809                         x4 = inxold;
00810                         x5 = inxold+1;
00811                         x6 = inxold+2;
00812                         x7 = inxold+3;
00813                 }
00814 
00815                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00816                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00817                         data[x7]*tablex7 ;
00818 
00819                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00820         } else if ( nz < 2 ) {  // 2D
00821                 dely = restrict1(dely, ny);
00822                 int inyold = int(round(dely));
00823                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00824                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00825                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00826                 float tablex4 = kb.i0win_tab(delx-inxold);
00827                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00828                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00829                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00830 
00831                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00832                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00833                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00834                 float tabley4 = kb.i0win_tab(dely-inyold);
00835                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00836                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00837                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00838 
00839                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00840 
00841                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
00842                         x1 = (inxold-3+nx)%nx;
00843                         x2 = (inxold-2+nx)%nx;
00844                         x3 = (inxold-1+nx)%nx;
00845                         x4 = (inxold  +nx)%nx;
00846                         x5 = (inxold+1+nx)%nx;
00847                         x6 = (inxold+2+nx)%nx;
00848                         x7 = (inxold+3+nx)%nx;
00849 
00850                         y1 = ((inyold-3+ny)%ny)*nx;
00851                         y2 = ((inyold-2+ny)%ny)*nx;
00852                         y3 = ((inyold-1+ny)%ny)*nx;
00853                         y4 = ((inyold  +ny)%ny)*nx;
00854                         y5 = ((inyold+1+ny)%ny)*nx;
00855                         y6 = ((inyold+2+ny)%ny)*nx;
00856                         y7 = ((inyold+3+ny)%ny)*nx;
00857                 } else {
00858                         x1 = inxold-3;
00859                         x2 = inxold-2;
00860                         x3 = inxold-1;
00861                         x4 = inxold;
00862                         x5 = inxold+1;
00863                         x6 = inxold+2;
00864                         x7 = inxold+3;
00865 
00866                         y1 = (inyold-3)*nx;
00867                         y2 = (inyold-2)*nx;
00868                         y3 = (inyold-1)*nx;
00869                         y4 = inyold*nx;
00870                         y5 = (inyold+1)*nx;
00871                         y6 = (inyold+2)*nx;
00872                         y7 = (inyold+3)*nx;
00873                 }
00874 
00875                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00876                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00877                              data[x7+y1]*tablex7 ) * tabley1 +
00878                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00879                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00880                              data[x7+y2]*tablex7 ) * tabley2 +
00881                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00882                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00883                              data[x7+y3]*tablex7 ) * tabley3 +
00884                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00885                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00886                              data[x7+y4]*tablex7 ) * tabley4 +
00887                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00888                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00889                              data[x7+y5]*tablex7 ) * tabley5 +
00890                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00891                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00892                              data[x7+y6]*tablex7 ) * tabley6 +
00893                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00894                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00895                              data[x7+y7]*tablex7 ) * tabley7;
00896 
00897                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00898                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00899         } else {  //  3D
00900                 dely = restrict1(dely, ny);
00901                 int inyold = int(Util::round(dely));
00902                 delz = restrict1(delz, nz);
00903                 int inzold = int(Util::round(delz));
00904 
00905                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00906                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00907                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00908                 float tablex4 = kb.i0win_tab(delx-inxold);
00909                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00910                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00911                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00912 
00913                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00914                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00915                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00916                 float tabley4 = kb.i0win_tab(dely-inyold);
00917                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00918                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00919                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00920 
00921                 float tablez1 = kb.i0win_tab(delz-inzold+3);
00922                 float tablez2 = kb.i0win_tab(delz-inzold+2);
00923                 float tablez3 = kb.i0win_tab(delz-inzold+1);
00924                 float tablez4 = kb.i0win_tab(delz-inzold);
00925                 float tablez5 = kb.i0win_tab(delz-inzold-1);
00926                 float tablez6 = kb.i0win_tab(delz-inzold-2);
00927                 float tablez7 = kb.i0win_tab(delz-inzold-3);
00928 
00929                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00930 
00931                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
00932                         x1 = (inxold-3+nx)%nx;
00933                         x2 = (inxold-2+nx)%nx;
00934                         x3 = (inxold-1+nx)%nx;
00935                         x4 = (inxold  +nx)%nx;
00936                         x5 = (inxold+1+nx)%nx;
00937                         x6 = (inxold+2+nx)%nx;
00938                         x7 = (inxold+3+nx)%nx;
00939 
00940                         y1 = ((inyold-3+ny)%ny)*nx;
00941                         y2 = ((inyold-2+ny)%ny)*nx;
00942                         y3 = ((inyold-1+ny)%ny)*nx;
00943                         y4 = ((inyold  +ny)%ny)*nx;
00944                         y5 = ((inyold+1+ny)%ny)*nx;
00945                         y6 = ((inyold+2+ny)%ny)*nx;
00946                         y7 = ((inyold+3+ny)%ny)*nx;
00947 
00948                         z1 = ((inzold-3+nz)%nz)*nx*ny;
00949                         z2 = ((inzold-2+nz)%nz)*nx*ny;
00950                         z3 = ((inzold-1+nz)%nz)*nx*ny;
00951                         z4 = ((inzold  +nz)%nz)*nx*ny;
00952                         z5 = ((inzold+1+nz)%nz)*nx*ny;
00953                         z6 = ((inzold+2+nz)%nz)*nx*ny;
00954                         z7 = ((inzold+3+nz)%nz)*nx*ny;
00955                 } else {
00956                         x1 = inxold-3;
00957                         x2 = inxold-2;
00958                         x3 = inxold-1;
00959                         x4 = inxold;
00960                         x5 = inxold+1;
00961                         x6 = inxold+2;
00962                         x7 = inxold+3;
00963 
00964                         y1 = (inyold-3)*nx;
00965                         y2 = (inyold-2)*nx;
00966                         y3 = (inyold-1)*nx;
00967                         y4 = inyold*nx;
00968                         y5 = (inyold+1)*nx;
00969                         y6 = (inyold+2)*nx;
00970                         y7 = (inyold+3)*nx;
00971 
00972                         z1 = (inzold-3)*nx*ny;
00973                         z2 = (inzold-2)*nx*ny;
00974                         z3 = (inzold-1)*nx*ny;
00975                         z4 = inzold*nx*ny;
00976                         z5 = (inzold+1)*nx*ny;
00977                         z6 = (inzold+2)*nx*ny;
00978                         z7 = (inzold+3)*nx*ny;
00979                 }
00980 
00981                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00982                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00983                              data[x7+y1+z1]*tablex7 ) * tabley1 +
00984                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00985                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00986                              data[x7+y2+z1]*tablex7 ) * tabley2 +
00987                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00988                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00989                              data[x7+y3+z1]*tablex7 ) * tabley3 +
00990                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00991                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00992                              data[x7+y4+z1]*tablex7 ) * tabley4 +
00993                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00994                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00995                              data[x7+y5+z1]*tablex7 ) * tabley5 +
00996                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00997                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00998                              data[x7+y6+z1]*tablex7 ) * tabley6 +
00999                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01000                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01001                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01002                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01003                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01004                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01005                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01006                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01007                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01008                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01009                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01010                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01011                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01012                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01013                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01014                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01015                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01016                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01017                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01018                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01019                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01020                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01021                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01022                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01023                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01024                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01025                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01026                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01027                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01028                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01029                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01030                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01031                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01032                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01033                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01034                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01035                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01036                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01037                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01038                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01039                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01040                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01041                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01042                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01043                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01044                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01045                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01046                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01047                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01048                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01049                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01050                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01051                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01052                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01053                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01054                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01055                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01056                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01057                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01058                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01059                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01060                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01061                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01062                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01063                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01064                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01065                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01066                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01067                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01068                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01069                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01070                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01071                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01072                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01073                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01074                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01075                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01076                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01077                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01078                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01079                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01080                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01081                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01082                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01083                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01084                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01085                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01086                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01087                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01088                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01089                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01090                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01091                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01092                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01093                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01094                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01095                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01096                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01097                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01098                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01099                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01100                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01101                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01102                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01103                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01104                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01105                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01106                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01107                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01108                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01109                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01110                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01111                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01112                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01113                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01114                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01115                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01116                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01117                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01118                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01119                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01120                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01121                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01122                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01123                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01124                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01125                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01126                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01127                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01128 
01129                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01130                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01131                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01132         }
01133         return pixel/w;
01134 }
01135 
01136 float  Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01137         int K = kb.get_window_size();
01138         int kbmin = -K/2;
01139         int kbmax = -kbmin;
01140         int kbc = kbmax+1;
01141 
01142         float pixel =0.0f;
01143         float w=0.0f;
01144 
01145     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01146         delx = restrict1(delx, nx);
01147         int inxold = int(round(delx));
01148         if ( ny < 2 ) {  //1D
01149                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01150                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01151                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01152                 float tablex4 = kb.i0win_tab(delx-inxold);
01153                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01154                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01155                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01156 
01157                 int x1, x2, x3, x4, x5, x6, x7;
01158 
01159                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01160                         x1 = (inxold-3+nx)%nx;
01161                         x2 = (inxold-2+nx)%nx;
01162                         x3 = (inxold-1+nx)%nx;
01163                         x4 = (inxold  +nx)%nx;
01164                         x5 = (inxold+1+nx)%nx;
01165                         x6 = (inxold+2+nx)%nx;
01166                         x7 = (inxold+3+nx)%nx;
01167                 } else {
01168                         x1 = inxold-3;
01169                         x2 = inxold-2;
01170                         x3 = inxold-1;
01171                         x4 = inxold;
01172                         x5 = inxold+1;
01173                         x6 = inxold+2;
01174                         x7 = inxold+3;
01175                 }
01176 
01177                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01178                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01179                         data[x7]*tablex7 ;
01180 
01181                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01182         } else if ( nz < 2 ) {  // 2D
01183 
01184                 delx = argdelx;
01185                 // the wrap around is not done circulantly for 2D case; if (argdelx, argdely) is not in the image, then make them (xnew, ynew) which is definitely in the image
01186                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01187                 delx = (float)xnew*2.0f;
01188                 dely = (float)ynew*2.0f;
01189                 }
01190 
01191                 int inxold = int(round(delx));
01192                 int inyold = int(round(dely));
01193 
01194                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01195                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01196                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01197                 float tablex4 = kb.i0win_tab(delx-inxold);
01198                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01199                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01200                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01201 
01202                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01203                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01204                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01205                 float tabley4 = kb.i0win_tab(dely-inyold);
01206                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01207                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01208                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01209 
01210                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01211 
01212                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01213                         x1 = (inxold-3+nx)%nx;
01214                         x2 = (inxold-2+nx)%nx;
01215                         x3 = (inxold-1+nx)%nx;
01216                         x4 = (inxold  +nx)%nx;
01217                         x5 = (inxold+1+nx)%nx;
01218                         x6 = (inxold+2+nx)%nx;
01219                         x7 = (inxold+3+nx)%nx;
01220 
01221                         y1 = ((inyold-3+ny)%ny)*nx;
01222                         y2 = ((inyold-2+ny)%ny)*nx;
01223                         y3 = ((inyold-1+ny)%ny)*nx;
01224                         y4 = ((inyold  +ny)%ny)*nx;
01225                         y5 = ((inyold+1+ny)%ny)*nx;
01226                         y6 = ((inyold+2+ny)%ny)*nx;
01227                         y7 = ((inyold+3+ny)%ny)*nx;
01228                 } else {
01229                         x1 = inxold-3;
01230                         x2 = inxold-2;
01231                         x3 = inxold-1;
01232                         x4 = inxold;
01233                         x5 = inxold+1;
01234                         x6 = inxold+2;
01235                         x7 = inxold+3;
01236 
01237                         y1 = (inyold-3)*nx;
01238                         y2 = (inyold-2)*nx;
01239                         y3 = (inyold-1)*nx;
01240                         y4 = inyold*nx;
01241                         y5 = (inyold+1)*nx;
01242                         y6 = (inyold+2)*nx;
01243                         y7 = (inyold+3)*nx;
01244                 }
01245 
01246                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01247                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01248                              data[x7+y1]*tablex7 ) * tabley1 +
01249                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01250                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01251                              data[x7+y2]*tablex7 ) * tabley2 +
01252                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01253                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01254                              data[x7+y3]*tablex7 ) * tabley3 +
01255                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01256                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01257                              data[x7+y4]*tablex7 ) * tabley4 +
01258                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01259                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01260                              data[x7+y5]*tablex7 ) * tabley5 +
01261                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01262                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01263                              data[x7+y6]*tablex7 ) * tabley6 +
01264                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01265                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01266                              data[x7+y7]*tablex7 ) * tabley7;
01267 
01268                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01269                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01270         } else {  //  3D
01271                 dely = restrict1(dely, ny);
01272                 int inyold = int(Util::round(dely));
01273                 delz = restrict1(delz, nz);
01274                 int inzold = int(Util::round(delz));
01275 
01276                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01277                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01278                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01279                 float tablex4 = kb.i0win_tab(delx-inxold);
01280                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01281                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01282                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01283 
01284                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01285                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01286                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01287                 float tabley4 = kb.i0win_tab(dely-inyold);
01288                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01289                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01290                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01291 
01292                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01293                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01294                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01295                 float tablez4 = kb.i0win_tab(delz-inzold);
01296                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01297                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01298                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01299 
01300                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01301 
01302                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01303                         x1 = (inxold-3+nx)%nx;
01304                         x2 = (inxold-2+nx)%nx;
01305                         x3 = (inxold-1+nx)%nx;
01306                         x4 = (inxold  +nx)%nx;
01307                         x5 = (inxold+1+nx)%nx;
01308                         x6 = (inxold+2+nx)%nx;
01309                         x7 = (inxold+3+nx)%nx;
01310 
01311                         y1 = ((inyold-3+ny)%ny)*nx;
01312                         y2 = ((inyold-2+ny)%ny)*nx;
01313                         y3 = ((inyold-1+ny)%ny)*nx;
01314                         y4 = ((inyold  +ny)%ny)*nx;
01315                         y5 = ((inyold+1+ny)%ny)*nx;
01316                         y6 = ((inyold+2+ny)%ny)*nx;
01317                         y7 = ((inyold+3+ny)%ny)*nx;
01318 
01319                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01320                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01321                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01322                         z4 = ((inzold  +nz)%nz)*nx*ny;
01323                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01324                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01325                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01326                 } else {
01327                         x1 = inxold-3;
01328                         x2 = inxold-2;
01329                         x3 = inxold-1;
01330                         x4 = inxold;
01331                         x5 = inxold+1;
01332                         x6 = inxold+2;
01333                         x7 = inxold+3;
01334 
01335                         y1 = (inyold-3)*nx;
01336                         y2 = (inyold-2)*nx;
01337                         y3 = (inyold-1)*nx;
01338                         y4 = inyold*nx;
01339                         y5 = (inyold+1)*nx;
01340                         y6 = (inyold+2)*nx;
01341                         y7 = (inyold+3)*nx;
01342 
01343                         z1 = (inzold-3)*nx*ny;
01344                         z2 = (inzold-2)*nx*ny;
01345                         z3 = (inzold-1)*nx*ny;
01346                         z4 = inzold*nx*ny;
01347                         z5 = (inzold+1)*nx*ny;
01348                         z6 = (inzold+2)*nx*ny;
01349                         z7 = (inzold+3)*nx*ny;
01350                 }
01351 
01352                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01353                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01354                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01355                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01356                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01357                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01358                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01359                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01360                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01361                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01362                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01363                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01364                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01365                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01366                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01367                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01368                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01369                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01370                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01371                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01372                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01373                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01374                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01375                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01376                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01377                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01378                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01379                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01380                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01381                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01382                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01383                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01384                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01385                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01386                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01387                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01388                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01389                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01390                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01391                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01392                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01393                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01394                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01395                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01396                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01397                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01398                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01399                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01400                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01401                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01402                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01403                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01404                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01405                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01406                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01407                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01408                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01409                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01410                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01411                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01412                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01413                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01414                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01415                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01416                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01417                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01418                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01419                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01420                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01421                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01422                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01423                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01424                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01425                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01426                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01427                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01428                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01429                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01430                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01431                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01432                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01433                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01434                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01435                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01436                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01437                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01438                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01439                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01440                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01441                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01442                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01443                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01444                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01445                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01446                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01447                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01448                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01449                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01450                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01451                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01452                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01453                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01454                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01455                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01456                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01457                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01458                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01459                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01460                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01461                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01462                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01463                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01464                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01465                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01466                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01467                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01468                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01469                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01470                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01471                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01472                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01473                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01474                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01475                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01476                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01477                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01478                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01479                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01480                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01481                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01482                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01483                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01484                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01485                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01486                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01487                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01488                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01489                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01490                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01491                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01492                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01493                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01494                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01495                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01496                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01497                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01498                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01499 
01500                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01501                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01502                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01503         }
01504         return pixel/w;
01505 }
01506 
01507 /*
01508 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01509 
01510         int nxreal = nx - 2;
01511         if (nxreal != ny)
01512                 throw ImageDimensionException("extractpoint requires ny == nx");
01513         int nhalf = nxreal/2;
01514         int kbsize = kb.get_window_size();
01515         int kbmin = -kbsize/2;
01516         int kbmax = -kbmin;
01517         bool flip = (nuxnew < 0.f);
01518         if (flip) {
01519                 nuxnew *= -1;
01520                 nuynew *= -1;
01521         }
01522         // put (xnew,ynew) on a grid.  The indices will be wrong for
01523         // the Fourier elements in the image, but the grid sizing will
01524         // be correct.
01525         int ixn = int(Util::round(nuxnew));
01526         int iyn = int(Util::round(nuynew));
01527         // set up some temporary weighting arrays
01528         float* wy0 = new float[kbmax - kbmin + 1];
01529         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01530         float* wx0 = new float[kbmax - kbmin + 1];
01531         float* wx = wx0 - kbmin;
01532         for (int i = kbmin; i <= kbmax; i++) {
01533                         int iyp = iyn + i;
01534                         wy[i] = kb.i0win_tab(nuynew - iyp);
01535                         int ixp = ixn + i;
01536                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01537         }
01538         // restrict loops to non-zero elements
01539         int iymin = 0;
01540         for (int iy = kbmin; iy <= -1; iy++) {
01541                 if (wy[iy] != 0.f) {
01542                         iymin = iy;
01543                         break;
01544                 }
01545         }
01546         int iymax = 0;
01547         for (int iy = kbmax; iy >= 1; iy--) {
01548                 if (wy[iy] != 0.f) {
01549                         iymax = iy;
01550                         break;
01551                 }
01552         }
01553         int ixmin = 0;
01554         for (int ix = kbmin; ix <= -1; ix++) {
01555                 if (wx[ix] != 0.f) {
01556                         ixmin = ix;
01557                         break;
01558                 }
01559         }
01560         int ixmax = 0;
01561         for (int ix = kbmax; ix >= 1; ix--) {
01562                 if (wx[ix] != 0.f) {
01563                         ixmax = ix;
01564                         break;
01565                 }
01566         }
01567         float wsum = 0.0f;
01568         for (int iy = iymin; iy <= iymax; iy++)
01569                 for (int ix = ixmin; ix <= ixmax; ix++)
01570                         wsum += wx[ix]*wy[iy];
01571 
01572         complex<float> result(0.f,0.f);
01573         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01574                 // (xin,yin) not within window border from the edge
01575                 for (int iy = iymin; iy <= iymax; iy++) {
01576                         int iyp = iyn + iy;
01577                         for (int ix = ixmin; ix <= ixmax; ix++) {
01578                                 int ixp = ixn + ix;
01579                                 float w = wx[ix]*wy[iy];
01580                                 complex<float> val = fimage->cmplx(ixp,iyp);
01581                                 result += val*w;
01582                         }
01583                 }
01584         } else {
01585                 // points that "stick out"
01586                 for (int iy = iymin; iy <= iymax; iy++) {
01587                         int iyp = iyn + iy;
01588                         for (int ix = ixmin; ix <= ixmax; ix++) {
01589                                 int ixp = ixn + ix;
01590                                 bool mirror = false;
01591                                 int ixt= ixp, iyt= iyp;
01592                                 if (ixt < 0) {
01593                                         ixt = -ixt;
01594                                         iyt = -iyt;
01595                                         mirror = !mirror;
01596                                 }
01597                                 if (ixt > nhalf) {
01598                                         ixt = nxreal - ixt;
01599                                         iyt = -iyt;
01600                                         mirror = !mirror;
01601                                 }
01602                                 if (iyt > nhalf-1)  iyt -= nxreal;
01603                                 if (iyt < -nhalf)   iyt += nxreal;
01604                                 float w = wx[ix]*wy[iy];
01605                                 complex<float> val = fimage->cmplx(ixt,iyt);
01606                                 if (mirror)  result += conj(val)*w;
01607                                 else         result += val*w;
01608                         }
01609                 }
01610         }
01611         if (flip)  result = conj(result)/wsum;
01612         else result /= wsum;
01613         delete [] wx0;
01614         delete [] wy0;
01615         return result;
01616 }*/
01617 
01618 /*
01619 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01620 
01621         int nxreal = nx - 2;
01622         if (nxreal != ny)
01623                 throw ImageDimensionException("extractpoint requires ny == nx");
01624         int nhalf = nxreal/2;
01625         bool flip = false;
01626         if (nuxnew < 0.f) {
01627                 nuxnew *= -1;
01628                 nuynew *= -1;
01629                 flip = true;
01630         }
01631         if (nuynew >= nhalf-0.5)  {
01632                 nuynew -= nxreal;
01633         } else if (nuynew < -nhalf-0.5) {
01634                 nuynew += nxreal;
01635         }
01636 
01637         // put (xnew,ynew) on a grid.  The indices will be wrong for
01638         // the Fourier elements in the image, but the grid sizing will
01639         // be correct.
01640         int ixn = int(Util::round(nuxnew));
01641         int iyn = int(Util::round(nuynew));
01642 
01643         // set up some temporary weighting arrays
01644         static float wy[7];
01645         static float wx[7];
01646 
01647         float iynn = nuynew - iyn;
01648         wy[0] = kb.i0win_tab(iynn+3);
01649         wy[1] = kb.i0win_tab(iynn+2);
01650         wy[2] = kb.i0win_tab(iynn+1);
01651         wy[3] = kb.i0win_tab(iynn);
01652         wy[4] = kb.i0win_tab(iynn-1);
01653         wy[5] = kb.i0win_tab(iynn-2);
01654         wy[6] = kb.i0win_tab(iynn-3);
01655 
01656         float ixnn = nuxnew - ixn;
01657         wx[0] = kb.i0win_tab(ixnn+3);
01658         wx[1] = kb.i0win_tab(ixnn+2);
01659         wx[2] = kb.i0win_tab(ixnn+1);
01660         wx[3] = kb.i0win_tab(ixnn);
01661         wx[4] = kb.i0win_tab(ixnn-1);
01662         wx[5] = kb.i0win_tab(ixnn-2);
01663         wx[6] = kb.i0win_tab(ixnn-3);
01664 
01665         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01666 
01667         complex<float> result(0.f,0.f);
01668         for (int iy = 0; iy < 7; iy++) {
01669                 int iyp = iyn + iy - 3 ;
01670                 for (int ix = 0; ix < 7; ix++) {
01671                         int ixp = ixn + ix - 3;
01672                         float w = wx[ix]*wy[iy];
01673                         complex<float> val = fimage->cmplx(ixp,iyp);
01674                         result += val*w;
01675                 }
01676         }
01677 
01678         if (flip)  result = conj(result)/wsum;
01679         else result /= wsum;
01680 
01681         return result;
01682 }*/
01683 
01684 
01685 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01686 
01687         int nxreal = nx - 2;
01688         if (nxreal != ny)
01689                 throw ImageDimensionException("extractpoint requires ny == nx");
01690         int nhalf = nxreal/2;
01691         bool flip = (nuxnew < 0.f);
01692         if (flip) {
01693                 nuxnew *= -1;
01694                 nuynew *= -1;
01695         }
01696         if (nuynew >= nhalf-0.5)  {
01697                 nuynew -= nxreal;
01698         } else if (nuynew < -nhalf-0.5) {
01699                 nuynew += nxreal;
01700         }
01701 
01702         // put (xnew,ynew) on a grid.  The indices will be wrong for
01703         // the Fourier elements in the image, but the grid sizing will
01704         // be correct.
01705         int ixn = int(Util::round(nuxnew));
01706         int iyn = int(Util::round(nuynew));
01707 
01708         // set up some temporary weighting arrays
01709         static float wy[7];
01710         static float wx[7];
01711 
01712         float iynn = nuynew - iyn;
01713         wy[0] = kb.i0win_tab(iynn+3);
01714         wy[1] = kb.i0win_tab(iynn+2);
01715         wy[2] = kb.i0win_tab(iynn+1);
01716         wy[3] = kb.i0win_tab(iynn);
01717         wy[4] = kb.i0win_tab(iynn-1);
01718         wy[5] = kb.i0win_tab(iynn-2);
01719         wy[6] = kb.i0win_tab(iynn-3);
01720 
01721         float ixnn = nuxnew - ixn;
01722         wx[0] = kb.i0win_tab(ixnn+3);
01723         wx[1] = kb.i0win_tab(ixnn+2);
01724         wx[2] = kb.i0win_tab(ixnn+1);
01725         wx[3] = kb.i0win_tab(ixnn);
01726         wx[4] = kb.i0win_tab(ixnn-1);
01727         wx[5] = kb.i0win_tab(ixnn-2);
01728         wx[6] = kb.i0win_tab(ixnn-3);
01729 
01730         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01731 
01732         complex<float> result(0.f,0.f);
01733         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01734                 // (xin,yin) not within window border from the edge
01735                 for (int iy = 0; iy < 7; iy++) {
01736                         int iyp = iyn + iy - 3 ;
01737                         for (int ix = 0; ix < 7; ix++) {
01738                                 int ixp = ixn + ix - 3;
01739                                 float w = wx[ix]*wy[iy];
01740                                 complex<float> val = fimage->cmplx(ixp,iyp);
01741                                 result += val*w;
01742                         }
01743                 }
01744         } else {
01745                 // points that "stick out"
01746                 for (int iy = 0; iy < 7; iy++) {
01747                         int iyp = iyn + iy - 3;
01748                         for (int ix = 0; ix < 7; ix++) {
01749                                 int ixp = ixn + ix - 3;
01750                                 bool mirror = false;
01751                                 int ixt = ixp, iyt = iyp;
01752                                 if (ixt < 0) {
01753                                         ixt = -ixt;
01754                                         iyt = -iyt;
01755                                         mirror = !mirror;
01756                                 }
01757                                 if (ixt > nhalf) {
01758                                         ixt = nxreal - ixt;
01759                                         iyt = -iyt;
01760                                         mirror = !mirror;
01761                                 }
01762                                 if (iyt > nhalf-1)  iyt -= nxreal;
01763                                 if (iyt < -nhalf)   iyt += nxreal;
01764                                 float w = wx[ix]*wy[iy];
01765                                 complex<float> val = fimage->cmplx(ixt,iyt);
01766                                 if (mirror)  result += conj(val)*w;
01767                                 else         result += val*w;
01768                         }
01769                 }
01770         }
01771         if (flip)  result = conj(result)/wsum;
01772         else result /= wsum;
01773         return result;
01774 }
01775 
01776 /*
01777 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01778 
01779         int nxreal = nx - 2;
01780         if (nxreal != ny)
01781                 throw ImageDimensionException("extractpoint requires ny == nx");
01782         int nhalf = nxreal/2;
01783         bool flip = (nuxnew < 0.f);
01784         if (flip) {
01785                 nuxnew *= -1;
01786                 nuynew *= -1;
01787         }
01788         // put (xnew,ynew) on a grid.  The indices will be wrong for
01789         // the Fourier elements in the image, but the grid sizing will
01790         // be correct.
01791         int ixn = int(Util::round(nuxnew));
01792         int iyn = int(Util::round(nuynew));
01793         // set up some temporary weighting arrays
01794         static float wy[7];
01795         static float wx[7];
01796 
01797         float iynn = nuynew - iyn;
01798         wy[0] = kb.i0win_tab(iynn+3);
01799         wy[1] = kb.i0win_tab(iynn+2);
01800         wy[2] = kb.i0win_tab(iynn+1);
01801         wy[3] = kb.i0win_tab(iynn);
01802         wy[4] = kb.i0win_tab(iynn-1);
01803         wy[5] = kb.i0win_tab(iynn-2);
01804         wy[6] = kb.i0win_tab(iynn-3);
01805 
01806         float ixnn = nuxnew - ixn;
01807         wx[0] = kb.i0win_tab(ixnn+3);
01808         wx[1] = kb.i0win_tab(ixnn+2);
01809         wx[2] = kb.i0win_tab(ixnn+1);
01810         wx[3] = kb.i0win_tab(ixnn);
01811         wx[4] = kb.i0win_tab(ixnn-1);
01812         wx[5] = kb.i0win_tab(ixnn-2);
01813         wx[6] = kb.i0win_tab(ixnn-3);
01814 
01815         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01816 
01817         complex<float> result(0.f,0.f);
01818 
01819         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01820                 // (xin,yin) not within window border from the edge
01821                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01822                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01823                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01824                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01825                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
01826                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
01827                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
01828                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
01829                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
01830                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
01831                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
01832                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
01833                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
01834                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
01835                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
01836                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
01837                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
01838                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
01839                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
01840                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
01841                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
01842                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
01843                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
01844                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
01845                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
01846                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
01847                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
01848                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
01849                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
01850                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
01851                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
01852                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
01853                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
01854                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
01855                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
01856                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
01857                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
01858                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
01859                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
01860                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
01861                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
01862                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
01863                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
01864                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
01865                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
01866                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
01867                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
01868                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
01869                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
01870 
01871         } else {
01872                 // points that "stick out"
01873                 for (int iy = 0; iy < 7; iy++) {
01874                         int iyp = iyn + iy - 3;
01875                         for (int ix = 0; ix < 7; ix++) {
01876                                 int ixp = ixn + ix - 3;
01877                                 bool mirror = false;
01878                                 int ixt= ixp, iyt= iyp;
01879                                 if (ixt < 0) {
01880                                         ixt = -ixt;
01881                                         iyt = -iyt;
01882                                         mirror = !mirror;
01883                                 }
01884                                 if (ixt > nhalf) {
01885                                         ixt = nxreal - ixt;
01886                                         iyt = -iyt;
01887                                         mirror = !mirror;
01888                                 }
01889                                 if (iyt > nhalf-1)  iyt -= nxreal;
01890                                 if (iyt < -nhalf)   iyt += nxreal;
01891                                 float w = wx[ix]*wy[iy];
01892                                 complex<float> val = fimage->cmplx(ixt,iyt);
01893                                 if (mirror)  result += conj(val)*w;
01894                                 else         result += val*w;
01895                         }
01896                 }
01897         }
01898         if (flip)  result = conj(result)/wsum;
01899         else result /= wsum;
01900         return result;
01901 }*/
01902 
01903 
01904 float Util::triquad(float R, float S, float T, float* fdata)
01905 {
01906 
01907     const float C2 = 0.5f;    //1.0 / 2.0;
01908     const float C4 = 0.25f;   //1.0 / 4.0;
01909     const float C8 = 0.125f;  //1.0 / 8.0;
01910 
01911     float  RS   = R * S;
01912     float  ST   = S * T;
01913     float  RT   = R * T;
01914     float  RST  = R * ST;
01915 
01916     float  RSQ  = 1-R*R;
01917     float  SSQ  = 1-S*S;
01918     float  TSQ  = 1-T*T;
01919 
01920     float  RM1  = (1-R);
01921     float  SM1  = (1-S);
01922     float  TM1  = (1-T);
01923 
01924     float  RP1  = (1+R);
01925     float  SP1  = (1+S);
01926     float  TP1  = (1+T);
01927 
01928     float triquad =
01929     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
01930         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
01931         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
01932         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
01933         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
01934         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
01935         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
01936         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
01937         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
01938 //
01939         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
01940         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
01941         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
01942         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
01943                       RSQ  * SSQ  * TSQ * fdata[13] +
01944         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
01945         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
01946         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
01947         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
01948  //
01949         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
01950         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
01951         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
01952         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
01953         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
01954         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
01955         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
01956         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
01957         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
01958      return triquad;
01959 }
01960 
01961 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01962                 : M(M_), fc(fc_), ntable(ntable_) {
01963         // Sinc-Blackman kernel
01964         build_sBtable();
01965 }
01966 
01967 void Util::sincBlackman::build_sBtable() {
01968         sBtable.resize(ntable+1);
01969         int ltab = int(round(float(ntable)/1.25f));
01970         int M2 = M/2;
01971         fltb = float(ltab)/M2;
01972         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01973         float x = 1.0e-7f;
01974         sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01975         for (int i=1; i <= ltab; i++) {
01976                 x = float(i)/fltb;
01977                 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01978                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
01979         }
01980 }
01981 
01982 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01983                                          int N_, float vtable_, int ntable_)
01984                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01985                   ntable(ntable_) {
01986         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
01987         if (0.f == v) v = float(K)/2;
01988         if (0.f == vtable) vtable = v;
01989         alphar = alpha*r;
01990         fac = static_cast<float>(twopi)*alphar*v;
01991         vadjust = 1.0f*v;
01992         facadj = static_cast<float>(twopi)*alphar*vadjust;
01993         build_I0table();
01994 }
01995 
01996 float Util::KaiserBessel::i0win(float x) const {
01997         float val0 = float(gsl_sf_bessel_I0(facadj));
01998         float absx = fabs(x);
01999         if (absx > vadjust) return 0.f;
02000         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02001         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02002         return res;
02003 }
02004 
02005 void Util::KaiserBessel::build_I0table() {
02006         i0table.resize(ntable+1); // i0table[0:ntable]
02007         int ltab = int(round(float(ntable)/1.25f));
02008         fltb = float(ltab)/(K/2);
02009         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02010         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02011         for (int i=0; i <= ltab; i++) {
02012                 float s = float(i)/fltb/N;
02013                 if (s < vadjust) {
02014                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02015                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02016                 } else {
02017                         i0table[i] = 0.f;
02018                 }
02019 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02020         }
02021 }
02022 
02023 float Util::KaiserBessel::I0table_maxerror() {
02024         float maxdiff = 0.f;
02025         for (int i = 1; i <= ntable; i++) {
02026                 float diff = fabs(i0table[i] - i0table[i-1]);
02027                 if (diff > maxdiff) maxdiff = diff;
02028         }
02029         return maxdiff;
02030 }
02031 
02032 float Util::KaiserBessel::sinhwin(float x) const {
02033         float val0 = sinh(fac)/fac;
02034         float absx = fabs(x);
02035         if (0.0 == x) {
02036                 float res = 1.0f;
02037                 return res;
02038         } else if (absx == alphar) {
02039                 return 1.0f/val0;
02040         } else if (absx < alphar) {
02041                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02042                 float facrt = fac*rt;
02043                 float res = (sinh(facrt)/facrt)/val0;
02044                 return res;
02045         } else {
02046                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02047                 float facrt = fac*rt;
02048                 float res = (sin(facrt)/facrt)/val0;
02049                 return res;
02050         }
02051 }
02052 
02053 float Util::FakeKaiserBessel::i0win(float x) const {
02054         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02055         float absx = fabs(x);
02056         if (absx > vadjust) return 0.f;
02057         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02058         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02059         return res;
02060 }
02061 
02062 void Util::FakeKaiserBessel::build_I0table() {
02063         i0table.resize(ntable+1); // i0table[0:ntable]
02064         int ltab = int(round(float(ntable)/1.1f));
02065         fltb = float(ltab)/(K/2);
02066         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02067         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02068         for (int i=0; i <= ltab; i++) {
02069                 float s = float(i)/fltb/N;
02070                 if (s < vadjust) {
02071                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02072                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02073                 } else {
02074                         i0table[i] = 0.f;
02075                 }
02076         }
02077 }
02078 
02079 float Util::FakeKaiserBessel::sinhwin(float x) const {
02080         float val0 = sinh(fac)/fac;
02081         float absx = fabs(x);
02082         if (0.0 == x) {
02083                 float res = 1.0f;
02084                 return res;
02085         } else if (absx == alphar) {
02086                 return 1.0f/val0;
02087         } else if (absx < alphar) {
02088                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02089                 float facrt = fac*rt;
02090                 float res = (sinh(facrt)/facrt)/val0;
02091                 return res;
02092         } else {
02093                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02094                 float facrt = fac*rt;
02095                 float res = (sin(facrt)/facrt)/val0;
02096                 return res;
02097         }
02098 }
02099 
02100 #if 0 // 1-st order KB window
02101 float Util::FakeKaiserBessel::sinhwin(float x) const {
02102         //float val0 = sinh(fac)/fac;
02103         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02104         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02105         float absx = fabs(x);
02106         if (0.0 == x) {
02107                 //float res = 1.0f;
02108                 float res = val0;
02109                 return res;
02110         } else if (absx == alphar) {
02111                 //return 1.0f/val0;
02112                 return prefix;
02113         } else if (absx < alphar) {
02114                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02115                 //float facrt = fac*rt;
02116                 float facrt = facadj*rt;
02117                 //float res = (sinh(facrt)/facrt)/val0;
02118                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02119                 return res;
02120         } else {
02121                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02122                 //float facrt = fac*rt;
02123                 float facrt = facadj*rt;
02124                 //float res = (sin(facrt)/facrt)/val0;
02125                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02126                 return res;
02127         }
02128 }
02129 #endif // 0
02130 
02131 
02132 
02133 #define  circ(i)         circ[i-1]
02134 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02135 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02136 
02137 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02138         int nsam = image->get_xsize();
02139         int nrow = image->get_ysize();
02140         int nring = numr.size()/3;
02141         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02142         EMData* out = new EMData();
02143         out->set_size(lcirc,1,1);
02144         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02145         float *xim  = image->get_data();
02146         float *circ = out->get_data();
02147 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02148    return out;
02149 }
02150 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02151           float *circ, int lcirc, int nring, char mode)
02152 {*/
02153 /*
02154 c
02155 c  purpose:
02156 c
02157 c  resmaple to polar coordinates
02158 c
02159 */
02160         //  dimension         xim(nsam,nrow),circ(lcirc)
02161         //  integer           numr(3,nring)
02162 
02163         double dfi, dpi;
02164         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02165         float  yq, xold, yold, fi, x, y;
02166 
02167         ns2 = nsam/2+1;
02168         nr2 = nrow/2+1;
02169         dpi = 2.0*atan(1.0);
02170 
02171         for (i=1;i<=nring;i++) {
02172                 // radius of the ring
02173                 inr = numr(1,i);
02174                 yq  = static_cast<float>(inr);
02175                 l   = numr(3,i);
02176                 if (mode == 'h' || mode == 'H')  lt = l/2;
02177                 else                             lt = l/4;
02178 
02179                 nsim           = lt-1;
02180                 dfi            = dpi/(nsim+1);
02181                 kcirc          = numr(2,i);
02182                 xold           = 0.0f;
02183                 yold           = static_cast<float>(inr);
02184                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02185                 xold           = static_cast<float>(inr);
02186                 yold           = 0.0f;
02187                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02188 
02189                 if (mode == 'f' || mode == 'F') {
02190                         xold              = 0.0f;
02191                         yold              = static_cast<float>(-inr);
02192                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02193                         xold              = static_cast<float>(-inr);
02194                         yold              = 0.0f;
02195                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02196                 }
02197 
02198                 for (j=1;j<=nsim;j++) {
02199                         fi               = static_cast<float>(dfi*j);
02200                         x                = sin(fi)*yq;
02201                         y                = cos(fi)*yq;
02202                         xold             = x;
02203                         yold             = y;
02204                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02205                         xold             =  y;
02206                         yold             = -x;
02207                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02208 
02209                         if (mode == 'f' || mode == 'F')  {
02210                                 xold                = -x;
02211                                 yold                = -y;
02212                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02213                                 xold                = -y;
02214                                 yold                =  x;
02215                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02216                         }
02217                 }
02218         }
02219         return  out;
02220 }
02221 
02222 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02223         int nsam = image->get_xsize();
02224         int nrow = image->get_ysize();
02225         int nring = numr.size()/3;
02226         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02227         EMData* out = new EMData();
02228         out->set_size(lcirc,1,1);
02229         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02230         float *xim  = image->get_data();
02231         float *circ = out->get_data();
02232         double dpi, dfi;
02233         int    it, jt, inr, l, nsim, kcirc, lt;
02234         float  xold, yold, fi, x, y;
02235 
02236         //     cns2 and cnr2 are predefined centers
02237         //     no need to set to zero, all elements are defined
02238         dpi = 2*atan(1.0);
02239         for (it=1; it<=nring; it++) {
02240                 // radius of the ring
02241                 inr = numr(1,it);
02242 
02243                 // "F" means a full circle interpolation
02244                 // "H" means a half circle interpolation
02245 
02246                 l = numr(3,it);
02247                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02248                 else                              lt = l / 4;
02249 
02250                 nsim  = lt - 1;
02251                 dfi   = dpi / (nsim+1);
02252                 kcirc = numr(2,it);
02253                 xold  = 0.0f+cns2;
02254                 yold  = inr+cnr2;
02255 
02256                 Assert( kcirc <= lcirc );
02257                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02258 
02259                 xold  = inr+cns2;
02260                 yold  = 0.0f+cnr2;
02261                 Assert( lt+kcirc <= lcirc );
02262                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02263 
02264                 if ( mode == 'f' || mode == 'F' ) {
02265                         xold = 0.0f+cns2;
02266                         yold = -inr+cnr2;
02267                         Assert( lt+lt+kcirc <= lcirc );
02268                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02269 
02270                         xold = -inr+cns2;
02271                         yold = 0.0f+cnr2;
02272                         Assert(lt+lt+lt+kcirc <= lcirc );
02273                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02274                 }
02275 
02276                 for (jt=1; jt<=nsim; jt++) {
02277                         fi   = static_cast<float>(dfi * jt);
02278                         x    = sin(fi) * inr;
02279                         y    = cos(fi) * inr;
02280 
02281                         xold = x+cns2;
02282                         yold = y+cnr2;
02283 
02284                         Assert( jt+kcirc <= lcirc );
02285                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02286 
02287                         xold = y+cns2;
02288                         yold = -x+cnr2;
02289 
02290                         Assert( jt+lt+kcirc <= lcirc );
02291                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02292 
02293                         if ( mode == 'f' || mode == 'F' ) {
02294                                 xold = -x+cns2;
02295                                 yold = -y+cnr2;
02296 
02297                                 Assert( jt+lt+lt+kcirc <= lcirc );
02298                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02299 
02300                                 xold = -y+cns2;
02301                                 yold = x+cnr2;
02302 
02303                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02304                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02305                         }
02306                 } // end for jt
02307         } //end for it
02308         return out;
02309 }
02310 
02311 float Util::bilinear(float xold, float yold, int nsam, int nrow, float* xim)
02312 {
02313 /*
02314 c  purpose: linear interpolation
02315   Optimized for speed, circular closer removed, checking of ranges removed
02316 */
02317     float bilinear;
02318     int   ixold, iyold;
02319 
02320 /*
02321         float xdif, ydif, xrem, yrem;
02322         ixold   = (int) floor(xold);
02323         iyold   = (int) floor(yold);
02324         ydif = yold - iyold;
02325         yrem = 1.0f - ydif;
02326 
02327         //  May want to insert if?
02328 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02329 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02330 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02331         xdif = xold - ixold;
02332         xrem = 1.0f- xdif;
02333 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02334 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02335 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02336         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02337                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02338 
02339     return bilinear;
02340 }
02341 */
02342         float xdif, ydif;
02343 
02344         ixold   = (int) xold;
02345         iyold   = (int) yold;
02346         ydif = yold - iyold;
02347 
02348         //  May want to insert it?
02349 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02350 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02351 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02352         xdif = xold - ixold;
02353         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02354                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02355                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02356 
02357         return bilinear;
02358 }
02359 
02360 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02361              int  *numr, float *circ, int , int  nring, char  mode) {
02362         double dpi, dfi;
02363         int    it, jt, inr, l, nsim, kcirc, lt;
02364         float   xold, yold, fi, x, y;
02365 
02366         //     cns2 and cnr2 are predefined centers
02367         //     no need to set to zero, all elements are defined
02368 
02369         dpi = 2*atan(1.0);
02370         for (it=1; it<=nring; it++) {
02371                 // radius of the ring
02372                 inr = numr(1,it);
02373 
02374                 l = numr(3,it);
02375                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02376                 else                              lt = l / 4;
02377 
02378                 nsim  = lt - 1;
02379                 dfi   = dpi / (nsim+1);
02380                 kcirc = numr(2,it);
02381 
02382 
02383                 xold  = 0.0f+cns2;
02384                 yold  = inr+cnr2;
02385 
02386                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02387 
02388                 xold  = inr+cns2;
02389                 yold  = 0.0f+cnr2;
02390                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391 
02392                 if ( mode == 'f' || mode == 'F' ) {
02393                         xold = 0.0f+cns2;
02394                         yold = -inr+cnr2;
02395                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02396 
02397                         xold = -inr+cns2;
02398                         yold = 0.0f+cnr2;
02399                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400                 }
02401 
02402                 for (jt=1; jt<=nsim; jt++) {
02403                         fi   = static_cast<float>(dfi * jt);
02404                         x    = sin(fi) * inr;
02405                         y    = cos(fi) * inr;
02406 
02407                         xold = x+cns2;
02408                         yold = y+cnr2;
02409                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02410 
02411                         xold = y+cns2;
02412                         yold = -x+cnr2;
02413                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414 
02415                         if ( mode == 'f' || mode == 'F' ) {
02416                                 xold = -x+cns2;
02417                                 yold = -y+cnr2;
02418                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02419 
02420                                 xold = -y+cns2;
02421                                 yold = x+cnr2;
02422                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02423                         }
02424                 } // end for jt
02425         } //end for it
02426 }
02427 /*
02428 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02429              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02430 {
02431    double dpi, dfi;
02432    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02433    float  yq, fi, x, y;
02434 
02435    //     cns2 and cnr2 are predefined centers
02436    //     no need to set to zero, all elements are defined
02437 
02438    dpi = 2*atan(1.0);
02439    for (it=1; it<=nring; it++) {
02440       // radius of the ring
02441       inr = numr(1,it);
02442       yq  = inr;
02443 
02444       l = numr(3,it);
02445       if ( mode == 'h' || mode == 'H' ) {
02446          lt = l / 2;
02447       }
02448       else { // if ( mode == 'f' || mode == 'F' )
02449          lt = l / 4;
02450       }
02451 
02452       nsim  = lt - 1;
02453       dfi   = dpi / (nsim+1);
02454       kcirc = numr(2,it);
02455 
02456 
02457         xold = (int) (0.0+cns2);
02458         yold = (int) (inr+cnr2);
02459 
02460         circ(kcirc) = xim(xold, yold);
02461 
02462       xold = (int) (inr+cns2);
02463       yold = (int) (0.0+cnr2);
02464       circ(lt+kcirc) = xim(xold, yold);
02465 
02466       if ( mode == 'f' || mode == 'F' ) {
02467          xold  = (int) (0.0+cns2);
02468          yold = (int) (-inr+cnr2);
02469          circ(lt+lt+kcirc) = xim(xold, yold);
02470 
02471          xold  = (int) (-inr+cns2);
02472          yold = (int) (0.0+cnr2);
02473          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02474       }
02475 
02476       for (jt=1; jt<=nsim; jt++) {
02477          fi   = dfi * jt;
02478          x    = sin(fi) * yq;
02479          y    = cos(fi) * yq;
02480 
02481          xold  = (int) (x+cns2);
02482          yold = (int) (y+cnr2);
02483          circ(jt+kcirc) = xim(xold, yold);
02484 
02485          xold  = (int) (y+cns2);
02486          yold = (int) (-x+cnr2);
02487          circ(jt+lt+kcirc) = xim(xold, yold);
02488 
02489          if ( mode == 'f' || mode == 'F' ) {
02490             xold  = (int) (-x+cns2);
02491             yold = (int) (-y+cnr2);
02492             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02493 
02494             xold  = (int) (-y+cns2);
02495             yold = (int) (x+cnr2);
02496             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02497          }
02498       } // end for jt
02499    } //end for it
02500 }
02501 */
02502 //xim((int) floor(xold), (int) floor(yold))
02503 #undef  xim
02504 
02505 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02506 // input image is twice the size of the original image
02507         int nring = numr.size()/3;
02508         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02509         EMData* out = new EMData();
02510         out->set_size(lcirc,1,1);
02511         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02512         float *circ = out->get_data();
02513         float *fimage = image->get_data();
02514         int nx = image->get_xsize();
02515         int ny = image->get_ysize();
02516         int nz = image->get_zsize();
02517         double dpi, dfi;
02518         int    it, jt, inr, l, nsim, kcirc, lt;
02519         float  yq, xold, yold, fi, x, y;
02520 
02521         //     cns2 and cnr2 are predefined centers
02522         //     no need to set to zero, all elements are defined
02523 
02524         dpi = 2*atan(1.0);
02525         for (it=1;it<=nring;it++) {
02526                 // radius of the ring
02527                 inr = numr(1,it);
02528                 yq  = static_cast<float>(inr);
02529 
02530                 l = numr(3,it);
02531                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02532                 else                               lt = l / 4;
02533 
02534                 nsim  = lt - 1;
02535                 dfi   = dpi / (nsim+1);
02536                 kcirc = numr(2,it);
02537                 xold  = 0.0f;
02538                 yold  = static_cast<float>(inr);
02539                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02540 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02541 
02542                 xold  = static_cast<float>(inr);
02543                 yold  = 0.0f;
02544                 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02545 //      circ(lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02546 
02547         if ( mode == 'f' || mode == 'F' ) {
02548                 xold = 0.0f;
02549                 yold = static_cast<float>(-inr);
02550                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02551 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02552 
02553                 xold = static_cast<float>(-inr);
02554                 yold = 0.0f;
02555                 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02556 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02557         }
02558 
02559         for (jt=1;jt<=nsim;jt++) {
02560                 fi   = static_cast<float>(dfi * jt);
02561                 x    = sin(fi) * yq;
02562                 y    = cos(fi) * yq;
02563 
02564                 xold = x;
02565                 yold = y;
02566                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02567 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02568 
02569                 xold = y;
02570                 yold = -x;
02571                 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02572 //         circ(jt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02573 
02574         if ( mode == 'f' || mode == 'F' ) {
02575                 xold = -x;
02576                 yold = -y;
02577                 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02578 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02579 
02580                 xold = -y;
02581                 yold = x;
02582                 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02583 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02584         }
02585         } // end for jt
02586         } //end for it
02587         return  out;
02588 }
02589 
02590 /*
02591 
02592         A set of 1-D power-of-two FFTs
02593         Pawel & Chao 01/20/06
02594 
02595 fftr_q(xcmplx,nv)
02596   single precision
02597 
02598  dimension xcmplx(2,iabs(nv)/2);
02599  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02600  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02601 
02602 
02603 fftr_d(xcmplx,nv)
02604   double precision
02605 
02606  dimension xcmplx(2,iabs(nv)/2);
02607  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02608  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02609 
02610 
02611 
02612 */
02613 #define  tab1(i)      tab1[i-1]
02614 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02615 #define  br(i)        br[i-1]
02616 #define  bi(i)        bi[i-1]
02617 //-----------------------------------------
02618 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02619 {
02620         double rni,sgn,tr1,tr2,ti1,ti2;
02621         double cc,c,ss,s,t,x2,x3,x4,x5;
02622         int    b3,b4,b5,b6,b7,b56;
02623         int    n, k, l, j, i, ix0, ix1, status=0;
02624 
02625         const double tab1[] = {
02626                 9.58737990959775e-5,
02627                 1.91747597310703e-4,
02628                 3.83495187571395e-4,
02629                 7.66990318742704e-4,
02630                 1.53398018628476e-3,
02631                 3.06795676296598e-3,
02632                 6.13588464915449e-3,
02633                 1.22715382857199e-2,
02634                 2.45412285229123e-2,
02635                 4.90676743274181e-2,
02636                 9.80171403295604e-2,
02637                 1.95090322016128e-1,
02638                 3.82683432365090e-1,
02639                 7.07106781186546e-1,
02640                 1.00000000000000,
02641         };
02642 
02643         n=(int)pow(2.0f,ln);
02644 
02645         k=abs(ks);
02646         l=16-ln;
02647         b3=n*k;
02648         b6=b3;
02649         b7=k;
02650         if (ks > 0) {
02651                 sgn=1.0f;
02652         } else {
02653                 sgn=-1.0f;
02654                 rni=1.0f/(float)(n);
02655                 j=1;
02656                 for (i=1; i<=n; i++) {
02657                         br(j)=br(j)*rni;
02658                         bi(j)=bi(j)*rni;
02659                         j=j+k;
02660                 }
02661         }
02662 
02663 L12:
02664    b6=b6/2;
02665    b5=b6;
02666    b4=2*b6;
02667    b56=b5-b6;
02668 
02669 L14:
02670    tr1=br(b5+1);
02671    ti1=bi(b5+1);
02672    tr2=br(b56+1);
02673    ti2=bi(b56+1);
02674 
02675    br(b5+1)=tr2-tr1;
02676    bi(b5+1)=ti2-ti1;
02677    br(b56+1)=tr1+tr2;
02678    bi(b56+1)=ti1+ti2;
02679 
02680    b5=b5+b4;
02681    b56=b5-b6;
02682    if ( b5 <= b3 )  goto  L14;
02683    if ( b6 == b7 )  goto  L20;
02684 
02685    b4=b7;
02686    cc=2.0f*pow(tab1(l),2);
02687    c=1.0f-cc;
02688    l++;
02689    ss=sgn*tab1(l);
02690    s=ss;
02691 
02692 L16:
02693    b5=b6+b4;
02694    b4=2*b6;
02695    b56=b5-b6;
02696 
02697 L18:
02698    tr1=br(b5+1);
02699    ti1=bi(b5+1);
02700    tr2=br(b56+1);
02701    ti2=bi(b56+1);
02702    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02703    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02704    br(b56+1)=tr1+tr2;
02705    bi(b56+1)=ti1+ti2;
02706 
02707    b5=b5+b4;
02708    b56=b5-b6;
02709    if ( b5 <= b3 )  goto  L18;
02710    b4=b5-b6;
02711    b5=b4-b3;
02712    c=-c;
02713    b4=b6-b5;
02714    if ( b5 < b4 )  goto  L16;
02715    b4=b4+b7;
02716    if ( b4 >= b5 ) goto  L12;
02717 
02718    t=c-cc*c-ss*s;
02719    s=s+ss*c-cc*s;
02720    c=t;
02721    goto  L16;
02722 
02723 L20:
02724    ix0=b3/2;
02725    b3=b3-b7;
02726    b4=0;
02727    b5=0;
02728    b6=ix0;
02729    ix1=0;
02730    if (b6 == b7) goto EXIT;
02731 
02732 L22:
02733    b4=b3-b4;
02734    b5=b3-b5;
02735    x2=br(b4+1);
02736    x3=br(b5+1);
02737    x4=bi(b4+1);
02738    x5=bi(b5+1);
02739    br(b4+1)=x3;
02740    br(b5+1)=x2;
02741    bi(b4+1)=x5;
02742    bi(b5+1)=x4;
02743    if(b6 < b4)  goto  L22;
02744 
02745 L24:
02746    b4=b4+b7;
02747    b5=b6+b5;
02748    x2=br(b4+1);
02749    x3=br(b5+1);
02750    x4=bi(b4+1);
02751    x5=bi(b5+1);
02752    br(b4+1)=x3;
02753    br(b5+1)=x2;
02754    bi(b4+1)=x5;
02755    bi(b5+1)=x4;
02756    ix0=b6;
02757 
02758 L26:
02759    ix0=ix0/2;
02760    ix1=ix1-ix0;
02761    if( ix1 >= 0)  goto L26;
02762 
02763    ix0=2*ix0;
02764    b4=b4+b7;
02765    ix1=ix1+ix0;
02766    b5=ix1;
02767    if ( b5 >= b4)  goto  L22;
02768    if ( b4 < b6)   goto  L24;
02769 
02770 EXIT:
02771    status = 0;
02772 }
02773 
02774 // -----------------------------------------------------------------
02775 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02776 {
02777         //  dimension  br(1),bi(1)
02778 
02779         int b3,b4,b5,b6,b7,b56;
02780         int n, k, l, j, i, ix0, ix1;
02781         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02782         int status=0;
02783 
02784         const float tab1[] = {
02785                 9.58737990959775e-5f,
02786                 1.91747597310703e-4f,
02787                 3.83495187571395e-4f,
02788                 7.66990318742704e-4f,
02789                 1.53398018628476e-3f,
02790                 3.06795676296598e-3f,
02791                 6.13588464915449e-3f,
02792                 1.22715382857199e-2f,
02793                 2.45412285229123e-2f,
02794                 4.90676743274181e-2f,
02795                 9.80171403295604e-2f,
02796                 1.95090322016128e-1f,
02797                 3.82683432365090e-1f,
02798                 7.07106781186546e-1f,
02799                 1.00000000000000f,
02800         };
02801 
02802         n=(int)pow(2.0f,ln);
02803 
02804         k=abs(ks);
02805         l=16-ln;
02806         b3=n*k;
02807         b6=b3;
02808         b7=k;
02809         if( ks > 0 ) {
02810                 sgn=1.0f;
02811         } else {
02812                 sgn=-1.0f;
02813                 rni=1.0f/(float)n;
02814                 j=1;
02815                 for (i=1; i<=n; i++) {
02816                         br(j)=br(j)*rni;
02817                         bi(j)=bi(j)*rni;
02818                         j=j+k;
02819                 }
02820         }
02821 L12:
02822    b6=b6/2;
02823    b5=b6;
02824    b4=2*b6;
02825    b56=b5-b6;
02826 L14:
02827    tr1=br(b5+1);
02828    ti1=bi(b5+1);
02829 
02830    tr2=br(b56+1);
02831    ti2=bi(b56+1);
02832 
02833    br(b5+1)=tr2-tr1;
02834    bi(b5+1)=ti2-ti1;
02835    br(b56+1)=tr1+tr2;
02836    bi(b56+1)=ti1+ti2;
02837 
02838    b5=b5+b4;
02839    b56=b5-b6;
02840    if ( b5 <= b3 )  goto  L14;
02841    if ( b6 == b7 )  goto  L20;
02842 
02843    b4=b7;
02844    cc=2.0f*pow(tab1(l),2);
02845    c=1.0f-cc;
02846    l++;
02847    ss=sgn*tab1(l);
02848    s=ss;
02849 L16:
02850    b5=b6+b4;
02851    b4=2*b6;
02852    b56=b5-b6;
02853 L18:
02854    tr1=br(b5+1);
02855    ti1=bi(b5+1);
02856    tr2=br(b56+1);
02857    ti2=bi(b56+1);
02858    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02859    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02860    br(b56+1)=tr1+tr2;
02861    bi(b56+1)=ti1+ti2;
02862 
02863    b5=b5+b4;
02864    b56=b5-b6;
02865    if(b5 <= b3)  goto L18;
02866    b4=b5-b6;
02867    b5=b4-b3;
02868    c=-c;
02869    b4=b6-b5;
02870    if(b5 < b4)  goto  L16;
02871    b4=b4+b7;
02872    if(b4 >= b5) goto  L12;
02873 
02874    t=c-cc*c-ss*s;
02875    s=s+ss*c-cc*s;
02876    c=t;
02877    goto  L16;
02878 L20:
02879    ix0=b3/2;
02880    b3=b3-b7;
02881    b4=0;
02882    b5=0;
02883    b6=ix0;
02884    ix1=0;
02885    if ( b6 == b7) goto EXIT;
02886 L22:
02887    b4=b3-b4;
02888    b5=b3-b5;
02889    x2=br(b4+1);
02890    x3=br(b5+1);
02891    x4=bi(b4+1);
02892    x5=bi(b5+1);
02893    br(b4+1)=x3;
02894    br(b5+1)=x2;
02895    bi(b4+1)=x5;
02896    bi(b5+1)=x4;
02897    if (b6 < b4) goto  L22;
02898 L24:
02899    b4=b4+b7;
02900    b5=b6+b5;
02901    x2=br(b4+1);
02902    x3=br(b5+1);
02903    x4=bi(b4+1);
02904    x5=bi(b5+1);
02905    br(b4+1)=x3;
02906    br(b5+1)=x2;
02907    bi(b4+1)=x5;
02908    bi(b5+1)=x4;
02909    ix0=b6;
02910 L26:
02911    ix0=ix0/2;
02912    ix1=ix1-ix0;
02913    if(ix1 >= 0)  goto  L26;
02914 
02915    ix0=2*ix0;
02916    b4=b4+b7;
02917    ix1=ix1+ix0;
02918    b5=ix1;
02919    if (b5 >= b4)  goto  L22;
02920    if (b4 < b6)   goto  L24;
02921 EXIT:
02922    status = 0;
02923 }
02924 
02925 void  Util::fftr_q(float *xcmplx, int nv)
02926 {
02927    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02928 
02929         int nu, inv, nu1, n, isub, n2, i1, i2, i;
02930         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02931 
02932         const float tab1[] = {
02933                 9.58737990959775e-5f,
02934                 1.91747597310703e-4f,
02935                 3.83495187571395e-4f,
02936                 7.66990318742704e-4f,
02937                 1.53398018628476e-3f,
02938                 3.06795676296598e-3f,
02939                 6.13588464915449e-3f,
02940                 1.22715382857199e-2f,
02941                 2.45412285229123e-2f,
02942                 4.90676743274181e-2f,
02943                 9.80171403295604e-2f,
02944                 1.95090322016128e-1f,
02945                 3.82683432365090e-1f,
02946                 7.07106781186546e-1f,
02947                 1.00000000000000f,
02948         };
02949 
02950         nu=abs(nv);
02951         inv=nv/nu;
02952         nu1=nu-1;
02953         n=(int)pow(2.f,nu1);
02954         isub=16-nu1;
02955 
02956         ss=-tab1(isub);
02957         cc=-2.0f*pow(tab1(isub-1),2.f);
02958         c=1.0f;
02959         s=0.0f;
02960         n2=n/2;
02961         if ( inv > 0) {
02962                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02963                 tr=xcmplx(1,1);
02964                 ti=xcmplx(2,1);
02965                 xcmplx(1,1)=tr+ti;
02966                 xcmplx(2,1)=tr-ti;
02967                 for (i=1;i<=n2;i++) {
02968                         i1=i+1;
02969                         i2=n-i+1;
02970                         tr1=xcmplx(1,i1);
02971                         tr2=xcmplx(1,i2);
02972                         ti1=xcmplx(2,i1);
02973                         ti2=xcmplx(2,i2);
02974                         t=(cc*c-ss*s)+c;
02975                         s=(cc*s+ss*c)+s;
02976                         c=t;
02977                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02978                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02979                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02981                 }
02982         } else {
02983                 tr=xcmplx(1,1);
02984                 ti=xcmplx(2,1);
02985                 xcmplx(1,1)=0.5f*(tr+ti);
02986                 xcmplx(2,1)=0.5f*(tr-ti);
02987                 for (i=1; i<=n2; i++) {
02988                         i1=i+1;
02989                         i2=n-i+1;
02990                         tr1=xcmplx(1,i1);
02991                         tr2=xcmplx(1,i2);
02992                         ti1=xcmplx(2,i1);
02993                         ti2=xcmplx(2,i2);
02994                         t=(cc*c-ss*s)+c;
02995                         s=(cc*s+ss*c)+s;
02996                         c=t;
02997                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02998                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02999                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03001                 }
03002                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03003         }
03004 }
03005 
03006 // -------------------------------------------
03007 void  Util::fftr_d(double *xcmplx, int nv)
03008 {
03009         // double precision  x(2,1)
03010         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03011         double tr1,tr2,ti1,ti2,tr,ti;
03012         double cc,c,ss,s,t;
03013         const double tab1[] = {
03014                 9.58737990959775e-5,
03015                 1.91747597310703e-4,
03016                 3.83495187571395e-4,
03017                 7.66990318742704e-4,
03018                 1.53398018628476e-3,
03019                 3.06795676296598e-3,
03020                 6.13588464915449e-3,
03021                 1.22715382857199e-2,
03022                 2.45412285229123e-2,
03023                 4.90676743274181e-2,
03024                 9.80171403295604e-2,
03025                 1.95090322016128e-1,
03026                 3.82683432365090e-1,
03027                 7.07106781186546e-1,
03028                 1.00000000000000,
03029         };
03030 
03031         nu=abs(nv);
03032         inv=nv/nu;
03033         nu1=nu-1;
03034         n=(int)pow(2.0f,nu1);
03035         isub=16-nu1;
03036         ss=-tab1(isub);
03037         cc=-2.0*pow(tab1(isub-1),2);
03038         c=1.0f;
03039         s=0.0f;
03040         n2=n/2;
03041 
03042         if ( inv > 0 ) {
03043                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03044                 tr=xcmplx(1,1);
03045                 ti=xcmplx(2,1);
03046                 xcmplx(1,1)=tr+ti;
03047                 xcmplx(2,1)=tr-ti;
03048                 for (i=1;i<=n2;i++) {
03049                         i1=i+1;
03050                         i2=n-i+1;
03051                         tr1=xcmplx(1,i1);
03052                         tr2=xcmplx(1,i2);
03053                         ti1=xcmplx(2,i1);
03054                         ti2=xcmplx(2,i2);
03055                         t=(cc*c-ss*s)+c;
03056                         s=(cc*s+ss*c)+s;
03057                         c=t;
03058                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03059                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03060                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03062                 }
03063         } else {
03064                 tr=xcmplx(1,1);
03065                 ti=xcmplx(2,1);
03066                 xcmplx(1,1)=0.5*(tr+ti);
03067                 xcmplx(2,1)=0.5*(tr-ti);
03068                 for (i=1; i<=n2; i++) {
03069                         i1=i+1;
03070                         i2=n-i+1;
03071                         tr1=xcmplx(1,i1);
03072                         tr2=xcmplx(1,i2);
03073                         ti1=xcmplx(2,i1);
03074                         ti2=xcmplx(2,i2);
03075                         t=(cc*c-ss*s)+c;
03076                         s=(cc*s+ss*c)+s;
03077                         c=t;
03078                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03079                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03080                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03082                 }
03083                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03084         }
03085 }
03086 #undef  tab1
03087 #undef  xcmplx
03088 #undef  br
03089 #undef  bi
03090 
03091 
03092 void Util::Frngs(EMData* circp, vector<int> numr){
03093         int nring = numr.size()/3;
03094         float *circ = circp->get_data();
03095         int i, l;
03096         for (i=1; i<=nring;i++) {
03097 
03098 #ifdef _WIN32
03099                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03100 #else
03101                 l=(int)(log2(numr(3,i)));
03102 #endif  //_WIN32
03103 
03104                 fftr_q(&circ(numr(2,i)),l);
03105         }
03106 }
03107 
03108 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03109         int nring = numr.size()/3;
03110         float *circ = circp->get_data();
03111         int i, l;
03112         for (i=1; i<=nring;i++) {
03113 
03114 #ifdef _WIN32
03115                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03116 #else
03117                 l=(int)(log2(numr(3,i)));
03118 #endif  //_WIN32
03119 
03120                 fftr_q(&circ(numr(2,i)),-l);
03121         }
03122 }
03123 #undef  circ
03124 
03125 #define  b(i)            b[i-1]
03126 void Util::prb1d(double *b, int npoint, float *pos) {
03127         double  c2,c3;
03128         int     nhalf;
03129 
03130         nhalf = npoint/2 + 1;
03131         *pos  = 0.0;
03132 
03133         if (npoint == 7) {
03134                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03135                      - 6.*b(6) + 31.*b(7);
03136                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03137         }
03138         else if (npoint == 5) {
03139                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03140                    + 46.*b(5) ) / (-70.);
03141                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03142         }
03143         else if (npoint == 3) {
03144                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03145                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03146         }
03147         //else if (npoint == 9) {
03148         else  { // at least one has to be true!!
03149                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03150                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03151                      + 1092.*b(9) ) / (-4620.);
03152                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03153                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03154         }
03155         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03156 }
03157 #undef  b
03158 
03159 #define  circ1(i)        circ1[i-1]
03160 #define  circ2(i)        circ2[i-1]
03161 #define  t(i)            t[i-1]
03162 #define  q(i)            q[i-1]
03163 #define  b(i)            b[i-1]
03164 #define  t7(i)           t7[i-1]
03165 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03166         //  neg = 0 straight,  neg = 1 mirrored
03167         int nring = numr.size()/3;
03168         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03169         int maxrin = numr[numr.size()-1];
03170         double qn;   float  tot;
03171         float *circ1 = circ1p->get_data();
03172         float *circ2 = circ2p->get_data();
03173 /*
03174 c checks single position, neg is flag for checking mirrored position
03175 c
03176 c  input - fourier transforms of rings!
03177 c  first set is conjugated (mirrored) if neg
03178 c  circ1 already multiplied by weights!
03179 c       automatic arrays
03180         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03181         double precision  q(maxrin)
03182         double precision  t7(-3:3)
03183 */
03184         float *t;
03185         double t7[7], *q;
03186         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03187         float  pos;
03188 
03189 #ifdef _WIN32
03190         ip = -(int)(log((float)maxrin)/log(2.0f));
03191 #else
03192         ip = -(int) (log2(maxrin));
03193 #endif  //_WIN32
03194 
03195         q = (double*)calloc(maxrin, sizeof(double));
03196         t = (float*)calloc(maxrin, sizeof(float));
03197 
03198 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03199         for (i=1; i<=nring; i++) {
03200                 numr3i = numr(3,i);
03201                 numr2i = numr(2,i);
03202 
03203                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03204 
03205                 if (numr3i != maxrin) {
03206                          // test .ne. first for speed on some compilers
03207                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03208                         t(2)            = 0.0;
03209 
03210                         if (neg) {
03211                                 // first set is conjugated (mirrored)
03212                                 for (j=3;j<=numr3i;j=j+2) {
03213                                         jc = j+numr2i-1;
03214                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03215                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03216                                 }
03217                         } else {
03218                                 for (j=3;j<=numr3i;j=j+2) {
03219                                         jc = j+numr2i-1;
03220                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03221                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03222                                 }
03223                         }
03224                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03225                 } else {
03226                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03227                         if (neg) {
03228                                 // first set is conjugated (mirrored)
03229                                 for (j=3;j<=maxrin;j=j+2) {
03230                                         jc = j+numr2i-1;
03231                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03232                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03233                                 }
03234                         } else {
03235                                 for (j=3;j<=maxrin;j=j+2) {
03236                                         jc = j+numr2i-1;
03237                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03238                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03239                                 }
03240                         }
03241                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03242                 }
03243         }
03244 
03245         fftr_d(q,ip);
03246 
03247         qn = -1.0e20;
03248         for (j=1;j<=maxrin;j++) {
03249            if (q(j) >= qn) {
03250                   qn = q(j); jtot = j;
03251            }
03252         }
03253 
03254         for (k=-3; k<=3; k++) {
03255                 j = (jtot+k+maxrin-1)%maxrin + 1;
03256                 t7(k+4) = q(j);
03257         }
03258 
03259         prb1d(t7,7,&pos);
03260 
03261         tot = (float)jtot + pos;
03262 
03263         if (q) free(q);
03264         if (t) free(t);
03265 
03266         Dict retvals;
03267         retvals["qn"] = qn;
03268         retvals["tot"] = tot;
03269         return  retvals;
03270 }
03271 
03272 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03273    //  neg = 0 straight,  neg = 1 mirrored
03274         int nring = numr.size()/3;
03275         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03276         int maxrin = numr[numr.size()-1];
03277         double qn;   float  tot;
03278         float *circ1 = circ1p->get_data();
03279         float *circ2 = circ2p->get_data();
03280 /*
03281 c checks single position, neg is flag for checking mirrored position
03282 c
03283 c  input - fourier transforms of rings!
03284 c  first set is conjugated (mirrored) if neg
03285 c  multiplication by weights!
03286 c       automatic arrays
03287         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03288         double precision  q(maxrin)
03289         double precision  t7(-3:3)
03290 */
03291         float *t;
03292         double t7[7], *q;
03293         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03294         float  pos;
03295 
03296 #ifdef _WIN32
03297         ip = -(int)(log((float)maxrin)/log(2.0f));
03298 #else
03299         ip = -(int) (log2(maxrin));
03300 #endif  //_WIN32
03301 
03302         q = (double*)calloc(maxrin, sizeof(double));
03303         t = (float*)calloc(maxrin, sizeof(float));
03304 
03305 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03306         for (i=1;i<=nring;i++) {
03307                 numr3i = numr(3,i);
03308                 numr2i = numr(2,i);
03309 
03310                 t(1) = circ1(numr2i) * circ2(numr2i);
03311 
03312                 if (numr3i != maxrin) {
03313                         // test .ne. first for speed on some compilers
03314                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03315                         t(2)      = 0.0;
03316 
03317                         if (neg) {
03318                                 // first set is conjugated (mirrored)
03319                                 for (j=3; j<=numr3i; j=j+2) {
03320                                         jc = j+numr2i-1;
03321                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03322                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03323                                 }
03324                         } else {
03325                                 for (j=3; j<=numr3i; j=j+2) {
03326                                         jc = j+numr2i-1;
03327                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03328                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03329                                 }
03330                         }
03331                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03332                 } else {
03333                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03334                         if (neg) {
03335                                 // first set is conjugated (mirrored)
03336                                 for (j=3; j<=maxrin; j=j+2) {
03337                                         jc = j+numr2i-1;
03338                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03339                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03340                                 }
03341                         } else {
03342                                 for (j=3; j<=maxrin; j=j+2) {
03343                                 jc = j+numr2i-1;
03344                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03345                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03346                                 }
03347                         }
03348                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03349                 }
03350         }
03351 
03352         fftr_d(q,ip);
03353 
03354         qn = -1.0e20;
03355         for (j=1;j<=maxrin;j++) {
03356                 //cout << j << "  " << q(j) << endl;
03357                 if (q(j) >= qn) {
03358                         qn = q(j);
03359                         jtot = j;
03360                 }
03361         }
03362 
03363         for (k=-3; k<=3; k++) {
03364                 j = (jtot+k+maxrin-1)%maxrin + 1;
03365                 t7(k+4) = q(j);
03366         }
03367 
03368         prb1d(t7,7,&pos);
03369 
03370         tot = (float)jtot + pos;
03371 
03372         //if (q) free(q);
03373         if (t) free(t);
03374 
03375         Dict retvals;
03376         //tot = 1;
03377         //qn = q(1);
03378         retvals["qn"] = qn;
03379         retvals["tot"] = tot;
03380 
03381         if (q) free(q);
03382 
03383         return  retvals;
03384 }
03385 
03386 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03387         int nring = numr.size()/3;
03388         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03389         int maxrin = numr[numr.size()-1];
03390         double qn; float tot; double qm; float tmt;
03391         float *circ1 = circ1p->get_data();
03392         float *circ2 = circ2p->get_data();
03393 /*
03394 c
03395 c  checks both straight & mirrored positions
03396 c
03397 c  input - fourier transforms of rings!!
03398 c  circ1 already multiplied by weights!
03399 c
03400 */
03401 
03402         // dimension             circ1(lcirc),circ2(lcirc)
03403 
03404         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03405         double *t, *q, t7[7];
03406 
03407         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03408         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03409 
03410         qn  = 0.0f;
03411         qm  = 0.0f;
03412         tot = 0.0f;
03413         tmt = 0.0f;
03414 #ifdef _WIN32
03415         ip = -(int)(log((float)maxrin)/log(2.0f));
03416 #else
03417         ip = -(int)(log2(maxrin));
03418 #endif  //_WIN32
03419   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03420 
03421         //  c - straight  = circ1 * conjg(circ2)
03422         //  zero q array
03423 
03424         q = (double*)calloc(maxrin,sizeof(double));
03425 
03426         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03427         //   zero t array
03428         t = (double*)calloc(maxrin,sizeof(double));
03429 
03430    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03431         for (i=1; i<=nring; i++) {
03432 
03433                 numr3i = numr(3,i);   // Number of samples of this ring
03434                 numr2i = numr(2,i);   // The beginning point of this ring
03435 
03436                 t1   = circ1(numr2i) * circ2(numr2i);
03437                 q(1) += t1;
03438                 t(1) += t1;
03439 
03440                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03441                 if (numr3i == maxrin)  {
03442                         q(2) += t1;
03443                         t(2) += t1;
03444                 } else {
03445                         q(numr3i+1) += t1;
03446                         t(numr3i+1) += t1;
03447                 }
03448 
03449                 for (j=3; j<=numr3i; j += 2) {
03450                         jc     = j+numr2i-1;
03451 
03452 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03453 //                                ----- -----    ----- -----
03454 //                                 t1     t2      t3    t4
03455 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03456 //                                    ----- -----    ----- -----
03457 //                                     t1    t2       t3    t4
03458 
03459                         c1     = circ1(jc);
03460                         c2     = circ1(jc+1);
03461                         d1     = circ2(jc);
03462                         d2     = circ2(jc+1);
03463 
03464                         t1     = c1 * d1;
03465                         t2     = c2 * d2;
03466                         t3     = c1 * d2;
03467                         t4     = c2 * d1;
03468 
03469                         q(j)   += t1 + t2;
03470                         q(j+1) += -t3 + t4;
03471                         t(j)   += t1 - t2;
03472                         t(j+1) += -t3 - t4;
03473                 }
03474         }
03475         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03476         fftr_d(q,ip);
03477 
03478         qn  = -1.0e20;
03479         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03480                 if (q(j) >= qn) {
03481                         qn  = q(j);
03482                         jtot = j;
03483                 }
03484         }
03485 
03486         for (k=-3; k<=3; k++) {
03487                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03488                 t7(k+4) = q(j);
03489         }
03490 
03491         // interpolate
03492         prb1d(t7,7,&pos);
03493         tot = (float)(jtot)+pos;
03494         // Do not interpolate
03495         //tot = (float)(jtot);
03496 
03497         // mirrored
03498         fftr_d(t,ip);
03499 
03500         // find angle
03501         qm = -1.0e20;
03502         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03503                 if ( t(j) >= qm ) {
03504                         qm   = t(j);
03505                         jtot = j;
03506                 }
03507         }
03508 
03509         for (k=-3; k<=3; k++)  {
03510                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03511                 t7(k+4) = t(j);
03512         }
03513 
03514         // interpolate
03515 
03516         prb1d(t7,7,&pos);
03517         tmt = float(jtot) + pos;
03518         // Do not interpolate
03519         //tmt = float(jtot);
03520 
03521         free(t);
03522         free(q);
03523 
03524         Dict retvals;
03525         retvals["qn"] = qn;
03526         retvals["tot"] = tot;
03527         retvals["qm"] = qm;
03528         retvals["tmt"] = tmt;
03529         return retvals;
03530 }
03531 
03532 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03533         int nring = numr.size()/3;
03534         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03535         int maxrin = numr[numr.size()-1];
03536         double qn; float tot; double qm; float tmt;
03537         float *circ1 = circ1p->get_data();
03538         float *circ2 = circ2p->get_data();
03539 /*
03540 c
03541 c  checks both straight & mirrored positions
03542 c
03543 c  input - fourier transforms of rings!!
03544 c  circ1 already multiplied by weights!
03545 c
03546 */
03547 
03548         // dimension             circ1(lcirc),circ2(lcirc)
03549 
03550         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03551         double *t, *q, t7[7];
03552 
03553         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03554         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03555 
03556         qn  = 0.0f;
03557         qm  = 0.0f;
03558         tot = 0.0f;
03559         tmt = 0.0f;
03560 #ifdef _WIN32
03561         ip = -(int)(log((float)maxrin)/log(2.0f));
03562 #else
03563         ip = -(int)(log2(maxrin));
03564 #endif  //_WIN32
03565   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03566 
03567         //  c - straight  = circ1 * conjg(circ2)
03568         //  zero q array
03569 
03570         q = (double*)calloc(maxrin,sizeof(double));
03571 
03572         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03573         //   zero t array
03574         t = (double*)calloc(maxrin,sizeof(double));
03575 
03576    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03577         for (i=1; i<=nring; i++) {
03578 
03579                 numr3i = numr(3,i);   // Number of samples of this ring
03580                 numr2i = numr(2,i);   // The beginning point of this ring
03581 
03582                 t1   = circ1(numr2i) * circ2(numr2i);
03583                 q(1) += t1;
03584                 t(1) += t1;
03585 
03586                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03587                 if (numr3i == maxrin)  {
03588                         q(2) += t1;
03589                         t(2) += t1;
03590                 } else {
03591                         q(numr3i+1) += t1;
03592                         t(numr3i+1) += t1;
03593                 }
03594 
03595                 for (j=3; j<=numr3i; j += 2) {
03596                         jc     = j+numr2i-1;
03597 
03598 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03599 //                                ----- -----    ----- -----
03600 //                                 t1     t2      t3    t4
03601 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03602 //                                    ----- -----    ----- -----
03603 //                                     t1    t2       t3    t4
03604 
03605                         c1     = circ1(jc);
03606                         c2     = circ1(jc+1);
03607                         d1     = circ2(jc);
03608                         d2     = circ2(jc+1);
03609 
03610                         t1     = c1 * d1;
03611                         t2     = c2 * d2;
03612                         t3     = c1 * d2;
03613                         t4     = c2 * d1;
03614 
03615                         q(j)   += t1 + t2;
03616                         q(j+1) += -t3 + t4;
03617                         t(j)   += t1 - t2;
03618                         t(j+1) += -t3 - t4;
03619                 }
03620         }
03621         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03622         fftr_d(q,ip);
03623 
03624         qn  = -1.0e20;
03625         
03626         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03627         int jstep = static_cast<int>(delta/360.0*maxrin);
03628         if (jstep < 1) { jstep = 1; } 
03629         
03630         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03631                 if (q(j) >= qn) {
03632                         qn  = q(j);
03633                         jtot = j;
03634                 }
03635         }
03636 
03637         //for (k=-3; k<=3; k++) {
03638         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03639         //      t7(k+4) = q(j);
03640         //}
03641 
03642         // interpolate
03643         //prb1d(t7,7,&pos);
03644         //tot = (float)(jtot)+pos;
03645         // Do not interpolate
03646         tot = (float)(jtot);
03647 
03648         // mirrored
03649         fftr_d(t,ip);
03650 
03651         // find angle
03652         qm = -1.0e20;
03653         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03654                 if ( t(j) >= qm ) {
03655                         qm   = t(j);
03656                         jtot = j;
03657                 }
03658         }
03659 
03660         //for (k=-3; k<=3; k++)  {
03661         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03662         //      t7(k+4) = t(j);
03663         //}
03664 
03665         // interpolate
03666 
03667         //prb1d(t7,7,&pos);
03668         //tmt = float(jtot) + pos;
03669         // Do not interpolate
03670         tmt = float(jtot);
03671 
03672         free(t);
03673         free(q);
03674 
03675         Dict retvals;
03676         retvals["qn"] = qn;
03677         retvals["tot"] = tot;
03678         retvals["qm"] = qm;
03679         retvals["tmt"] = tmt;
03680         return retvals;
03681 }
03682 
03683 
03684 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03685         int nring = numr.size()/3;
03686         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03687         int maxrin = numr[numr.size()-1];
03688         double qn; float tot; double qm; float tmt;
03689         float *circ1 = circ1p->get_data();
03690         float *circ2 = circ2p->get_data();
03691 
03692         // dimension             circ1(lcirc),circ2(lcirc)
03693 
03694         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03695         double *t, *q, t7[7];
03696 
03697         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03698         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03699 
03700         qn  = 0.0f;
03701         qm  = 0.0f;
03702         tot = 0.0f;
03703         tmt = 0.0f;
03704 #ifdef _WIN32
03705         ip = -(int)(log((float)maxrin)/log(2.0f));
03706 #else
03707         ip = -(int)(log2(maxrin));
03708 #endif  //_WIN32
03709   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03710 
03711         //  c - straight  = circ1 * conjg(circ2)
03712         //  zero q array
03713 
03714         q = (double*)calloc(maxrin,sizeof(double));
03715 
03716         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03717         //   zero t array
03718         t = (double*)calloc(maxrin,sizeof(double));
03719 
03720    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03721         for (i=1; i<=nring; i++) {
03722 
03723                 numr3i = numr(3,i);   // Number of samples of this ring
03724                 numr2i = numr(2,i);   // The beginning point of this ring
03725 
03726                 t1   = circ1(numr2i) * circ2(numr2i);
03727                 q(1) += t1;
03728                 t(1) += t1;
03729 
03730                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03731                 if (numr3i == maxrin)  {
03732                         q(2) += t1;
03733                         t(2) += t1;
03734                 } else {
03735                         q(numr3i+1) += t1;
03736                         t(numr3i+1) += t1;
03737                 }
03738 
03739                 for (j=3; j<=numr3i; j += 2) {
03740                         jc     = j+numr2i-1;
03741 
03742 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03743 //                                ----- -----    ----- -----
03744 //                                 t1     t2      t3    t4
03745 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03746 //                                    ----- -----    ----- -----
03747 //                                     t1    t2       t3    t4
03748 
03749                         c1     = circ1(jc);
03750                         c2     = circ1(jc+1);
03751                         d1     = circ2(jc);
03752                         d2     = circ2(jc+1);
03753 
03754                         t1     = c1 * d1;
03755                         t2     = c2 * d2;
03756                         t3     = c1 * d2;
03757                         t4     = c2 * d1;
03758 
03759                         q(j)   += t1 + t2;
03760                         q(j+1) += -t3 + t4;
03761                         t(j)   += t1 - t2;
03762                         t(j+1) += -t3 - t4;
03763                 }
03764         }
03765         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03766         fftr_d(q,ip);
03767 
03768         int psi_range  = int(psi_max/360.0*maxrin+0.5);
03769         const int psi_0 = 0;
03770         int psi_180    = int(  180.0/360.0*maxrin+0.5);
03771 
03772         qn  = -1.0e20;
03773         for (k=-psi_range; k<=psi_range; k++) {
03774                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03775                 if (q(j) >= qn) {
03776                         qn  = q(j);
03777                         jtot = j;
03778                 }
03779         }
03780 
03781         for (k=-psi_range; k<=psi_range; k++) {
03782                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03783                 if (q(j) >= qn) {
03784                         qn  = q(j);
03785                         jtot = j;
03786                 }
03787         }
03788 
03789         for (k=-3; k<=3; k++) {
03790                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03791                 t7(k+4) = q(j);
03792         }
03793 
03794         // interpolate
03795         prb1d(t7,7,&pos);
03796         tot = (float)(jtot)+pos;
03797         // Do not interpolate
03798         //tot = (float)(jtot);
03799 
03800         // mirrored
03801         fftr_d(t,ip);
03802 
03803         // find angle
03804         qm = -1.0e20;
03805         for (k=-psi_range; k<=psi_range; k++) {
03806                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03807                 if (t(j) >= qm) {
03808                         qm  = t(j);
03809                         jtot = j;
03810                 }
03811         }
03812 
03813         for (k=-psi_range; k<=psi_range; k++) {
03814                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03815                 if (t(j) >= qm) {
03816                         qm  = t(j);
03817                         jtot = j;
03818                 }
03819         }
03820 
03821         for (k=-3; k<=3; k++)  {
03822                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03823                 t7(k+4) = t(j);
03824         }
03825 
03826         // interpolate
03827 
03828         prb1d(t7,7,&pos);
03829         tmt = float(jtot) + pos;
03830         // Do not interpolate
03831         //tmt = float(jtot);
03832 
03833         free(t);
03834         free(q);
03835 
03836         Dict retvals;
03837         retvals["qn"] = qn;
03838         retvals["tot"] = tot;
03839         retvals["qm"] = qm;
03840         retvals["tmt"] = tmt;
03841         return retvals;
03842 }
03843 
03844 
03845 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03846 // flag 0 - straignt, 1 - mirror
03847 
03848         int nring = numr.size()/3;
03849         int maxrin = numr[numr.size()-1];
03850         double qn; float tot; double qm; float tmt;
03851         float *circ1 = circ1p->get_data();
03852         float *circ2 = circ2p->get_data();
03853 
03854         double *q, t7[7];
03855 
03856         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03857         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03858 
03859         qn  = 0.0f;
03860         qm  = 0.0f;
03861         tot = 0.0f;
03862         tmt = 0.0f;
03863 #ifdef _WIN32
03864         ip = -(int)(log((float)maxrin)/log(2.0f));
03865 #else
03866         ip = -(int)(log2(maxrin));
03867 #endif  //_WIN32
03868 
03869         //  c - straight  = circ1 * conjg(circ2)
03870         //  zero q array
03871 
03872         q = (double*)calloc(maxrin,sizeof(double));
03873 
03874    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03875         if (flag==0) {
03876                 for (i=1; i<=nring; i++) {
03877 
03878                         numr3i = numr(3,i);   // Number of samples of this ring
03879                         numr2i = numr(2,i);   // The beginning point of this ring
03880 
03881                         t1   = circ1(numr2i) * circ2(numr2i);
03882                         q(1) += t1;
03883 
03884                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03885                         if (numr3i == maxrin)  {
03886                                 q(2) += t1;
03887                         } else {
03888                                 q(numr3i+1) += t1;
03889                         }
03890 
03891                         for (j=3; j<=numr3i; j += 2) {
03892                                 jc     = j+numr2i-1;
03893 
03894         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03895         //                                ----- -----    ----- -----
03896         //                                 t1     t2      t3    t4
03897 
03898                                 c1     = circ1(jc);
03899                                 c2     = circ1(jc+1);
03900                                 d1     = circ2(jc);
03901                                 d2     = circ2(jc+1);
03902 
03903                                 t1     = c1 * d1;
03904                                 t3     = c1 * d2;
03905                                 t2     = c2 * d2;
03906                                 t4     = c2 * d1;
03907 
03908                                 q(j)   += t1 + t2;
03909                                 q(j+1) += -t3 + t4;
03910                         }
03911                 }
03912         } else {
03913                 for (i=1; i<=nring; i++) {
03914 
03915                         numr3i = numr(3,i);   // Number of samples of this ring
03916                         numr2i = numr(2,i);   // The beginning point of this ring
03917 
03918                         t1   = circ1(numr2i) * circ2(numr2i);
03919                         q(1) += t1;
03920 
03921                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03922                         if (numr3i == maxrin)  {
03923                                 q(2) += t1;
03924                         } else {
03925                                 q(numr3i+1) += t1;
03926                         }
03927 
03928                         for (j=3; j<=numr3i; j += 2) {
03929                                 jc     = j+numr2i-1;
03930 
03931         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03932         //                                ----- -----    ----- -----
03933         //                                 t1     t2      t3    t4
03934 
03935                                 c1     = circ1(jc);
03936                                 c2     = circ1(jc+1);
03937                                 d1     = circ2(jc);
03938                                 d2     = circ2(jc+1);
03939 
03940                                 t1     = c1 * d1;
03941                                 t3     = c1 * d2;
03942                                 t2     = c2 * d2;
03943                                 t4     = c2 * d1;
03944 
03945                                 q(j)   += t1 - t2;
03946                                 q(j+1) += -t3 - t4;
03947                         }
03948                 }
03949         }
03950         fftr_d(q,ip);
03951 
03952         qn  = -1.0e20;
03953         int psi_pos = int(psi/360.0*maxrin+0.5);
03954 
03955         for (k=-5; k<=5; k++) {
03956                 j = (psi_pos+maxrin-1)%maxrin+1;
03957                 if (q(j) >= qn) {
03958                         qn  = q(j);
03959                         jtot = j;
03960                 }
03961         }
03962 
03963         for (k=-3; k<=3; k++) {
03964                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03965                 t7(k+4) = q(j);
03966         }
03967 
03968         // interpolate
03969         prb1d(t7,7,&pos);
03970         tot = (float)(jtot)+pos;
03971         free(q);
03972 
03973         Dict retvals;
03974         retvals["qn"] = qn;
03975         retvals["tot"] = tot;
03976         return retvals;
03977 }
03978 
03979 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03980         int nring = numr.size()/3;
03981         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03982         int maxrin = numr[numr.size()-1];
03983         double qn; float tot;
03984         float *circ1 = circ1p->get_data();
03985         float *circ2 = circ2p->get_data();
03986 /*
03987 c
03988 c  checks only straight position
03989 c
03990 c  input - fourier transforms of rings!!
03991 c  circ1 already multiplied by weights!
03992 c
03993 */
03994 
03995         // dimension             circ1(lcirc),circ2(lcirc)
03996 
03997         // q(maxrin), t7(-3:3)  //maxrin+2 removed
03998         double *q, t7[7];
03999 
04000         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04001         float c1, c2, d1, d2, pos;
04002 
04003         qn  = 0.0;
04004         tot = 0.0;
04005 #ifdef _WIN32
04006         ip = -(int)(log((float)maxrin)/log(2.0f));
04007 #else
04008    ip = -(int)(log2(maxrin));
04009 #endif  //_WIN32
04010         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04011 
04012         //  c - straight  = circ1 * conjg(circ2)
04013         //  zero q array
04014 
04015         q = (double*)calloc(maxrin,sizeof(double));
04016 
04017                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04018         for (i=1; i<=nring; i++) {
04019 
04020                 numr3i = numr(3,i);   // Number of samples of this ring
04021                 numr2i = numr(2,i);   // The beginning point of this ring
04022 
04023                 q(1) += circ1(numr2i) * circ2(numr2i);
04024 
04025                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04026                 else  q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04027 
04028                 for (j=3; j<=numr3i; j += 2) {
04029                         jc     = j+numr2i-1;
04030 
04031 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04032 //                                ----- -----    ----- -----
04033 //                                 t1     t2      t3    t4
04034 
04035                         c1     = circ1(jc);
04036                         c2     = circ1(jc+1);
04037                         d1     = circ2(jc);
04038                         d2     = circ2(jc+1);
04039 
04040                         q(j)   += c1 * d1 + c2 * d2;
04041                         q(j+1) += -c1 * d2 + c2 * d1;
04042                 }
04043         }
04044 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04045         fftr_d(q,ip);
04046 
04047         qn  = -1.0e20;
04048         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04049                 if (q(j) >= qn) {
04050                         qn  = q(j);
04051                         jtot = j;
04052                 }
04053         }
04054 
04055         for (k=-3; k<=3; k++)  {
04056                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04057                 t7(k+4) = q(j);
04058         }
04059 
04060         // interpolate
04061         prb1d(t7,7,&pos);
04062         tot = (float)(jtot)+pos;
04063         // Do not interpolate
04064         //*tot = (float)(jtot);
04065 
04066         free(q);
04067 
04068         Dict retvals;
04069         retvals["qn"] = qn;
04070         retvals["tot"] = tot;
04071         return retvals;
04072 }
04073 
04074 #define  dout(i,j)        dout[i+maxrin*j]
04075 #define  circ1b(i)        circ1b[i-1]
04076 #define  circ2b(i)        circ2b[i-1]
04077 
04078 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04079 
04080    // dimension         circ1(lcirc),circ2(lcirc)
04081 
04082         int   ip, jc, numr3i, numr2i, i, j;
04083         float t1, t2, t3, t4, c1, c2, d1, d2;
04084 
04085         int nring = numr.size()/3;
04086         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04087         int maxrin = numr[numr.size()-1];
04088 
04089         float* circ1b = circ1->get_data();
04090         float* circ2b = circ2->get_data();
04091 
04092         // t(maxrin), q(maxrin)  // removed +2
04093         double *t, *q;
04094 
04095         q = (double*)calloc(maxrin,sizeof(double));
04096         t = (double*)calloc(maxrin,sizeof(double));
04097 
04098 #ifdef _WIN32
04099         ip = -(int)(log((float)maxrin)/log(2.0f));
04100 #else
04101         ip = -(int)(log2(maxrin));
04102 #endif  //_WIN32
04103 
04104         //  q - straight  = circ1 * conjg(circ2)
04105 
04106         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04107 
04108         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04109 
04110         for (i=1; i<=nring; i++) {
04111 
04112                 numr3i = numr(3,i);
04113                 numr2i = numr(2,i);
04114 
04115                 t1   = circ1b(numr2i) * circ2b(numr2i);
04116                 q(1) = q(1)+t1;
04117                 t(1) = t(1)+t1;
04118 
04119                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04120                 if (numr3i == maxrin)  {
04121                         q(2) += t1;
04122                         t(2) += t1;
04123                 } else {
04124                         q(numr3i+1) += t1;
04125                         t(numr3i+1) += t1;
04126                 }
04127 
04128                 for (j=3; j<=numr3i; j=j+2) {
04129                         jc     = j+numr2i-1;
04130 
04131                         c1     = circ1b(jc);
04132                         c2     = circ1b(jc+1);
04133                         d1     = circ2b(jc);
04134                         d2     = circ2b(jc+1);
04135 
04136                         t1     = c1 * d1;
04137                         t3     = c1 * d2;
04138                         t2     = c2 * d2;
04139                         t4     = c2 * d1;
04140 
04141                         q(j)   += t1 + t2;
04142                         q(j+1) += - t3 + t4;
04143                         t(j)   += t1 - t2;
04144                         t(j+1) += - t3 - t4;
04145                 }
04146         }
04147 
04148         // straight
04149         fftr_d(q,ip);
04150 
04151         // mirrored
04152         fftr_d(t,ip);
04153 
04154         EMData* out = new EMData();
04155         out->set_size(maxrin,2,1);
04156         float *dout = out->get_data();
04157         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04158         //out->set_size(maxrin,1,1);
04159         //float *dout = out->get_data();
04160         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04161         free(t);
04162         free(q);
04163         return out;
04164 }
04165 
04166 
04167 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04168 
04169         int maxrin = numr[numr.size()-1];
04170 
04171         vector<float> r(2*maxrin);
04172 
04173         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04174 
04175         return r;
04176 }
04177 
04178 #define  dout(i,j)        dout[i+maxrin*j]
04179 #define  circ1b(i)        circ1b[i-1]
04180 #define  circ2b(i)        circ2b[i-1]
04181 
04182 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04183 
04184    // dimension         circ1(lcirc),circ2(lcirc)
04185 
04186         int   ip, jc, numr3i, numr2i, i, j;
04187         float t1, t2, t3, t4, c1, c2, d1, d2;
04188 
04189         int nring = numr.size()/3;
04190         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04191         int maxrin = numr[numr.size()-1];
04192 
04193         float* circ1b = circ1->get_data();
04194         float* circ2b = circ2->get_data();
04195 
04196 #ifdef _WIN32
04197         ip = -(int)(log((float)maxrin)/log(2.0f));
04198 #else
04199         ip = -(int)(log2(maxrin));
04200 #endif  //_WIN32
04201         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04202 
04203         //  q - straight  = circ1 * conjg(circ2)
04204 
04205         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04206 
04207         for (i=1; i<=nring; i++) {
04208 
04209                 numr3i = numr(3,i);
04210                 numr2i = numr(2,i);
04211 
04212                 t1   = circ1b(numr2i) * circ2b(numr2i);
04213                 q(1) += t1;
04214                 t(1) += t1;
04215 
04216                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04217                 if (numr3i == maxrin)  {
04218                         q(2) += t1;
04219                         t(2) += t1;
04220                 } else {
04221                         q(numr3i+1) += t1;
04222                         t(numr3i+1) += t1;
04223                 }
04224 
04225                 for (j=3; j<=numr3i; j=j+2) {
04226                         jc     = j+numr2i-1;
04227 
04228                         c1     = circ1b(jc);
04229                         c2     = circ1b(jc+1);
04230                         d1     = circ2b(jc);
04231                         d2     = circ2b(jc+1);
04232 
04233                         t1     = c1 * d1;
04234                         t3     = c1 * d2;
04235                         t2     = c2 * d2;
04236                         t4     = c2 * d1;
04237 
04238                         q(j)   += t1 + t2;
04239                         q(j+1) += -t3 + t4;
04240                         t(j)   += t1 - t2;
04241                         t(j+1) += -t3 - t4;
04242                 }
04243         }
04244         // straight
04245         fftr_q(q,ip);
04246         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04247 
04248         // mirrored
04249         fftr_q(t,ip);
04250 }
04251 
04252 
04253 
04254 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04255 {
04256 
04257         int   ip, jc, numr3i, numr2i, i, j;
04258         float t1, t2, t3, t4, c1, c2, d1, d2;
04259 
04260         int nring = numr.size()/3;
04261         int maxrin = numr[numr.size()-1];
04262 
04263         float* circ1b = circ1->get_data();
04264         float* circ2b = circ2->get_data();
04265 
04266         double *q;
04267 
04268         q = (double*)calloc(maxrin,sizeof(double));
04269 
04270 #ifdef _WIN32
04271         ip = -(int)(log((float)maxrin)/log(2.0f));
04272 #else
04273         ip = -(int)(log2(maxrin));
04274 #endif  //_WIN32
04275 
04276          //  q - straight  = circ1 * conjg(circ2)
04277 
04278         for (i=1;i<=nring;i++) {
04279 
04280                 numr3i = numr(3,i);
04281                 numr2i = numr(2,i);
04282 
04283                 t1   = circ1b(numr2i) * circ2b(numr2i);
04284                 q(1) = q(1)+t1;
04285 
04286                 if (numr3i == maxrin)  {
04287                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04288                         q(2) = q(2)+t1;
04289                 } else {
04290                         t1              = circ1b(numr2i+1) * circ2b(numr2i+1);
04291                         q(numr3i+1) = q(numr3i+1)+t1;
04292                 }
04293 
04294                 for (j=3;j<=numr3i;j=j+2) {
04295                         jc     = j+numr2i-1;
04296 
04297                         c1     = circ1b(jc);
04298                         c2     = circ1b(jc+1);
04299                         d1     = circ2b(jc);
04300                         d2     = circ2b(jc+1);
04301 
04302                         t1     = c1 * d1;
04303                         t3     = c1 * d2;
04304                         t2     = c2 * d2;
04305                         t4     = c2 * d1;
04306 
04307                         q(j)   = q(j)   + t1 + t2;
04308                         q(j+1) = q(j+1) - t3 + t4;
04309                 }
04310         }
04311 
04312         // straight
04313         fftr_d(q,ip);
04314 
04315         EMData* out = new EMData();
04316         out->set_size(maxrin,1,1);
04317         float *dout = out->get_data();
04318         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04319         free(q);
04320         return out;
04321 
04322 }
04323 
04324 
04325 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04326 {
04327 
04328         int   ip, jc, numr3i, numr2i, i, j;
04329         float t1, t2, t3, t4, c1, c2, d1, d2;
04330 
04331         int nring = numr.size()/3;
04332         int maxrin = numr[numr.size()-1];
04333 
04334         float* circ1b = circ1->get_data();
04335         float* circ2b = circ2->get_data();
04336 
04337         double *t;
04338 
04339         t = (double*)calloc(maxrin,sizeof(double));
04340 
04341 #ifdef _WIN32
04342         ip = -(int)(log((float)maxrin)/log(2.0f));
04343 #else
04344         ip = -(int)(log2(maxrin));
04345 #endif  //_WIN32
04346 
04347          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04348 
04349         for (i=1;i<=nring;i++) {
04350 
04351                 numr3i = numr(3,i);
04352                 numr2i = numr(2,i);
04353 
04354                 t1   = circ1b(numr2i) * circ2b(numr2i);
04355                 t(1) = t(1)+t1;
04356 
04357                 if (numr3i == maxrin)  {
04358                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04359                         t(2) = t(2)+t1;
04360                 }
04361 
04362                 for (j=3;j<=numr3i;j=j+2) {
04363                         jc     = j+numr2i-1;
04364 
04365                         c1     = circ1b(jc);
04366                         c2     = circ1b(jc+1);
04367                         d1     = circ2b(jc);
04368                         d2     = circ2b(jc+1);
04369 
04370                         t1     = c1 * d1;
04371                         t3     = c1 * d2;
04372                         t2     = c2 * d2;
04373                         t4     = c2 * d1;
04374 
04375                         t(j)   = t(j)   + t1 - t2;
04376                         t(j+1) = t(j+1) - t3 - t4;
04377                 }
04378         }
04379 
04380         // mirrored
04381         fftr_d(t,ip);
04382 
04383         EMData* out = new EMData();
04384         out->set_size(maxrin,1,1);
04385         float *dout = out->get_data();
04386         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04387         free(t);
04388         return out;
04389 
04390 }
04391 
04392 #undef circ1b
04393 #undef circ2b
04394 #undef dout
04395 
04396 #undef  circ1
04397 #undef  circ2
04398 #undef  t
04399 #undef  q
04400 #undef  b
04401 #undef  t7
04402 
04403 
04404 #define    QUADPI                   3.141592653589793238462643383279502884197
04405 #define    PI2                      2*QUADPI
04406 
04407 float Util::ener(EMData* ave, vector<int> numr) {
04408         ENTERFUNC;
04409         long double ener,en;
04410 
04411         int nring = numr.size()/3;
04412         float *aveptr = ave->get_data();
04413 
04414         ener = 0.0;
04415         for (int i=1; i<=nring; i++) {
04416                 int numr3i = numr(3,i);
04417                 int np     = numr(2,i)-1;
04418                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04419                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04420                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04421                 ener += en/numr3i;
04422         }
04423         EXITFUNC;
04424         return static_cast<float>(ener);
04425 }
04426 
04427 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04428         ENTERFUNC;
04429         long double ener, en;
04430         float arg, cs, si;
04431 
04432         int nima = data.size();
04433         int nring = numr.size()/3;
04434         int maxrin = numr(3,nring);
04435 
04436         ener = 0.0;
04437         for (int i=1; i<=nring; i++) {
04438                 int numr3i = numr(3,i);
04439                 int np     = numr(2,i)-1;
04440                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04441                 float temp1 = 0.0, temp2 = 0.0;
04442                 for (int kk=0; kk<nima; kk++) {
04443                         float *ptr = data[kk]->get_data();
04444                         temp1 += ptr[np];
04445                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04446                 }
04447                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04448                 for (int j=2; j<numr3i; j+=2) {
04449                         float tempr = 0.0, tempi = 0.0;
04450                         for (int kk=0; kk<nima; kk++) {
04451                                 float *ptr = data[kk]->get_data();
04452                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04453                                 cs = cos(arg);
04454                                 si = sin(arg);
04455                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04456                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04457                         }
04458                         en += tq*(tempr*tempr+tempi*tempi);
04459                 }
04460                 ener += en/numr3i;
04461         }
04462         EXITFUNC;
04463         return static_cast<float>(ener);
04464 }
04465 
04466 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04467         int nring = numr.size()/3;
04468         float *ave = avep->get_data();
04469         float *dat = datp->get_data();
04470         int i, j, numr3i, np;
04471         float  arg, cs, si;
04472         int maxrin = numr(3,nring);
04473         if(mirror == 1) { //for mirrored data has to be conjugated
04474                 for (i=1; i<=nring; i++) {
04475                         numr3i = numr(3,i);
04476                         np     = numr(2,i)-1;
04477                         ave[np]   += dat[np];
04478                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04479                         for (j=2; j<numr3i; j=j+2) {
04480                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04481                                 cs = cos(arg);
04482                                 si = sin(arg);
04483                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04484                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04485                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04486                         }
04487                 }
04488         } else {
04489                 for (i=1; i<=nring; i++) {
04490                         numr3i = numr(3,i);
04491                         np     = numr(2,i)-1;
04492                         ave[np]   += dat[np];
04493                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04494                         for (j=2; j<numr3i; j=j+2) {
04495                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04496                                 cs = cos(arg);
04497                                 si = sin(arg);
04498                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04499                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04500                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04501                         }
04502                 }
04503         }
04504         avep->update();
04505         EXITFUNC;
04506 }
04507 
04508 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04509         int nring = numr.size()/3;
04510         float *ave = avep->get_data();
04511         float *dat = datp->get_data();
04512         int i, j, numr3i, np;
04513         float  arg, cs, si;
04514         int maxrin = numr(3,nring);
04515         if(mirror == 1) { //for mirrored data has to be conjugated
04516                 for (i=1; i<=nring; i++) {
04517                         numr3i = numr(3,i);
04518                         np     = numr(2,i)-1;
04519                         ave[np]   -= dat[np];
04520                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04521                         for (j=2; j<numr3i; j=j+2) {
04522                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04523                                 cs = cos(arg);
04524                                 si = sin(arg);
04525                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04526                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04527                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04528                         }
04529                 }
04530         } else {
04531                 for (i=1; i<=nring; i++) {
04532                         numr3i = numr(3,i);
04533                         np     = numr(2,i)-1;
04534                         ave[np]   -= dat[np];
04535                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04536                         for (j=2; j<numr3i; j=j+2) {
04537                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04538                                 cs = cos(arg);
04539                                 si = sin(arg);
04540                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04541                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04542                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04543                         }
04544                 }
04545         }
04546         avep->update();
04547         EXITFUNC;
04548 }
04549 
04550 
04551 #undef    QUADPI
04552 #undef    PI2
04553 
04554 #undef  numr
04555 #undef  circ
04556 
04557 
04558 #define QUADPI   3.141592653589793238462643383279502884197
04559 #define PI2      QUADPI*2
04560 #define deg_rad  QUADPI/180.0
04561 #define rad_deg  180.0/QUADPI
04562 
04563 struct ori_t
04564 {
04565     int iphi;
04566     int itht;
04567     int id;
04568 };
04569 
04570 
04571 struct cmpang
04572 {
04573     bool operator()( const ori_t& a, const ori_t& b )
04574     {
04575         if( a.itht != b.itht )
04576         {
04577             return a.itht < b.itht;
04578         }
04579 
04580         return a.iphi < b.iphi;
04581     }
04582 };
04583 
04584 
04585 vector<double> Util::cml_weights(const vector<float>& cml){
04586         static const int NBIN = 100;
04587         int nline=cml.size()/2;
04588         vector<double> weights(nline);
04589 
04590         vector<ori_t> angs(nline);
04591         for( int i=0; i < nline; ++i ) {
04592                 angs[i].iphi = int( NBIN*cml[2*i] );
04593                 angs[i].itht = int( NBIN*cml[2*i+1] );
04594                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04595                 angs[i].id = i;
04596         }
04597 
04598         //std::cout << "# of angs: " << angs.size() << std::endl;
04599 
04600         std::sort( angs.begin(), angs.end(), cmpang() );
04601 
04602         vector<float> newphi;
04603         vector<float> newtht;
04604         vector< vector<int> > indices;
04605 
04606         int curt_iphi = -1;
04607         int curt_itht = -1;
04608         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04609                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04610                         Assert( indices.size() > 0 );
04611                         indices.back().push_back(angs[i].id);
04612                 } else {
04613                         curt_iphi = angs[i].iphi;
04614                         curt_itht = angs[i].itht;
04615 
04616                         newphi.push_back( float(curt_iphi)/NBIN );
04617                         newtht.push_back( float(curt_itht)/NBIN );
04618                         indices.push_back( vector<int>(1,angs[i].id) );
04619                 }
04620         }
04621 
04622         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04623 
04624 
04625         int num_agl = newphi.size();
04626 
04627         if(num_agl>2) {
04628                 vector<double> w=Util::vrdg(newphi, newtht);
04629 
04630                 Assert( w.size()==newphi.size() );
04631                 Assert( indices.size()==newphi.size() );
04632 
04633                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04634                     /*
04635                     std::cout << "phi,tht,w,n: ";
04636                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04637                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04638                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04639                     std::cout << indices[i].size() << "(";
04640                     */
04641 
04642                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04643                             int id = indices[i][j];
04644                             weights[id] = w[i]/indices[i].size();
04645                             //std::cout << id << " ";
04646                     }
04647 
04648                     //std::cout << ")" << std::endl;
04649 
04650                 }
04651         } else {
04652                 cout<<"warning in Util.cml_weights"<<endl;
04653                 double val = PI2/float(nline);
04654                 for(int i=0; i<nline; i++)  weights[i]=val;
04655         }
04656 
04657         return weights;
04658 
04659 }
04660 
04661 /****************************************************
04662  * New code for common-lines
04663  ****************************************************/
04664 
04665 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04666 {
04667         int i;
04668         int nx=img->get_xsize();
04669         float *img_ptr  = img->get_data();
04670         float *line_ptr = line->get_data();
04671         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04672         img->update();
04673 }
04674 
04675 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04676     int j;
04677     int nx = sino->get_xsize();
04678     int i = nx * pos_line;
04679     float r1, r2;
04680     float *line_ptr = line->get_data();
04681     float *sino_ptr = sino->get_data();
04682     for (j=ilf;j<=ihf; j += 2) {
04683         r1 = line_ptr[j];
04684         r2 = line_ptr[j + 1];
04685         sino_ptr[i + j - ilf] = r1;
04686         sino_ptr[i + j - ilf + 1] = r2;
04687         sino_ptr[i + nx * nblines + j - ilf] = r1;
04688         sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04689     }
04690     sino->update();
04691 }
04692 
04693 vector<double> Util::cml_init_rot(vector<float> Ori){
04694     int nb_ori = Ori.size() / 4;
04695     int i, ind;
04696     float ph, th, ps;
04697     double cph, cth, cps, sph, sth, sps;
04698     vector<double> Rot(nb_ori*9);
04699     for (i=0; i<nb_ori; ++i){
04700         ind = i*4;
04701         // spider convention phi=psi-90, psi=phi+90
04702         ph = Ori[ind+2]-90;
04703         th = Ori[ind+1];
04704         ps = Ori[ind]+90;
04705         ph *= deg_rad;
04706         th *= deg_rad;
04707         ps *= deg_rad;
04708         // pre-calculate some trigo stuffs
04709         cph = cos(ph);
04710         cth = cos(th);
04711         cps = cos(ps);
04712         sph = sin(ph);
04713         sth = sin(th);
04714         sps = sin(ps);
04715         // fill rotation matrix
04716         ind = i*9;
04717         Rot[ind] = cph*cps-cth*sps*sph;
04718         Rot[ind+1] = cph*sps+cth*cps*sph;
04719         Rot[ind+2] = sth*sph;
04720         Rot[ind+3] = -sph*cps-cth*sps*cph;
04721         Rot[ind+4] = -sph*sps+cth*cps*cph;
04722         Rot[ind+5] = sth*cph;
04723         Rot[ind+6] = sth*sps;
04724         Rot[ind+7] = -sth*cps;
04725         Rot[ind+8] = cth;
04726     }
04727 
04728     return Rot;
04729 }
04730 
04731 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04732     float ph, ps;
04733     double cph, cth, cps, sph, sth, sps;
04734     int ind = iprj*9;
04735     // spider convention phi=psi-90, psi=phi+90
04736     ph = nps-90;
04737     ps = nph+90;
04738     ph *= deg_rad;
04739     th *= deg_rad;
04740     ps *= deg_rad;
04741     // pre-calculate some trigo stuffs
04742     cph = cos(ph);
04743     cth = cos(th);
04744     cps = cos(ps);
04745     sph = sin(ph);
04746     sth = sin(th);
04747     sps = sin(ps);
04748     // fill rotation matrix
04749     Rot[ind] = (float)(cph*cps-cth*sps*sph);
04750     Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04751     Rot[ind+2] = (float)(sth*sph);
04752     Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04753     Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04754     Rot[ind+5] = (float)(sth*cph);
04755     Rot[ind+6] = (float)(sth*sps);
04756     Rot[ind+7] = (float)(-sth*cps);
04757     Rot[ind+8] = (float)(cth);
04758 
04759     return Rot;
04760 }
04761 
04762 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04763     vector<int> com(2*(n_prj - 1));
04764     int a = i_prj*9;
04765     int i, b, c;
04766     int n1=0, n2=0;
04767     float vmax = 1 - 1.0e-6f;
04768     double r11, r12, r13, r23, r31, r32, r33;
04769 
04770     c = 0;
04771     for (i=0; i<n_prj; ++i){
04772         if (i!=i_prj){
04773             b = i*9;
04774             // this is equivalent to R = A*B'
04775             r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04776             r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04777             r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04778             r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04779             r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04780             r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04781             r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04782             if (r33 > vmax) {
04783                 n2 = 270;
04784                 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04785             }
04786             else if (r33 < -vmax) {
04787                 n2 = 270;
04788                 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04789             } else {
04790                 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04791                 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04792                 if (n1 < 0) {n1 += 360;}
04793                 if (n2 <= 0) {n2 = abs(n2);}
04794                 else {n2 = 360 - n2;}
04795             }
04796 
04797             if (n1 >= 360){n1 = n1 % 360;}
04798             if (n2 >= 360){n2 = n2 % 360;}
04799 
04800             // store common-lines
04801             b = c*2;
04802             com[b] = n1;
04803             com[b+1] = n2;
04804             ++c;
04805         }
04806     }
04807 
04808     return com;
04809 
04810 }
04811 
04812 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int n_prj, int n_lines) {
04813     vector<int> com(2*n_lines);
04814     int a=0, b, c, l;
04815     int n1=0, n2=0, mem=-1;
04816     float vmax = 1 - 1.0e-6f;
04817     double r11, r12, r13, r23, r31, r32, r33;
04818     c = 0;
04819     for (l=0; l<n_lines; ++l){
04820         c = 2*l;
04821         if (seq[c]!=mem){
04822             mem = seq[c];
04823             a = seq[c]*9;
04824         }
04825         b = seq[c+1]*9;
04826 
04827         // this is equivalent to R = A*B'
04828         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04829         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04830         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04831         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04832         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04833         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04834         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04835         if (r33 > vmax) {
04836             n2 = 270;
04837             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04838         }
04839         else if (r33 < -vmax) {
04840             n2 = 270;
04841             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04842         } else {
04843             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04844             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04845             if (n1 < 0) {n1 += 360;}
04846             if (n2 <= 0) {n2 = abs(n2);}
04847             else {n2 = 360 - n2;}
04848         }
04849         if (n1 >= 360){n1 = n1 % 360;}
04850         if (n2 >= 360){n2 = n2 % 360;}
04851 
04852         // store common-lines
04853         com[c] = n1;
04854         com[c+1] = n2;
04855     }
04856 
04857     return com;
04858 
04859 }
04860 
04861 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int nprj, int nlines){
04862     // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04863     vector<double> cml(2*nlines); // [phi, theta] / line
04864     float ph1, th1;
04865     float ph2, th2;
04866     double nx, ny, nz;
04867     double norm;
04868     double sth1=0, sph1=0, cth1=0, cph1=0;
04869     double sth2, sph2, cth2, cph2;
04870     int l, ind, c;
04871     int mem = -1;
04872     for (l=0; l<nlines; ++l){
04873         c = 2*l;
04874         if (seq[c]!=mem){
04875             mem = seq[c];
04876             ind = 4*seq[c];
04877             ph1 = Ori[ind]*deg_rad;
04878             th1 = Ori[ind+1]*deg_rad;
04879             sth1 = sin(th1);
04880             sph1 = sin(ph1);
04881             cth1 = cos(th1);
04882             cph1 = cos(ph1);
04883         }
04884         ind = 4*seq[c+1];
04885         ph2 = Ori[ind]*deg_rad;
04886         th2 = Ori[ind+1]*deg_rad;
04887         sth2 = sin(th2);
04888         cth2 = cos(th2);
04889         sph2 = sin(ph2);
04890         cph2 = cos(ph2);
04891         // cross product
04892         nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04893         ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04894         nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04895         norm = sqrt(nx*nx+ny*ny+nz*nz);
04896         nx /= norm;
04897         ny /= norm;
04898         nz /= norm;
04899         // apply mirror if need
04900         if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04901         // compute theta and phi
04902         cml[c+1] = acos(nz);
04903         if (cml[c+1] == 0) {cml[c] = 0;}
04904         else {
04905             cml[c+1] *= rad_deg;
04906             if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04907             cml[c] = rad_deg * atan2(nx, ny);
04908             cml[c] = fmod(360 + cml[c], 360);
04909 
04910         }
04911     }
04912 
04913     return cml;
04914 }
04915 
04916 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04917     double res = 0;
04918     double buf = 0;
04919     float* line_1;
04920     float* line_2;
04921     int i, n, ind;
04922     int lnlen = data[0]->get_xsize();
04923     for (n=0; n<n_lines; ++n) {
04924         ind = n*2;
04925         line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04926         line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04927         buf = 0;
04928         for (i=0; i<lnlen; ++i) {
04929             buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04930         }
04931         res += buf * weights[n];
04932     }
04933 
04934     return res;
04935 
04936 }
04937 
04938 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04939                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04940     // res: [best_disc, best_ipsi]
04941     // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04942     // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04943     vector<double> res(2);
04944     int lnlen = data[0]->get_xsize();
04945     int end = 2*(n_prj-1);
04946     double disc, buf, bdisc, tmp;
04947     int n, i, ipsi, ind, bipsi, c;
04948     float* line_1;
04949     float* line_2;
04950     bdisc = 1.0e6;
04951     bipsi = -1;
04952     // loop psi
04953     for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04954         // discrepancy
04955         disc = 0;
04956         c = 0;
04957         for (n=0; n<n_prj; ++n) {
04958             if(n!=iprj) {
04959                 ind = 2*c;
04960                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04961                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04962                 buf = 0;
04963                 for (i=0; i<lnlen; ++i) {
04964                     tmp = line_1[i]-line_2[i];
04965                     buf += tmp*tmp;
04966                 }
04967                 disc += buf * weights[iw[c]];
04968                 ++c;
04969             }
04970         }
04971         // select the best value
04972         if (disc <= bdisc) {
04973             bdisc = disc;
04974             bipsi = ipsi;
04975         }
04976         // update common-lines
04977         for (i=0; i<end; i+=2){
04978             com[i] += d_psi;
04979             if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04980         }
04981     }
04982     res[0] = bdisc;
04983     res[1] = float(bipsi);
04984 
04985     return res;
04986 }
04987 
04988 #undef  QUADPI
04989 #undef  PI2
04990 #undef  deg_rad
04991 #undef  rad_deg
04992 
04993 /****************************************************
04994  * END OF NEW CODE FOR COMMON-LINES
04995  ****************************************************/
04996 
04997 // helper function for k-means
04998 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04999         ENTERFUNC;
05000 
05001         int nima = data.size();
05002         vector<float> res(nima);
05003         double result = 0.;
05004         double valmin = 1.0e20;
05005         int valpos = -1;
05006 
05007         for (int kk=0; kk<nima; kk++){
05008         result = 0;
05009 
05010         float *y_data = data[kk]->get_data();
05011         float *x_data = image->get_data();
05012         long totsize = image->get_xsize()*image->get_ysize();
05013         for (long i = 0; i < totsize; i++) {
05014             double temp = x_data[i]- y_data[i];
05015             result += temp*temp;
05016         }
05017         result /= totsize;
05018         res[kk] = (float)result;
05019 
05020         if(result<valmin) {valmin = result; valpos = kk;}
05021 
05022         }
05023 
05024         Dict retvals;
05025         retvals["dist"] = res;
05026         retvals["pos"]  = valpos;
05027 
05028         EXITFUNC;
05029         return retvals;
05030 
05031 }
05032 
05033 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05034         ENTERFUNC;
05035 
05036         int nima = data.size();
05037         vector<float> res(nima);
05038         double result = 0.;
05039         double valmin = 1.0e20;
05040         int valpos = -1;
05041 
05042         for (int kk=0; kk<nima; kk++){
05043         result = 0;
05044         //validate_input_args(image, data[kk]);
05045 
05046         float *y_data = data[kk]->get_data();
05047         float *x_data = image->get_data();
05048 
05049         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05050         int nx  = data[kk]->get_xsize();
05051         int ny  = data[kk]->get_ysize();
05052         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05053         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05054 
05055         int ixb = 2*((nx+1)%2);
05056         int iyb = ny%2;
05057         int iz = 0;
05058 
05059         for ( int iy = 0; iy <= ny-1; iy++) {
05060             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05061                 int ii = ix + (iy  + iz * ny)* lsd2;
05062                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05063             }
05064         }
05065         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05066             int ii = (iy  + iz * ny)* lsd2;
05067             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05068             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05069         }
05070         if(nx%2 == 0) {
05071             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05072                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05073                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05074                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05075             }
05076 
05077         }
05078         result *= 2;
05079         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05080         if(ny%2 == 0) {
05081             int ii = (ny/2  + iz * ny)* lsd2;
05082             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05083         }
05084         if(nx%2 == 0) {
05085             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05086             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05087             if(ny%2 == 0) {
05088                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05089                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05090             }
05091         }
05092 
05093         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05094         res[kk] = (float)result;
05095 
05096         if(result<valmin) {valmin = result; valpos = kk;}
05097 
05098         }
05099 
05100         Dict retvals;
05101         retvals["dist"] = res;
05102         retvals["pos"]  = valpos;
05103 
05104         EXITFUNC;
05105         return retvals;
05106 }
05107 
05108 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05109     long int d2 = group2[s2 - 1] - group2[0];
05110     long int p2 = 0;
05111     long int i1 = 0;
05112     long int i2 = 0;
05113     long int max = 0;
05114     long int cont = 0;
05115     long int i = 0;
05116     int stop1 = 0;
05117     int stop2 = 0;
05118 
05119     for (i=0; i<s1; i++) {
05120         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05121         if (p2 >= s2) {p2 = s2 - 1;}
05122         i1 = p2;
05123         i2 = p2;
05124         max = s2;
05125         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05126 
05127         stop1 = 0;
05128         stop2 = 0;
05129         while (max--) {
05130             if (group1[i] == group2[i1]) {
05131                 if (flag) {stb[cont] = group1[i];}
05132                 cont++;
05133                 break;
05134             }
05135             if (group2[i1] < group1[i]) {stop1=1;}
05136             if (group1[i] == group2[i2]) {
05137                 if (flag) {stb[cont] = group1[i];}
05138                 cont++;
05139                 break;
05140             }
05141             if (group2[i2] > group1[i]) {stop2=1;}
05142             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05143 
05144             if (stop1 & stop2) {break;}
05145             i1--;
05146             i2++;
05147             if (i1 < 0) {i1 = 0;}
05148             if (i2 >= s2) {i2 = s2 - 1;}
05149         }
05150         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05151     }
05152 
05153     return cont;
05154 }
05155 
05156 
05157 
05158 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*nx]
05159 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*new_nx]
05160 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05161 {
05162         /* Exception Handle */
05163         if (!img) {
05164                 throw NullPointerException("NULL input image");
05165         }
05166         /* ============================== */
05167 
05168         // Get the size of the input image
05169         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05170         /* ============================== */
05171 
05172 
05173         /* Exception Handle */
05174         if ((x_step-1 > nx/2 || y_step-1 > ny/2 || z_step-1 > nz/2) || (x_step-1)<0 || (y_step-1)<0 || (z_step-1)<0)
05175         {
05176                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05177                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05178         }
05179         /* ============================== */
05180 
05181 
05182         /*    Calculation of the start point */
05183         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05184         /* ============================*/
05185 
05186 
05187         /* Calculation of the size of the decimated image */
05188         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05189         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05190         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05191         if(r1>1){r1=1;}
05192         if(r2>1){r2=1;}
05193         if(r3>1){r3=1;}
05194         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05195         /* ===========================================*/
05196 
05197 
05198         EMData* img2 = new EMData();
05199         img2->set_size(new_nx,new_ny,new_nz);
05200         float *new_ptr = img2->get_data();
05201         float *old_ptr = img->get_data();
05202         int iptr, jptr, kptr = 0;
05203         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05204                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05205                         for (int i=new_st_x; i<nx; i+=x_step) {
05206                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05207                         iptr++;}
05208                 jptr++;}
05209         kptr++;}
05210         img2->update();
05211         return img2;
05212 }
05213 #undef old_ptr
05214 #undef new_ptr
05215 
05216 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*nx]
05217 #define outp(i,j,k) outp[i+(j+(k*new_ny))*new_nx]
05218 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05219 {
05220         /* Exception Handle */
05221         if (!img) throw NullPointerException("NULL input image");
05222         /* ============================== */
05223 
05224         // Get the size of the input image
05225         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05226         /* ============================== */
05227 
05228         /* Exception Handle */
05229         if(new_nx>nx || new_ny>ny || new_nz>nz)
05230                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05231         if((nx/2)-(new_nx/2)+x_offset<0 || (ny/2)-(new_ny/2)+y_offset<0 || (nz/2)-(new_nz/2)+z_offset<0)
05232                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05233         if(x_offset>((nx-(nx/2))-(new_nx-(new_nx/2))) || y_offset>((ny-(ny/2))-(new_ny-(new_ny/2))) || z_offset>((nz-(nz/2))-(new_nz-(new_nz/2))))
05234                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05235         /* ============================== */
05236 
05237         /*    Calculation of the start point */
05238         int  new_st_x = nx/2-new_nx/2 + x_offset,
05239              new_st_y = ny/2-new_ny/2 + y_offset,
05240              new_st_z = nz/2-new_nz/2 + z_offset;
05241         /* ============================== */
05242 
05243         /* Exception Handle */
05244         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05245                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05246         /* ============================== */
05247 
05248         EMData* wind = img->copy_head();
05249         wind->set_size(new_nx, new_ny, new_nz);
05250         float *outp=wind->get_data();
05251         float *inp=img->get_data();
05252 
05253         for (int k=0; k<new_nz; k++)
05254                 for(int j=0; j<new_ny; j++)
05255                         for(int i=0; i<new_nx; i++)
05256                                 outp(i,j,k) = inp(i,j,k);
05257         wind->update();
05258         return wind;
05259 }
05260 #undef inp
05261 #undef outp
05262 
05263 #define inp(i,j,k) inp[i+(j+(k*ny))*nx]
05264 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*new_nx]
05265 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset,char *params)
05266 {
05267         /* Exception Handle */
05268         if (!img)  throw NullPointerException("NULL input image");
05269         /* ============================== */
05270 
05271         // Get the size of the input image
05272         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05273         /* ============================== */
05274 
05275         /* Exception Handle */
05276         if(new_nx<nx || new_ny<ny || new_nz<nz)
05277                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05278         if((new_nx/2)-(nx/2)+x_offset<0 || (new_ny/2)-(ny/2)+y_offset<0 || (new_nz/2)-(nz/2)+z_offset<0)
05279                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05280         if(x_offset>((new_nx-(new_nx/2))-(nx-(nx/2))) || y_offset>((new_ny-(new_ny/2))-(ny-(ny/2))) || z_offset>((new_nz-(new_nz/2))-(nz-(nz/2))))
05281                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05282         /* ============================== */
05283 
05284         EMData* pading = img->copy_head();
05285         pading->set_size(new_nx, new_ny, new_nz);
05286         float *inp  = img->get_data();
05287         float *outp = pading->get_data();
05288 
05289 
05290         /* Calculation of the average and the circumference values for background substitution
05291         =======================================================================================*/
05292         float background;
05293 
05294         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05295         else if (strcmp(params,"circumference")==0) {
05296                 float sum1=0.0f;
05297                 int cnt=0;
05298                 for(int i=0;i<nx;i++) {
05299                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05300                         cnt+=2;
05301                 }
05302                 if(nz-1 == 0) {
05303                         for (int j=1;j<ny-1;j++) {
05304                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05305                                 cnt+=2;
05306                         }
05307                 } else {
05308                         for (int k=1;k<nz-1;k++) {
05309                                 for (int j=1;j<ny-1;j++) {
05310                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05311                                         cnt+=2;
05312                                 }
05313                         }
05314                 }
05315                 background = sum1/cnt;
05316         } else {
05317                 background = static_cast<float>( atof( params ) );
05318         }
05319         /*=====================================================================================*/
05320 
05321          /*Initial Padding */
05322         int new_st_x=0,new_st_y=0,new_st_z=0;
05323         for (int k=0;k<new_nz;k++)
05324                 for(int j=0;j<new_ny;j++)
05325                         for (int i=0;i<new_nx;i++)
05326                                 outp(i,j,k)=background;
05327         /*============================== */
05328 
05329         /*    Calculation of the start point */
05330         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05331         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05332         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05333         /* ============================== */
05334 
05335         for (int k=0;k<nz;k++)
05336                 for(int j=0;j<ny;j++)
05337                         for(int i=0;i<nx;i++)
05338                                 outp(i,j,k)=inp(i,j,k);
05339         pading->update();
05340         return pading;
05341 }
05342 #undef inp
05343 #undef outp
05344 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05345 
05346 void Util::colreverse(float* beg, float* end, int nx) {
05347         float* tmp = new float[nx];
05348         int n = (end - beg)/nx;
05349         int nhalf = n/2;
05350         for (int i = 0; i < nhalf; i++) {
05351                 // swap col i and col n-1-i
05352                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05353                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05354                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05355         }
05356         delete[] tmp;
05357 }
05358 
05359 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05360 {
05361         int nxy = nx*ny;
05362         colreverse(beg, end, nxy);
05363 }
05364 
05365 
05366 void Util::cyclicshift(EMData *image, Dict params) {
05367 
05368         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05369 
05370         int dx = params["dx"];
05371         int dy = params["dy"];
05372         int dz = params["dz"];
05373 
05374         // The reverse trick we're using shifts to the left (a negative shift)
05375         int nx = image->get_xsize();
05376         dx %= nx;
05377         if (dx < 0) dx += nx;
05378         int ny = image->get_ysize();
05379         dy %= ny;
05380         if (dy < 0) dy += ny;
05381         int nz = image->get_zsize();
05382         dz %= nz;
05383         if (dz < 0) dz += nz;
05384 
05385         int mx = -(dx - nx);
05386         int my = -(dy - ny);
05387         int mz = -(dz - nz);
05388 
05389         float* data = image->get_data();
05390         // x-reverses
05391         if (mx != 0) {
05392                 for (int iz = 0; iz < nz; iz++)
05393                        for (int iy = 0; iy < ny; iy++) {
05394                                 // reverses for column iy
05395                                 int offset = nx*iy + nx*ny*iz; // starting location for column iy in slice iz
05396                                 reverse(&data[offset],&data[offset+mx]);
05397                                 reverse(&data[offset+mx],&data[offset+nx]);
05398                                 reverse(&data[offset],&data[offset+nx]);
05399                         }
05400         }
05401         // y-reverses
05402         if (my != 0) {
05403                 for (int iz = 0; iz < nz; iz++) {
05404                         int offset = nx*ny*iz;
05405                         colreverse(&data[offset], &data[offset + my*nx], nx);
05406                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05407                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05408                 }
05409         }
05410         if (mz != 0) {
05411                 slicereverse(&data[0], &data[mz*ny*nx], nx, ny);
05412                 slicereverse(&data[mz*ny*nx], &data[nz*ny*nx], nx, ny);
05413                 slicereverse(&data[0], &data[nz*ny*nx], nx ,ny);
05414         }
05415         image->update();
05416 }
05417 
05418 //-----------------------------------------------------------------------------------------------------------------------
05419 
05420 
05421 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05422 {
05423         if (image->is_complex())
05424                 throw ImageFormatException("Cannot do histogram on Fourier image");
05425         //float hmax, hmin;
05426         float *imageptr=0, *maskptr=0;
05427         int nx=image->get_xsize();
05428         int ny=image->get_ysize();
05429         int nz=image->get_zsize();
05430 
05431         if(mask != NULL){
05432                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05433                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05434                 maskptr =mask->get_data();
05435         }
05436         if( nbins == 0) nbins = nx;
05437         vector <float> freq(2*nbins, 0.0);
05438 
05439         imageptr=image->get_data();
05440         if( hmin == hmax ) {
05441                 if(mask == NULL) {
05442                         hmax = image->get_attr("maximum");
05443                         hmin = image->get_attr("minimum");
05444                 } else {
05445                         bool  First = true;
05446                         for (int i = 0;i < nx*ny*nz; i++) {
05447                         if (maskptr[i]>=0.5f) {
05448                                         if(First) {
05449                                                 hmax = imageptr[i];
05450                                                 hmin = imageptr[i];
05451                                                 First = false;
05452                                         } else {
05453                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05454                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05455                                         }
05456                                 }
05457                         }
05458                 }
05459         }
05460         float hdiff = hmax - hmin;
05461         float ff = (nbins-1)/hdiff;
05462         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05463         if(mask == NULL) {
05464                 for(int i = 0; i < nx*ny*nz; i++) {
05465                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05466                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05467                 }
05468         } else {
05469                 for(int i = 0; i < nx*ny*nz; i++) {
05470                         if(maskptr[i] >= 0.5) {
05471                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05472                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05473                         }
05474                 }
05475         }
05476         return freq;
05477 }
05478 
05479 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05480 {
05481         /* Exception Handle */
05482         if (img->is_complex() || ref->is_complex())
05483                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05484 
05485         if(mask != NULL){
05486                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05487                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05488         /* ===================================================== */
05489 
05490         /* Image size calculation */
05491         int size_ref = ((ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05492         int size_img = ((img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05493         /* ===================================================== */
05494 
05495         /* The reference image attributes */
05496         float *ref_ptr = ref->get_data();
05497         float ref_h_min = ref->get_attr("minimum");
05498         float ref_h_max = ref->get_attr("maximum");
05499         float ref_h_avg = ref->get_attr("mean");
05500         float ref_h_sig = ref->get_attr("sigma");
05501         /* ===================================================== */
05502 
05503         /* Input image under mask attributes */
05504         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05505 
05506         vector<float> img_data = Util::infomask(img, mask);
05507         float img_avg = img_data[0];
05508         float img_sig = img_data[1];
05509 
05510         /* The image under mask -- size calculation */
05511         int cnt=0;
05512         for(int i=0;i<size_img;i++)
05513                 if (mask_ptr[i]>0.5f)
05514                                 cnt++;
05515         /* ===================================================== */
05516 
05517         /* Histogram of reference image calculation */
05518         float ref_h_diff = ref_h_max - ref_h_min;
05519 
05520         #ifdef _WIN32
05521                 int hist_len = _MIN((int)size_ref/16,_MIN((int)size_img/16,256));
05522         #else
05523                 int hist_len = std::min((int)size_ref/16,std::min((int)size_img/16,256));
05524         #endif  //_WIN32
05525 
05526         float *ref_freq_bin = new float[3*hist_len];
05527 
05528         //initialize value in each bin to zero
05529         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05530 
05531         for (int i = 0;i < size_ref;i++) {
05532                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05533                 ref_freq_bin[L]++;
05534         }
05535         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05536 
05537         //Parameters Calculation (i.e) 'A' x + 'B'
05538         float A = ref_h_sig/img_sig;
05539         float B = ref_h_avg - (A*img_avg);
05540 
05541         vector<float> args;
05542         args.push_back(A);
05543         args.push_back(B);
05544 
05545         vector<float> scale;
05546         scale.push_back(1.e-7f*A);
05547         scale.push_back(-1.e-7f*B);
05548 
05549         vector<float> ref_freq_hist;
05550         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05551 
05552         vector<float> data;
05553         data.push_back(ref_h_diff);
05554         data.push_back(ref_h_min);
05555 
05556         Dict parameter;
05557 
05558         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05559         parameter["args"] = args;
05560         parameter["scale"]= scale;
05561         parameter["data"] = data;
05562         parameter["ref_freq_bin"] = ref_freq_hist;
05563         parameter["size_img"]=size_img;
05564         parameter["hist_len"]=hist_len;
05565         /* ===================================================== */
05566 
05567         return parameter;
05568 }
05569 
05570 
05571 float Util::hist_comp_freq(float PA,float PB,int size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05572 {
05573         float *img_ptr = img->get_data();
05574         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05575 
05576         int *img_freq_bin = new int[3*hist_len];
05577         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05578         for(int i = 0;i < size_img;i++) {
05579                 if(mask_ptr[i] > 0.5f) {
05580                         float img_xn = img_ptr[i]*PA + PB;
05581                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05582                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05583                 }
05584         }
05585         int freq_hist = 0;
05586 
05587         for(int i = 0;i < (3*hist_len);i++) freq_hist += (int)pow((float)((int)ref_freq_hist[i] - (int)img_freq_bin[i]),2.f);
05588         freq_hist = (-freq_hist);
05589         return static_cast<float>(freq_hist);
05590 }
05591 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05592 #define    QUADPI                       3.141592653589793238462643383279502884197
05593 #define    DGR_TO_RAD                   QUADPI/180
05594 #define    DM(I)                        DM          [I-1]
05595 #define    SS(I)                        SS          [I-1]
05596 Dict Util::CANG(float PHI,float THETA,float PSI)
05597 {
05598         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05599         vector<float>   DM,SS;
05600 
05601         for(int i =0;i<9;i++) DM.push_back(0);
05602 
05603         for(int i =0;i<6;i++) SS.push_back(0);
05604 
05605         CPHI = cos(double(PHI)*DGR_TO_RAD);
05606         SPHI = sin(double(PHI)*DGR_TO_RAD);
05607         CTHE = cos(double(THETA)*DGR_TO_RAD);
05608         STHE = sin(double(THETA)*DGR_TO_RAD);
05609         CPSI = cos(double(PSI)*DGR_TO_RAD);
05610         SPSI = sin(double(PSI)*DGR_TO_RAD);
05611 
05612         SS(1) = float(CPHI);
05613         SS(2) = float(SPHI);
05614         SS(3) = float(CTHE);
05615         SS(4) = float(STHE);
05616         SS(5) = float(CPSI);
05617         SS(6) = float(SPSI);
05618 
05619         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05620         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05621         DM(3) = float(-STHE*CPSI);
05622         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05623         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05624         DM(6) = float(STHE*SPSI);
05625         DM(7) = float(STHE*CPHI);
05626         DM(8) = float(STHE*SPHI);
05627         DM(9) = float(CTHE);
05628 
05629         Dict DMnSS;
05630         DMnSS["DM"] = DM;
05631         DMnSS["SS"] = SS;
05632 
05633         return(DMnSS);
05634 }
05635 #undef SS
05636 #undef DM
05637 #undef QUADPI
05638 #undef DGR_TO_RAD
05639 //-----------------------------------------------------------------------------------------------------------------------
05640 #define    DM(I)                        DM[I-1]
05641 #define    B(i,j)                       Bptr[i-1+((j-1)*NSAM)]
05642 #define    CUBE(i,j,k)                  CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*NX3D]
05643 
05644 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05645 {
05646 
05647         float  *Bptr = B->get_data();
05648         float  *CUBEptr = CUBE->get_data();
05649 
05650         int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05651         float DIPX,DIPY,XB,YB,XBB,YBB;
05652 
05653         Transform * t = B->get_attr("xform.projection");
05654         Dict d = t->get_params("spider");
05655         if(t) {delete t; t=0;}
05656         //  Unsure about sign of shifts, check later PAP 06/28/09
05657         float x_shift = d[ "tx" ];
05658         float y_shift = d[ "ty" ];
05659         x_shift = -x_shift;
05660         y_shift = -y_shift;
05661 
05662         NSAM = B->get_xsize();
05663         NROW = B->get_ysize();
05664         NX3D = CUBE->get_xsize();
05665         NY3D = CUBE->get_ysize();
05666         NZC  = CUBE->get_zsize();
05667 
05668 
05669         LDPX   = NX3D/2 +1;
05670         LDPY   = NY3D/2 +1;
05671         LDPZ   = NZC/2 +1;
05672         LDPNMX = NSAM/2 +1;
05673         LDPNMY = NROW/2 +1;
05674         NZ1    = 1;
05675 
05676         for(int K=1;K<=NZC;K++) {
05677                 KZ=K-1+NZ1;
05678                 for(int J=1;J<=NY3D;J++) {
05679                         XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05680                         YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05681                         for(int I=1;I<=NX3D;I++) {
05682                                 XB  = (I-1)*DM(1)+XBB-x_shift;
05683                                 IQX = int(XB+float(LDPNMX));
05684                                 if (IQX <1 || IQX >= NSAM) continue;
05685                                 YB  = (I-1)*DM(4)+YBB-y_shift;
05686                                 IQY = int(YB+float(LDPNMY));
05687                                 if (IQY<1 || IQY>=NROW)  continue;
05688                                 DIPX = XB+LDPNMX-IQX;
05689                                 DIPY = YB+LDPNMY-IQY;
05690 
05691                                 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05692                         }
05693                 }
05694         }
05695 }
05696 
05697 #undef DM
05698 #undef B
05699 #undef CUBE
05700 
05701 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05702 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05703 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05704 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05705 
05706 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K,vector<float> exptable)
05707 {
05708         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05709         float WW,OX,OY,Y;
05710 
05711         NSAM = PROJ->get_xsize();
05712         NROW = PROJ->get_ysize();
05713         NNNN = NSAM+2-(NSAM%2);
05714         NR2  = NROW/2;
05715 
05716         NANG = int(SS.size())/6;
05717 
05718         EMData* W = new EMData();
05719         int Wnx = NNNN/2;
05720         W->set_size(Wnx,NROW,1);
05721         W->to_zero();
05722         float *Wptr = W->get_data();
05723         float *PROJptr = PROJ->get_data();
05724         float indcnst = 1000/2.0;
05725         // we create look-up table for 1001 uniformly distributed samples [0,2];
05726 
05727         for (L=1; L<=NANG; L++) {
05728                 OX = SS(6,K)*SS(4,L)*(-SS(1,L)*SS(2,K)+ SS(1,K)*SS(2,L)) + SS(5,K)*(-SS(3,L)*SS(4,K)+SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)));
05729                 OY = SS(5,K)*SS(4,L)*(-SS(1,L)*SS(2,K)+ SS(1,K)*SS(2,L)) - SS(6,K)*(-SS(3,L)*SS(4,K)+SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)));
05730 
05731                 if(OX != 0.0f || OY!=0.0f) {
05732                         //int count = 0;
05733                         for(int J=1;J<=NROW;J++) {
05734                                 JY = (J-1);
05735                                 if(JY > NR2) JY=JY-NROW;
05736                                 for(int I=1;I<=NNNN/2;I++) {
05737                                         Y =  fabs(OX * (I-1) + OY * JY);
05738                                         if(Y < 2.0f) W(I,J) += exptable[int(Y*indcnst)];//exp(-4*Y*Y);//
05739                                     //if(Y < 2.0f) Wptr[count++] += exp(-4*Y*Y);//exptable[int(Y*indcnst)];//
05740                                 }
05741                         }
05742                 } else {
05743                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05744                 }
05745         }
05746 
05747         EMData* proj_in = PROJ;
05748 
05749         PROJ = PROJ->norm_pad( false, 2);
05750         PROJ->do_fft_inplace();
05751         PROJ->update();
05752         PROJptr = PROJ->get_data();
05753 
05754         float WNRMinv,temp;
05755         float osnr = 1.0f/SNR;
05756         WNRMinv = 1/W(1,1);
05757         for(int J=1;J<=NROW;J++)
05758                 for(int I=1;I<=NNNN;I+=2) {
05759                         KX          = (I+1)/2;
05760                         temp    = W(KX,J)*WNRMinv;
05761                         WW      = temp/(temp*temp + osnr);
05762                         PROJ(I,J)       *= WW;
05763                         PROJ(I+1,J) *= WW;
05764                 }
05765         delete W; W = 0;
05766         PROJ->do_ift_inplace();
05767         PROJ->depad();
05768 
05769         float* data_src = PROJ->get_data();
05770         float* data_dst = proj_in->get_data();
05771 
05772         int ntotal = NSAM*NROW;
05773         for( int i=0; i < ntotal; ++i )
05774         {
05775             data_dst[i] = data_src[i];
05776         }
05777 
05778         proj_in->update();
05779 
05780         delete PROJ;
05781 }
05782 
05783 #undef PROJ
05784 #undef W
05785 #undef SS
05786 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05787 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05788 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05789 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05790 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
05791 #define    CC(i)                        CC          [i-1]
05792 #define    CP(i)                        CP          [i-1]
05793 #define    VP(i)                        VP          [i-1]
05794 #define    VV(i)                        VV          [i-1]
05795 #define    AMAX1(i,j)                   i>j?i:j
05796 #define    AMIN1(i,j)                   i<j?i:j
05797 
05798 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05799 {
05800         float rad2deg =(180.0f/3.1415926f);
05801         float deg2rad = (3.1415926f/180.0f);
05802 
05803         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05804 
05805         NSAM = PROJ->get_xsize();
05806         NROW = PROJ->get_ysize();
05807         NNNN = NSAM+2-(NSAM%2);
05808         NR2  = NROW/2;
05809         NANG = int(SS.size())/6;
05810 
05811         float RI[9];
05812         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05813         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05814         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05815         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05816         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05817         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05818         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05819         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05820         RI(3,3)=SS(3,NUMP);
05821 
05822         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05823 
05824         EMData* W = new EMData();
05825         int Wnx = NNNN/2;
05826         W->set_size(NNNN/2,NROW,1);
05827         W->to_one();
05828         float *Wptr = W->get_data();
05829 
05830         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05831 
05832         for (L=1; L<=NANG; L++) {
05833                 if (L != NUMP) {
05834                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05835                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05836                         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);
05837 
05838                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
05839                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05840                         ALPHA=rad2deg*float(asin(CCN));
05841                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05842                         if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05843                         if(ALPHA<1.0E-6) {
05844                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05845                         } else {
05846                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05847                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
05848                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05849                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05850                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05851                                 CP(1)   = 0.0;CP(2) = 0.0;
05852                                 VP(1)   = 0.0;VP(2) = 0.0;
05853 
05854                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05855                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05856                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05857                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05858 
05859                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05860 
05861                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
05862                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
05863                                 float tmpinv = 1.0f/TMP;
05864                                 for(int J=1;J<=NROW;J++) {
05865                                         JY = (J-1);
05866                                         if (JY>NR2)  JY=JY-NROW;
05867                                         for(int I=1;I<=NNNN/2;I++) {
05868                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05869                                                 RT     = 1.0f-FV/FM;
05870                                                 W(I,J) += ((RT>0.0f)*RT);
05871                                         }
05872                                 }
05873                         }
05874                 }
05875         }
05876 
05877         EMData* proj_in = PROJ;
05878 
05879         PROJ = PROJ->norm_pad( false, 2);
05880         PROJ->do_fft_inplace();
05881         PROJ->update();
05882         float *PROJptr = PROJ->get_data();
05883 
05884         int KX;
05885         float WW;
05886         for(int J=1; J<=NROW; J++)
05887                 for(int I=1; I<=NNNN; I+=2) {
05888                         KX          =  (I+1)/2;
05889                         WW          =  1.0f/W(KX,J);
05890                         PROJ(I,J)   = PROJ(I,J)*WW;
05891                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
05892                 }
05893         delete W; W = 0;
05894         PROJ->do_ift_inplace();
05895         PROJ->depad();
05896 
05897         float* data_src = PROJ->get_data();
05898         float* data_dst = proj_in->get_data();
05899 
05900         int ntotal = NSAM*NROW;
05901         for( int i=0; i < ntotal; ++i )
05902         {
05903             data_dst[i] = data_src[i];
05904         }
05905 
05906         proj_in->update();
05907         delete PROJ;
05908 }
05909 
05910 #undef   AMAX1
05911 #undef   AMIN1
05912 #undef   RI
05913 #undef   CC
05914 #undef   CP
05915 #undef   VV
05916 #undef   VP
05917 
05918 
05919 #undef   W
05920 #undef   SS
05921 #undef   PROJ
05922 
05923 //-----------------------------------------------------------------------------------------------------------------------
05924 Dict Util::ExpMinus4YSqr(float ymax,int nsamples)
05925 {
05926         //exp(-16) is 1.0E-7 approximately)
05927         vector<float> expvect;
05928 
05929         double inc = double(ymax)/nsamples;
05930         double temp;
05931         for(int i =0;i<nsamples;i++) {
05932                 temp = exp((-4*(i*inc)*(i*inc)));
05933                 expvect.push_back(float(temp));
05934         }
05935         expvect.push_back(0.0);
05936         Dict lookupdict;
05937         lookupdict["table"]    = expvect;
05938         lookupdict["ymax"]     = ymax;
05939         lookupdict["nsamples"] = nsamples;
05940 
05941         return lookupdict;
05942 }
05943 
05944 //-------------------------------------------------------------------------------------------------------------------------
05945 
05946 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
05947 {
05948         float cst  = cs*1.0e7f;
05949 
05950         wgh /= 100.0;
05951         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
05952         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
05953         float ak2 = ak*ak;
05954         float g1 = dzz*1.0e4f*lambda*ak2;
05955         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
05956 
05957         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
05958         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
05959 
05960         return ctfv;
05961 }
05962 
05963 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
05964 {
05965         /***********
05966         ***get the size of the image for validation purpose
05967         **************/
05968         int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();  //Aren't  these  implied?  Please check and let me know, PAP.
05969         /********
05970         ***Exception Handle
05971         *************/
05972         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05973                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
05974 
05975         int i, size = nx*ny*nz;
05976 
05977         float* img_ptr = image->get_data();
05978         float* mask_ptr = mask->get_data();
05979 
05980         int ln=0;  //length of the output image = number of points under the mask.
05981         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
05982 
05983         EMData* new_image = new EMData();
05984         new_image->set_size(ln,1,1); /* set size of the new image */
05985         float *new_ptr    = new_image->get_data();
05986 
05987         ln=-1;
05988         for(i = 0;i < size;i++){
05989                 if(mask_ptr[i] > 0.5f) {
05990                         ln++;
05991                         new_ptr[ln]=img_ptr[i];
05992                 }
05993         }
05994 
05995         return new_image;
05996 }
05997 
05998 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
05999 {
06000         /********
06001         ***Exception Handle
06002         *************/
06003         if(mask == NULL)
06004                 throw ImageDimensionException("The mask cannot be an null image");
06005 
06006         /***********
06007         ***get the size of the mask
06008         **************/
06009         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06010 
06011         int i,size = nx*ny*nz;                   /* loop counters */
06012         /* new image declaration */
06013         EMData *new_image = new EMData();
06014         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06015         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06016         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06017         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06018         int count = 0;
06019         float sum_under_mask = 0.0 ;
06020         for(i = 0;i < size;i++){
06021                         if(mask_ptr[i] > 0.5f){
06022                                 new_ptr[i] = img_ptr[count];
06023                                 sum_under_mask += img_ptr[count];
06024                                 count++;
06025                                 if( count > image->get_xsize() ) {
06026                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06027                                 }
06028                         }
06029         }
06030 
06031         if( count > image->get_xsize() ) {
06032             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06033         }
06034 
06035         float avg_under_mask = sum_under_mask / count;
06036         for(i = 0;i < size;i++) {
06037                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06038         }
06039         new_image->update();
06040         return new_image;
06041 }
06042 
06043 
06044 
06045 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06046 {
06047         vector<float>new_peak;
06048         int n1=peak1.size()/3;
06049         float p_size2=p_size*p_size;
06050         for (int i=0;i<n1;++i) {
06051                 vector<float>::iterator it2= peak1.begin()+3*i;
06052                 bool push_back1=true;
06053                 int n2=peak2.size()/3;
06054                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06055                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06056                 if(n2 ==0) {
06057                         new_peak.push_back(*it2);
06058                         new_peak.push_back(*(it2+1));
06059                         new_peak.push_back(*(it2+2));
06060                 } else  {
06061                         int j=0;
06062                         while (j< n2-1 ) {
06063                                 vector<float>::iterator it3= peak2.begin()+3*j;
06064                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06065                                 if(d2< p_size2 ) {
06066                                         if( (*it2)<(*it3) ) {
06067                                                 new_peak.push_back(*it3);
06068                                                 new_peak.push_back(*(it3+1));
06069                                                 new_peak.push_back(*(it3+2));
06070                                                 peak2.erase(it3);
06071                                                 peak2.erase(it3);
06072                                                 peak2.erase(it3);
06073                                                 push_back1=false;
06074                                         } else {
06075                                                 peak2.erase(it3);
06076                                                 peak2.erase(it3);
06077                                                 peak2.erase(it3);
06078                                         }
06079                                 } else  j=j+1;
06080                                 n2=peak2.size()/3;
06081                         }
06082                         if(push_back1) {
06083                                 new_peak.push_back(*it2);
06084                                 new_peak.push_back(*(it2+1));
06085                                 new_peak.push_back(*(it2+2));
06086                         }
06087                 }
06088         }
06089         return new_peak;
06090 }
06091 
06092 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06093 {
06094         // n size of the covariance/correlation matrix
06095         // covmat --- covariance/correlation matrix (n by n)
06096         // eigval --- returns eigenvalues
06097         // eigvec --- returns eigenvectors
06098 
06099         ENTERFUNC;
06100 
06101         int i;
06102 
06103         // make a copy of covmat so that it will not be overwritten
06104         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06105 
06106         char NEEDV = 'V';
06107         char UPLO = 'U';
06108         int lwork = -1;
06109         int info = 0;
06110         float *work, wsize;
06111 
06112         //  query to get optimal workspace
06113         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06114         lwork = (int)wsize;
06115 
06116         work = (float *)calloc(lwork, sizeof(float));
06117         //  calculate eigs
06118         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06119         free(work);
06120         EXITFUNC;
06121         return info;
06122 }
06123 
06124 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06125 {
06126 
06127         ENTERFUNC;
06128         int len = covmatpy.size();
06129         float *eigvec;
06130         float *eigval;
06131         float *covmat;
06132         int status = 0;
06133         eigval = (float*)calloc(ncov,sizeof(float));
06134         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06135         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06136 
06137         const float *covmat_ptr;
06138         covmat_ptr = &covmatpy[0];
06139         for(int i=0;i<len;i++){
06140             covmat[i] = covmat_ptr[i];
06141         }
06142 
06143         status = Util::coveig(ncov, covmat, eigval, eigvec);
06144 
06145         vector<float> eigval_py(ncov);
06146         const float *eigval_ptr;
06147         eigval_ptr = &eigval[0];
06148         for(int i=0;i<ncov;i++){
06149             eigval_py[i] = eigval_ptr[i];
06150         }
06151 
06152         vector<float> eigvec_py(ncov*ncov);
06153         const float *eigvec_ptr;
06154         eigvec_ptr = &eigvec[0];
06155         for(int i=0;i<ncov*ncov;i++){
06156             eigvec_py[i] = eigvec_ptr[i];
06157         }
06158 
06159         Dict res;
06160         res["eigval"] = eigval_py;
06161         res["eigvec"] = eigvec_py;
06162 
06163         EXITFUNC;
06164         return res;
06165 }
06166 
06167 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06168 {
06169         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06170 
06171         k=(int)pw.size();
06172         l=0;
06173         m=k;
06174         n2=n+2;
06175         n1=n+1;
06176         klmd=k+l+m;
06177         klm2d= k+l+m+2;
06178         nklmd=k+l+m+n;
06179         n2d=n+2;
06180         /*size has to be increased when N is large*/
06181         n_larg=klmd*2;
06182         klm2d=n_larg+klm2d;
06183         klmd=n_larg+klmd;
06184         nklmd=n_larg+nklmd;
06185         int size_q=klm2d*n2d;
06186         int size_cu=nklmd*2;
06187         static int i__;
06188 
06189          double *q ;
06190          double *x ;
06191          double *res;
06192          double *cu;
06193          float *q2;
06194          float *pw_;
06195          long int *iu;
06196          double *s;
06197          q = (double*)calloc(size_q,sizeof(double));
06198          x = (double*)calloc(n2d,sizeof(double));
06199          res = (double*)calloc(klmd,sizeof(double));
06200          cu =(double*)calloc(size_cu,sizeof(double));
06201          s = (double*)calloc(klmd,sizeof(double));
06202          q2 = (float*)calloc(size_q,sizeof(float));
06203          iu = (long int*)calloc(size_cu,sizeof(long int));
06204          pw_ = (float*)calloc(k,sizeof(float));
06205 
06206         for( i__ =0;i__<k;++i__)
06207                 {
06208                 pw_[i__]=log(pw[i__]); }
06209         long int l_k=k;
06210         long int l_n=n;
06211         long int l_iswi=iswi;
06212         vector<float> cl1_res;
06213         cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06214         free(q);
06215         free(x);
06216         free(res);
06217         free(s);
06218         free(cu);
06219         free(q2);
06220         free(iu);
06221         free(pw_);
06222         return cl1_res;
06223 }
06224 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)
06225 {
06226     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06227     float r__1;
06228     int tmp__i;
06229     long int i__, j;
06230     --s;
06231     --res;
06232     iu -= 3;
06233     cu -= 3;
06234     --x;
06235     long int klm2d;
06236     klm2d= *k+*k+2;
06237     klm2d=klm2d+klm2d;
06238     q_dim1 = klm2d;
06239     q_offset = 1 + q_dim1;
06240     q -= q_offset;
06241     q2_dim1 = klm2d;
06242     q2_offset = 1 + q2_dim1;
06243     q2 -= q2_offset;
06244     i__2=0;
06245     i__1 = *n - 1;
06246     tmp__i=0;
06247     for (j = 1; j <= i__1; ++j) {
06248         i__2 = *k;
06249         tmp__i+=1;
06250         for (i__ = 1; i__ <= i__2; ++i__) {
06251             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06252             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06253         }
06254     }
06255     for  (i__ = 1; i__ <= i__2; ++i__)
06256       { q2[i__ + *n * q2_dim1] = 1.f;
06257             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06258         }
06259    vector<float> fit_res;
06260    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06261    return fit_res;
06262 }
06263 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)
06264 {
06265     /* System generated locals */
06266     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06267 
06268     /* Local variables */
06269     long int i__, j, m, n1, ii, jj;
06270     double tmp;
06271     vector<float> p;
06272     --x;
06273     q_dim1 = *klm2d;
06274     q_offset = 1 + q_dim1;
06275     q -= q_offset;
06276     q1_dim1 = *klm2d;
06277     q1_offset = 1 + q1_dim1;
06278     q1 -= q1_offset;
06279     --s;
06280     --res;
06281     iu -= 3;
06282     cu -= 3;
06283 
06284     /* Function Body */
06285     long int l = 0;
06286 
06287 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06288     m = *ks;
06289     n1 = *n + 1;
06290     if (*iswi == 1) {
06291         i__1 = n1;
06292         for (jj = 1; jj <= i__1; ++jj) {
06293             i__2 = *ks;
06294             for (ii = 1; ii <= i__2; ++ii) {
06295         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06296 
06297                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06298                         ;
06299             }
06300         }
06301     } else if (*iswi == 2) {
06302         i__1 = *ks;
06303         for (ii = 1; ii <= i__1; ++ii) {
06304             i__2 = n1;
06305             for (jj = 1; jj <= i__2; ++jj) {
06306                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06307                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06308             }
06309         }
06310     } else if (*iswi == 3) {
06311         l = 2;
06312         i__1 = n1;
06313         for (jj = 1; jj <= i__1; ++jj) {
06314             i__2 = *ks + 2;
06315             for (ii = 1; ii <= i__2; ++ii) {
06316                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06317             }
06318             i__2 = *ks;
06319             for (ii = 1; ii <= i__2; ++ii) {
06320                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06321             }
06322         }
06323     } else if (*iswi == 4) {
06324         l = 2;
06325         i__1 = n1;
06326         for (jj = 1; jj <= i__1; ++jj) {
06327             i__2 = *ks + 2;
06328             for (ii = 1; ii <= i__2; ++ii) {
06329                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06330             }
06331             i__2 = *ks;
06332             for (ii = 1; ii <= i__2; ++ii) {
06333                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06334             }
06335         }
06336     } else if (*iswi == 5) {
06337         l = 1;
06338         i__1 = n1;
06339         for (jj = 1; jj <= i__1; ++jj) {
06340             i__2 = *ks + 1;
06341             for (ii = 1; ii <= i__2; ++ii) {
06342                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06343             }
06344             i__2 = *ks;
06345             for (ii = 1; ii <= i__2; ++ii) {
06346                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06347             }
06348         }
06349     } else if (*iswi == 6) {
06350         l = 1;
06351         i__1 = n1;
06352         for (jj = 1; jj <= i__1; ++jj) {
06353             i__2 = *ks + 1;
06354             for (ii = 1; ii <= i__2; ++ii) {
06355                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06356             }
06357             i__2 = *ks;
06358             for (ii = 1; ii <= i__2; ++ii) {
06359                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06360             }
06361         }
06362     } else if (*iswi == 7) {
06363         l = 3;
06364         i__1 = n1;
06365         for (jj = 1; jj <= i__1; ++jj) {
06366             i__2 = *ks + 3;
06367             for (ii = 1; ii <= i__2; ++ii) {
06368                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06369             }
06370             i__2 = *ks;
06371             for (ii = 1; ii <= i__2; ++ii) {
06372                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06373             }
06374         }
06375     } else if (*iswi == 8) {
06376         l = 4;
06377         i__1 = n1;
06378         for (jj = 1; jj <= i__1; ++jj) {
06379             i__2 = *ks + 4;
06380             for (ii = 1; ii <= i__2; ++ii) {
06381                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06382             }
06383             i__2 = *ks;
06384             for (ii = 1; ii <= i__2; ++ii) {
06385                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06386             }
06387         }
06388     }
06389 
06390     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06391     i__1 = *ks;
06392     int tmp__j=0;
06393     for (i__ = 1; i__ <= i__1; ++i__) {
06394         tmp = 0.f;
06395         i__2 = *n - 1;
06396         for (j = 1; j <= i__2; ++j) {
06397         tmp__j=j;
06398             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06399         }
06400         tmp += x[*n];
06401         p.push_back(static_cast<float>(exp(tmp)));
06402         p.push_back(q1[i__ + q1_dim1]);
06403     }
06404     i__2=*n;
06405     for (i__=1;i__<=i__2;++i__)
06406         { p.push_back(static_cast<float>(x[i__]));}
06407     return p;
06408 }
06409 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06410         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06411 {
06412 
06413     long int q_dim1, q_offset, i__1, i__2;
06414     double d__1;
06415 
06416     static long int i__, j;
06417     static double z__;
06418     static long int n1, n2, ia, ii, kk, in, nk, js;
06419     static double sn, zu, zv;
06420     static long int nk1, klm, nkl, jmn, jpn;
06421     static double cuv;
06422     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06423     static float xmin;
06424     static double xmax;
06425     static long int iout;
06426     static double xsum;
06427     static long int iineg, maxit;
06428     static double toler;
06429     static float error;
06430     static double pivot;
06431     static long int kforce, iphase;
06432     static double tpivot;
06433 
06434     --s;
06435     --res;
06436     iu -= 3;
06437     cu -= 3;
06438     --x;
06439     q_dim1 = *klm2d;
06440     q_offset = 1 + q_dim1;
06441     q -= q_offset;
06442 
06443     /* Function Body */
06444     maxit = 500;
06445     kode = 0;
06446     toler = 1e-4f;
06447     iter = 0;
06448     n1 = *n + 1;
06449     n2 = *n + 2;
06450     nk = *n + *k;
06451     nk1 = nk + 1;
06452     nkl = nk + *l;
06453     nkl1 = nkl + 1;
06454     klm = *k + *l + *m;
06455     klm1 = klm + 1;
06456     klm2 = klm + 2;
06457     nklm = *n + klm;
06458     kforce = 1;
06459     iter = 0;
06460     js = 1;
06461     ia = 0;
06462 /* SET UP LABELS IN Q. */
06463     i__1 = *n;
06464     for (j = 1; j <= i__1; ++j) {
06465         q[klm2 + j * q_dim1] = (double) j;
06466 /* L10: */
06467     }
06468     i__1 = klm;
06469     for (i__ = 1; i__ <= i__1; ++i__) {
06470         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06471         if (q[i__ + n1 * q_dim1] >= 0.f) {
06472             goto L30;
06473         }
06474         i__2 = n2;
06475         for (j = 1; j <= i__2; ++j) {
06476             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06477 /* L20: */
06478         }
06479 L30:
06480         ;
06481     }
06482 /* SET UP PHASE 1 COSTS. */
06483     iphase = 2;
06484     i__1 = nklm;
06485     for (j = 1; j <= i__1; ++j) {
06486         cu[(j << 1) + 1] = 0.f;
06487         cu[(j << 1) + 2] = 0.f;
06488         iu[(j << 1) + 1] = 0;
06489         iu[(j << 1) + 2] = 0;
06490 /* L40: */
06491     }
06492     if (*l == 0) {
06493         goto L60;
06494     }
06495     i__1 = nkl;
06496     for (j = nk1; j <= i__1; ++j) {
06497         cu[(j << 1) + 1] = 1.f;
06498         cu[(j << 1) + 2] = 1.f;
06499         iu[(j << 1) + 1] = 1;
06500         iu[(j << 1) + 2] = 1;
06501 /* L50: */
06502     }
06503     iphase = 1;
06504 L60:
06505     if (*m == 0) {
06506         goto L80;
06507     }
06508     i__1 = nklm;
06509     for (j = nkl1; j <= i__1; ++j) {
06510         cu[(j << 1) + 2] = 1.f;
06511         iu[(j << 1) + 2] = 1;
06512         jmn = j - *n;
06513         if (q[jmn + n2 * q_dim1] < 0.f) {
06514             iphase = 1;
06515         }
06516 /* L70: */
06517     }
06518 L80:
06519     if (kode == 0) {
06520         goto L150;
06521     }
06522     i__1 = *n;
06523     for (j = 1; j <= i__1; ++j) {
06524         if ((d__1 = x[j]) < 0.) {
06525             goto L90;
06526         } else if (d__1 == 0) {
06527             goto L110;
06528         } else {
06529             goto L100;
06530         }
06531 L90:
06532         cu[(j << 1) + 1] = 1.f;
06533         iu[(j << 1) + 1] = 1;
06534         goto L110;
06535 L100:
06536         cu[(j << 1) + 2] = 1.f;
06537         iu[(j << 1) + 2] = 1;
06538 L110:
06539         ;
06540     }
06541     i__1 = *k;
06542     for (j = 1; j <= i__1; ++j) {
06543         jpn = j + *n;
06544         if ((d__1 = res[j]) < 0.) {
06545             goto L120;
06546         } else if (d__1 == 0) {
06547             goto L140;
06548         } else {
06549             goto L130;
06550         }
06551 L120:
06552         cu[(jpn << 1) + 1] = 1.f;
06553         iu[(jpn << 1) + 1] = 1;
06554         if (q[j + n2 * q_dim1] > 0.f) {
06555             iphase = 1;
06556         }
06557         goto L140;
06558 L130:
06559         cu[(jpn << 1) + 2] = 1.f;
06560         iu[(jpn << 1) + 2] = 1;
06561         if (q[j + n2 * q_dim1] < 0.f) {
06562             iphase = 1;
06563         }
06564 L140:
06565         ;
06566     }
06567 L150:
06568     if (iphase == 2) {
06569         goto L500;
06570     }
06571 /* COMPUTE THE MARGINAL COSTS. */
06572 L160:
06573     i__1 = n1;
06574     for (j = js; j <= i__1; ++j) {
06575         xsum = 0.;
06576         i__2 = klm;
06577         for (i__ = 1; i__ <= i__2; ++i__) {
06578             ii = (long int) q[i__ + n2 * q_dim1];
06579             if (ii < 0) {
06580                 goto L170;
06581             }
06582             z__ = cu[(ii << 1) + 1];
06583             goto L180;
06584 L170:
06585             iineg = -ii;
06586             z__ = cu[(iineg << 1) + 2];
06587 L180:
06588             xsum += q[i__ + j * q_dim1] * z__;
06589 /*  180       XSUM = XSUM + Q(I,J)*Z */
06590 /* L190: */
06591         }
06592         q[klm1 + j * q_dim1] = xsum;
06593 /* L200: */
06594     }
06595     i__1 = *n;
06596     for (j = js; j <= i__1; ++j) {
06597         ii = (long int) q[klm2 + j * q_dim1];
06598         if (ii < 0) {
06599             goto L210;
06600         }
06601         z__ = cu[(ii << 1) + 1];
06602         goto L220;
06603 L210:
06604         iineg = -ii;
06605         z__ = cu[(iineg << 1) + 2];
06606 L220:
06607         q[klm1 + j * q_dim1] -= z__;
06608 /* L230: */
06609     }
06610 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06611 L240:
06612     xmax = 0.f;
06613     if (js > *n) {
06614         goto L490;
06615     }
06616     i__1 = *n;
06617     for (j = js; j <= i__1; ++j) {
06618         zu = q[klm1 + j * q_dim1];
06619         ii = (long int) q[klm2 + j * q_dim1];
06620         if (ii > 0) {
06621             goto L250;
06622         }
06623         ii = -ii;
06624         zv = zu;
06625         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06626         goto L260;
06627 L250:
06628         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06629 L260:
06630         if (kforce == 1 && ii > *n) {
06631             goto L280;
06632         }
06633         if (iu[(ii << 1) + 1] == 1) {
06634             goto L270;
06635         }
06636         if (zu <= xmax) {
06637             goto L270;
06638         }
06639         xmax = zu;
06640         in = j;
06641 L270:
06642         if (iu[(ii << 1) + 2] == 1) {
06643             goto L280;
06644         }
06645         if (zv <= xmax) {
06646             goto L280;
06647         }
06648         xmax = zv;
06649         in = j;
06650 L280:
06651         ;
06652     }
06653     if (xmax <= toler) {
06654         goto L490;
06655     }
06656     if (q[klm1 + in * q_dim1] == xmax) {
06657         goto L300;
06658     }
06659     i__1 = klm2;
06660     for (i__ = 1; i__ <= i__1; ++i__) {
06661         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06662 /* L290: */
06663     }
06664     q[klm1 + in * q_dim1] = xmax;
06665 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06666 L300:
06667     if (iphase == 1 || ia == 0) {
06668         goto L330;
06669     }
06670     xmax = 0.f;
06671     i__1 = ia;
06672     for (i__ = 1; i__ <= i__1; ++i__) {
06673         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06674         if (z__ <= xmax) {
06675             goto L310;
06676         }
06677         xmax = z__;
06678         iout = i__;
06679 L310:
06680         ;
06681     }
06682     if (xmax <= toler) {
06683         goto L330;
06684     }
06685     i__1 = n2;
06686     for (j = 1; j <= i__1; ++j) {
06687         z__ = q[ia + j * q_dim1];
06688         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06689         q[iout + j * q_dim1] = z__;
06690 /* L320: */
06691     }
06692     iout = ia;
06693     --ia;
06694     pivot = q[iout + in * q_dim1];
06695     goto L420;
06696 L330:
06697     kk = 0;
06698     i__1 = klm;
06699     for (i__ = 1; i__ <= i__1; ++i__) {
06700         z__ = q[i__ + in * q_dim1];
06701         if (z__ <= toler) {
06702             goto L340;
06703         }
06704         ++kk;
06705         res[kk] = q[i__ + n1 * q_dim1] / z__;
06706         s[kk] = (double) i__;
06707 L340:
06708         ;
06709     }
06710 L350:
06711     if (kk > 0) {
06712         goto L360;
06713     }
06714     kode = 2;
06715     goto L590;
06716 L360:
06717     xmin = static_cast<float>( res[1] );
06718     iout = (long int) s[1];
06719     j = 1;
06720     if (kk == 1) {
06721         goto L380;
06722     }
06723     i__1 = kk;
06724     for (i__ = 2; i__ <= i__1; ++i__) {
06725         if (res[i__] >= xmin) {
06726             goto L370;
06727         }
06728         j = i__;
06729         xmin = static_cast<float>( res[i__] );
06730         iout = (long int) s[i__];
06731 L370:
06732         ;
06733     }
06734     res[j] = res[kk];
06735     s[j] = s[kk];
06736 L380:
06737     --kk;
06738     pivot = q[iout + in * q_dim1];
06739     ii = (long int) q[iout + n2 * q_dim1];
06740     if (iphase == 1) {
06741         goto L400;
06742     }
06743     if (ii < 0) {
06744         goto L390;
06745     }
06746     if (iu[(ii << 1) + 2] == 1) {
06747         goto L420;
06748     }
06749     goto L400;
06750 L390:
06751     iineg = -ii;
06752     if (iu[(iineg << 1) + 1] == 1) {
06753         goto L420;
06754     }
06755 /* 400 II = IABS(II) */
06756 L400:
06757     ii = abs(ii);
06758     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06759     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06760         goto L420;
06761     }
06762 /* BYPASS INTERMEDIATE VERTICES. */
06763     i__1 = n1;
06764     for (j = js; j <= i__1; ++j) {
06765         z__ = q[iout + j * q_dim1];
06766         q[klm1 + j * q_dim1] -= z__ * cuv;
06767         q[iout + j * q_dim1] = -z__;
06768 /* L410: */
06769     }
06770     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06771     goto L350;
06772 /* GAUSS-JORDAN ELIMINATION. */
06773 L420:
06774     if (iter < maxit) {
06775         goto L430;
06776     }
06777     kode = 3;
06778     goto L590;
06779 L430:
06780     ++iter;
06781     i__1 = n1;
06782     for (j = js; j <= i__1; ++j) {
06783         if (j != in) {
06784             q[iout + j * q_dim1] /= pivot;
06785         }
06786 /* L440: */
06787     }
06788 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
06789 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
06790 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
06791 /*     DO 460 J=JS,N1 */
06792 /*        IF(J .EQ. IN) GO TO 460 */
06793 /*        Z = -Q(IOUT,J) */
06794 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
06795 /* 460 CONTINUE */
06796     i__1 = n1;
06797     for (j = js; j <= i__1; ++j) {
06798         if (j == in) {
06799             goto L460;
06800         }
06801         z__ = -q[iout + j * q_dim1];
06802         i__2 = klm1;
06803         for (i__ = 1; i__ <= i__2; ++i__) {
06804             if (i__ != iout) {
06805                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06806             }
06807 /* L450: */
06808         }
06809 L460:
06810         ;
06811     }
06812     tpivot = -pivot;
06813     i__1 = klm1;
06814     for (i__ = 1; i__ <= i__1; ++i__) {
06815         if (i__ != iout) {
06816             q[i__ + in * q_dim1] /= tpivot;
06817         }
06818 /* L470: */
06819     }
06820     q[iout + in * q_dim1] = 1.f / pivot;
06821     z__ = q[iout + n2 * q_dim1];
06822     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06823     q[klm2 + in * q_dim1] = z__;
06824     ii = (long int) abs(z__);
06825     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06826         goto L240;
06827     }
06828     i__1 = klm2;
06829     for (i__ = 1; i__ <= i__1; ++i__) {
06830         z__ = q[i__ + in * q_dim1];
06831         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06832         q[i__ + js * q_dim1] = z__;
06833 /* L480: */
06834     }
06835     ++js;
06836     goto L240;
06837 /* TEST FOR OPTIMALITY. */
06838 L490:
06839     if (kforce == 0) {
06840         goto L580;
06841     }
06842     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06843         goto L500;
06844     }
06845     kforce = 0;
06846     goto L240;
06847 /* SET UP PHASE 2 COSTS. */
06848 L500:
06849     iphase = 2;
06850     i__1 = nklm;
06851     for (j = 1; j <= i__1; ++j) {
06852         cu[(j << 1) + 1] = 0.f;
06853         cu[(j << 1) + 2] = 0.f;
06854 /* L510: */
06855     }
06856     i__1 = nk;
06857     for (j = n1; j <= i__1; ++j) {
06858         cu[(j << 1) + 1] = 1.f;
06859         cu[(j << 1) + 2] = 1.f;
06860 /* L520: */
06861     }
06862     i__1 = klm;
06863     for (i__ = 1; i__ <= i__1; ++i__) {
06864         ii = (long int) q[i__ + n2 * q_dim1];
06865         if (ii > 0) {
06866             goto L530;
06867         }
06868         ii = -ii;
06869         if (iu[(ii << 1) + 2] == 0) {
06870             goto L560;
06871         }
06872         cu[(ii << 1) + 2] = 0.f;
06873         goto L540;
06874 L530:
06875         if (iu[(ii << 1) + 1] == 0) {
06876             goto L560;
06877         }
06878         cu[(ii << 1) + 1] = 0.f;
06879 L540:
06880         ++ia;
06881         i__2 = n2;
06882         for (j = 1; j <= i__2; ++j) {
06883             z__ = q[ia + j * q_dim1];
06884             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06885             q[i__ + j * q_dim1] = z__;
06886 /* L550: */
06887         }
06888 L560:
06889         ;
06890     }
06891     goto L160;
06892 L570:
06893     if (q[klm1 + n1 * q_dim1] <= toler) {
06894         goto L500;
06895     }
06896     kode = 1;
06897     goto L590;
06898 L580:
06899     if (iphase == 1) {
06900         goto L570;
06901     }
06902 /* PREPARE OUTPUT. */
06903     kode = 0;
06904 L590:
06905     xsum = 0.;
06906     i__1 = *n;
06907     for (j = 1; j <= i__1; ++j) {
06908         x[j] = 0.f;
06909 /* L600: */
06910     }
06911     i__1 = klm;
06912     for (i__ = 1; i__ <= i__1; ++i__) {
06913         res[i__] = 0.f;
06914 /* L610: */
06915     }
06916     i__1 = klm;
06917     for (i__ = 1; i__ <= i__1; ++i__) {
06918         ii = (long int) q[i__ + n2 * q_dim1];
06919         sn = 1.f;
06920         if (ii > 0) {
06921             goto L620;
06922         }
06923         ii = -ii;
06924         sn = -1.f;
06925 L620:
06926         if (ii > *n) {
06927             goto L630;
06928         }
06929         x[ii] = sn * q[i__ + n1 * q_dim1];
06930         goto L640;
06931 L630:
06932         iimn = ii - *n;
06933         res[iimn] = sn * q[i__ + n1 * q_dim1];
06934         if (ii >= n1 && ii <= nk) {
06935             xsum += q[i__ + n1 * q_dim1];
06936         }
06937 L640:
06938         ;
06939     }
06940     error = (float)xsum;
06941     return;
06942 }
06943 
06944 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
06945 {
06946         int j,d;
06947         EMData * e = new EMData();
06948         float *eptr, *imgptr;
06949         imgptr = img->get_data();
06950         float SSE = 0.f;
06951         for (j = 0 ; j < N ; j++) {
06952                 e->read_image(images,S[j]);
06953                 eptr = e->get_data();
06954                 for (d = 0; d < size; d++) {
06955                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
06956                 }
06957         delete e;
06958         return SSE;
06959 }
06960 
06961 
06962 #define         mymax(x,y)              (((x)>(y))?(x):(y))
06963 #define         mymin(x,y)              (((x)<(y))?(x):(y))
06964 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
06965 
06966 
06967 #define         quadpi                  3.141592653589793238462643383279502884197
06968 #define         dgr_to_rad              quadpi/180
06969 #define         deg_to_rad              quadpi/180
06970 #define         rad_to_deg              180/quadpi
06971 #define         rad_to_dgr              180/quadpi
06972 #define         TRUE                    1
06973 #define         FALSE                   0
06974 
06975 
06976 #define theta(i)                theta   [i-1]
06977 #define phi(i)                  phi     [i-1]
06978 #define weight(i)               weight  [i-1]
06979 #define lband(i)                lband   [i-1]
06980 #define ts(i)                   ts      [i-1]
06981 #define thetast(i)              thetast [i-1]
06982 #define key(i)                  key     [i-1]
06983 
06984 
06985 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
06986 {
06987 
06988         ENTERFUNC;
06989 
06990         if ( th.size() != ph.size() ) {
06991                 LOGERR("images not same size");
06992                 throw ImageFormatException( "images not same size");
06993         }
06994 
06995         // rand_seed
06996         srand(10);
06997 
06998         int i,*key;
06999         int len = th.size();
07000         double *theta,*phi,*weight;
07001         theta   =       (double*) calloc(len,sizeof(double));
07002         phi     =       (double*) calloc(len,sizeof(double));
07003         weight  =       (double*) calloc(len,sizeof(double));
07004         key     =       (int*) calloc(len,sizeof(int));
07005         const float *thptr, *phptr;
07006 
07007         thptr = &th[0];
07008         phptr = &ph[0];
07009         for(i=1;i<=len;i++){
07010                 key(i) = i;
07011                 weight(i) = 0.0;
07012         }
07013 
07014         for(i = 0;i<len;i++){
07015                 theta[i] = thptr[i];
07016                 phi[i]   = phptr[i];
07017         }
07018 
07019         //  sort by theta
07020         Util::hsortd(theta, phi, key, len, 1);
07021 
07022         //Util::voronoidiag(theta,phi, weight, len);
07023         Util::voronoi(phi, theta, weight, len);
07024 
07025         //sort by key
07026         Util::hsortd(weight, weight, key, len, 2);
07027 
07028         free(theta);
07029         free(phi);
07030         free(key);
07031         vector<double> wt;
07032         double count = 0;
07033         for(i=1; i<= len; i++)
07034         {
07035                 wt.push_back(weight(i));
07036                 count += weight(i);
07037         }
07038 
07039         //if( abs(count-6.28) > 0.1 )
07040         //{
07041         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07042         //}
07043 
07044         free(weight);
07045 
07046         EXITFUNC;
07047         return wt;
07048 
07049 }
07050 
07051 struct  tmpstruct{
07052         double theta1,phi1;
07053         int key1;
07054         };
07055 
07056 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07057 {
07058         ENTERFUNC;
07059         vector<tmpstruct> tmp(len);
07060         int i;
07061         for(i = 1;i<=len;i++)
07062         {
07063                 tmp[i-1].theta1 = theta(i);
07064                 tmp[i-1].phi1 = phi(i);
07065                 tmp[i-1].key1 = key(i);
07066         }
07067 
07068         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07069         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07070 
07071         for(i = 1;i<=len;i++)
07072         {
07073                 theta(i) = tmp[i-1].theta1;
07074                 phi(i)   = tmp[i-1].phi1;
07075                 key(i)   = tmp[i-1].key1;
07076         }
07077         EXITFUNC;
07078 }
07079 
07080 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07081 {
07082         return(tmp1.theta1 < tmp2.theta1);
07083 }
07084 
07085 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07086 {
07087         return(tmp1.key1 < tmp2.key1);
07088 }
07089 
07090 /******************  VORONOI DIAGRAM **********************************/
07091 /*
07092 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07093 {
07094         ENTERFUNC;
07095 
07096         int     *lband;
07097         double  aat=0.0f,*ts;
07098         double  aa,acum,area;
07099         int     last;
07100         int numth       =       1;
07101         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07102 
07103         int i,it,l,k;
07104         int nband,lb,low,medium,lhigh,lbw,lenw;
07105 
07106 
07107         lband   =       (int*)calloc(nbt,sizeof(int));
07108         ts      =       (double*)calloc(nbt,sizeof(double));
07109 
07110         if(lband == NULL || ts == NULL ){
07111                 fprintf(stderr,"memory allocation failure!\n");
07112                 exit(1);
07113         }
07114 
07115         nband=nbt;
07116 
07117         while(nband>0){
07118                 Util::angstep(ts,nband);
07119 
07120                 l=1;
07121                 for(i=1;i<=n;i++){
07122                         if(theta(i)>ts(l)){
07123                                 lband(l)=i;
07124                                 l=l+1;
07125                                 if(l>nband)  exit(1);
07126                         }
07127                 }
07128 
07129                 l=1;
07130                 for(i=1;i<=n;i++){
07131                         if(theta(i)>ts(l)){
07132                                 lband(l)=i;
07133                                 l=l+1;
07134                                 if(l>nband)  exit(1);
07135                         }
07136                 }
07137 
07138                 lband(l)=n+1;
07139                 acum=0.0;
07140                 for(it=l;it>=1;it-=numth){
07141                         for(i=it;i>=mymax(1,it-numth+1);i--){
07142                         if(i==l) last   =        TRUE;
07143                         else     last   =        FALSE;
07144 
07145                         if(l==1){
07146                                 lb=1;
07147                                 low=1;
07148                                 medium=n+1;
07149                                 lhigh=n-lb+1;
07150                                 lbw=1;
07151                         }
07152                         else if(i==1){
07153                                 lb=1;
07154                                 low=1;
07155                                 medium=lband(1);
07156                                 lhigh=lband(2)-1;
07157                                 lbw=1;
07158                         }
07159                         else if(i==l){
07160                                 if(l==2)        lb=1;
07161                                 else            lb=lband(l-2);
07162                                 low=lband(l-1)-lb+1;
07163                                 medium=lband(l)-lb+1;
07164                                 lhigh=n-lb+1;
07165                                 lbw=lband(i-1);
07166                         }
07167                         else{
07168                                 if(i==2)        lb=1;
07169                                 else            lb=lband(i-2);
07170                                 low=lband(i-1)-lb+1;
07171                                 medium=lband(i)-lb+1;
07172                                 lhigh=lband(i+1)-1-lb+1;
07173                                 lbw=lband(i-1);
07174                         }
07175                         lenw=medium-low;
07176 
07177 
07178                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07179 
07180 
07181                         if(nband>1){
07182                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07183                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07184 
07185                                 aa = 0.0;
07186                                 for(k = lbw;k<=lbw+lenw-1;k++)
07187                                         aa = aa+weight(k);
07188 
07189                                 acum=acum+aa;
07190                                 aat=aa/area;
07191                                 }
07192 
07193                         }
07194                         for(i=it;mymax(1,it-numth+1);i--){
07195                         if(fabs(aat-1.0)>0.02){
07196                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07197                                 goto  label2;
07198                                 }
07199                         }
07200                 acum=acum/quadpi/2.0;
07201                 exit(1);
07202 label2:
07203 
07204                 continue;
07205                 }
07206 
07207         free(ts);
07208         free(lband);
07209 
07210         }
07211 
07212         EXITFUNC;
07213 }
07214 
07215 
07216 void Util::angstep(double* thetast,int len){
07217 
07218         ENTERFUNC;
07219 
07220         double t1,t2,tmp;
07221         int i;
07222         if(len>1){
07223                 t1=0;
07224                 for(i=1;i<=len-1;i++){
07225                         tmp=cos(t1)-1.0/((float)len);
07226                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07227                         thetast(i)=t2 * rad_to_deg;
07228                         t1=t2;
07229                 }
07230         }
07231         thetast(len)=90.0;
07232 
07233         EXITFUNC;
07234 }
07235 */
07236 /*
07237 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07238 {
07239 
07240         ENTERFUNC;
07241         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07242         int nt6, n, ier,nout,lnew,mdup,nd;
07243         int i,k,mt,status;
07244 
07245 
07246         double *ds, *x, *y, *z;
07247         double tol=1.0e-8;
07248         double a;
07249 
07250         if(last){
07251                 if(medium>nt)  n = nt+nt;
07252                 else           n = nt+nt-medium+1;
07253         }
07254         else{
07255                 n=nt;
07256         }
07257 
07258         nt6 = n*6;
07259 
07260         list = (int*)calloc(nt6,sizeof(int));
07261         lptr = (int*)calloc(nt6,sizeof(int));
07262         lend = (int*)calloc(n  ,sizeof(int));
07263         iwk  = (int*)calloc(n  ,sizeof(int));
07264         good = (int*)calloc(n  ,sizeof(int));
07265         key  = (int*)calloc(n  ,sizeof(int));
07266         indx = (int*)calloc(n  ,sizeof(int));
07267         lcnt = (int*)calloc(n  ,sizeof(int));
07268 
07269         ds      =       (double*) calloc(n,sizeof(double));
07270         x       =       (double*) calloc(n,sizeof(double));
07271         y       =       (double*) calloc(n,sizeof(double));
07272         z       =       (double*) calloc(n,sizeof(double));
07273 
07274         if (list == NULL ||
07275         lptr == NULL ||
07276         lend == NULL ||
07277         iwk  == NULL ||
07278         good == NULL ||
07279         key  == NULL ||
07280         indx == NULL ||
07281         lcnt == NULL ||
07282         x    == NULL ||
07283         y    == NULL ||
07284         z    == NULL ||
07285         ds   == NULL) {
07286                 printf("memory allocation failure!\n");
07287                 exit(1);
07288         }
07289 
07290 
07291 
07292         for(i = 1;i<=nt;i++){
07293                 x[i-1] = theta(i);
07294                 y[i-1] = phi(i);
07295         }
07296 
07297 
07298 
07299         if (last) {
07300                 for(i=nt+1;i<=n;i++){
07301                         x[i-1]=180.0-x[2*nt-i];
07302                         y[i-1]=180.0+y[2*nt-i];
07303                 }
07304         }
07305 
07306 
07307         Util::disorder2(x,y,key,n);
07308 
07309         Util::ang_to_xyz(x,y,z,n);
07310 
07311 
07312         //  Make sure that first three are no colinear
07313         label1:
07314         for(k=0; k<2; k++){
07315                 for(i=k+1; i<3; i++){
07316                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07317                                 Util::flip23(x, y, z, key, k, n);
07318                                 goto label1;
07319                         }
07320                 }
07321         }
07322 
07323 
07324         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07325 
07326 
07327         if (status != 0) {
07328                 printf(" error in trmsh3 \n");
07329                 exit(1);
07330         }
07331 
07332 
07333         mdup=n-nout;
07334         if (ier == -2) {
07335                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07336                 exit(1);
07337         }
07338         else if (ier > 0) {
07339                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07340                 exit(1);
07341         }
07342 
07343         nd=0;
07344         for (k=1;k<=n;k++){
07345                 if (indx[k-1]>0){
07346                         nd++;
07347                         good[nd-1]=k;
07348                 }
07349         }
07350 
07351 
07352         for(i = 1;i<=nout;i++) {
07353                 k=good[i-1];
07354                 if (key[k-1] >= low && key[k-1]<medium){
07355                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07356                         if (ier != 0){
07357                                 weight[key[k-1]-low] =-1.0;
07358                         }
07359                         else {
07360                                 weight[key[k-1]-low]=a/lcnt[i-1];
07361                         }
07362                 }
07363         }
07364 
07365 // Fill out the duplicated weights
07366         for(i = 1;i<=n;i++){
07367                 mt=-indx[i-1];
07368                 if (mt>0){
07369                         k=good[mt-1];
07370 //  This is a duplicated entry, get the already calculated
07371 //   weight and assign it.
07372                         if (key[i-1]>=low && key[i-1]<medium){
07373 //  Is it already calculated weight??
07374                                 if(key[k-1]>=low && key[k-1]<medium){
07375                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07376                                 }
07377                                 else{
07378 //  No, the weight is from the outside of valid region, calculate it anyway
07379                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07380                                         if (ier != 0){
07381                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07382                                                 weight[key[i-1]-low] =-1.0;
07383                                         }
07384                                         else {
07385                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07386                                         }
07387                                 }
07388                         }
07389                 }
07390         }
07391 
07392 
07393         free(list);
07394         free(lend);
07395         free(iwk);
07396         free(good);
07397         free(key);
07398 
07399         free(indx);
07400         free(lcnt);
07401         free(ds);
07402         free(x);
07403         free(y);
07404         free(z);
07405         EXITFUNC;
07406 }
07407 */
07408 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07409 {
07410 
07411         ENTERFUNC;
07412 
07413         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07414         int nt6, n, ier, nout, lnew, mdup, nd;
07415         int i,k,mt,status;
07416 
07417 
07418         double *ds, *x, *y, *z;
07419         double tol  = 1.0e-8;
07420         double dtol = 15;
07421         double a;
07422 
07423         /*if(last){
07424                 if(medium>nt)  n = nt+nt;
07425                 else           n = nt+nt-medium+1;
07426         }
07427         else{
07428                 n=nt;
07429         }*/
07430 
07431         n = nt + nt;
07432 
07433         nt6 = n*6;
07434 
07435         list = (int*)calloc(nt6,sizeof(int));
07436         lptr = (int*)calloc(nt6,sizeof(int));
07437         lend = (int*)calloc(n  ,sizeof(int));
07438         iwk  = (int*)calloc(n  ,sizeof(int));
07439         good = (int*)calloc(n  ,sizeof(int));
07440         key  = (int*)calloc(n  ,sizeof(int));
07441         indx = (int*)calloc(n  ,sizeof(int));
07442         lcnt = (int*)calloc(n  ,sizeof(int));
07443 
07444         ds      =       (double*) calloc(n,sizeof(double));
07445         x       =       (double*) calloc(n,sizeof(double));
07446         y       =       (double*) calloc(n,sizeof(double));
07447         z       =       (double*) calloc(n,sizeof(double));
07448 
07449         if (list == NULL ||
07450         lptr == NULL ||
07451         lend == NULL ||
07452         iwk  == NULL ||
07453         good == NULL ||
07454         key  == NULL ||
07455         indx == NULL ||
07456         lcnt == NULL ||
07457         x    == NULL ||
07458         y    == NULL ||
07459         z    == NULL ||
07460         ds   == NULL) {
07461                 printf("memory allocation failure!\n");
07462                 exit(1);
07463         }
07464 
07465         bool colinear=true;
07466         while(colinear)
07467         {
07468 
07469         L1:
07470             for(i = 0; i<nt; i++){
07471                 x[i] = theta[i];
07472                 y[i] = phi[i];
07473                 x[nt+i] = 180.0 - x[i];
07474                 y[nt+i] = 180.0 + y[i];
07475             }
07476 
07477             Util::disorder2(x, y, key, n);
07478 
07479             // check if the first three angles are not close, else shuffle
07480             double val;
07481             for(k=0; k<2; k++){
07482                 for(i=k+1; i<3; i++){
07483                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07484                     if( val  < dtol) {
07485                         goto L1;
07486                     }
07487                 }
07488             }
07489 
07490             Util::ang_to_xyz(x, y, z, n);
07491 
07492             //  Make sure that first three has no duplication
07493             bool dupnode=true;
07494             dupnode=true;
07495             while(dupnode)
07496             {
07497                 for(k=0; k<2; k++){
07498                     for(i=k+1; i<3; i++){
07499                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07500                                 Util::flip23(x, y, z, key, k, n);
07501                                 continue;
07502                         }
07503                     }
07504                 }
07505                 dupnode = false;
07506             }
07507 
07508 
07509             ier = 0;
07510 
07511             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07512 
07513             if (status != 0) {
07514                 printf(" error in trmsh3 \n");
07515                 exit(1);
07516             }
07517 
07518             if (ier > 0) {
07519                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07520                 exit(1);
07521             }
07522 
07523             mdup=n-nout;
07524             if (ier == -2) {
07525                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07526             }
07527             else
07528             {
07529                 colinear=false;
07530             }
07531         }
07532 
07533 
07534         Assert( ier != -2 );
07535 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07536 //  INDX contains node numbers from the squeezed list
07537         nd=0;
07538         for (k=1; k<=n; k++){
07539                 if (indx[k-1]>0) {
07540                         nd++;
07541                         good[nd-1]=k;
07542                 }
07543         }
07544 
07545 //
07546 // *** Compute the Voronoi region areas.
07547 //
07548         for(i = 1; i<=nout; i++) {
07549                 k=good[i-1];
07550                 //  We only need n weights from hemisphere
07551                 if (key[k-1] <= nt) {
07552 //  CALCULATE THE AREA
07553                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07554                         if (ier != 0){
07555 //  We set the weight to -1, this will signal the error in the calling
07556 //   program, as the area will turn out incorrect
07557                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07558                                 weight[key[k-1]-1] =-1.0;
07559                         } else {
07560 //  Assign the weight
07561                                 weight[key[k-1]-1]=a/lcnt[i-1];
07562                         }
07563                 }
07564         }
07565 
07566 
07567 // Fill out the duplicated weights
07568         for(i = 1; i<=n; i++){
07569                 mt =- indx[i-1];
07570                 if (mt>0){
07571                         k = good[mt-1];
07572 //  This is a duplicated entry, get the already calculated
07573 //   weight and assign it.
07574                 //  We only need n weights from hemisphere
07575                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07576                         }
07577         }
07578 
07579         free(list);
07580         free(lend);
07581         free(iwk);
07582         free(good);
07583         free(key);
07584         free(lptr);
07585         free(indx);
07586         free(lcnt);
07587         free(ds);
07588         free(x);
07589         free(y);
07590         free(z);
07591 
07592 
07593         EXITFUNC;
07594 }
07595 
07596 void Util::disorder2(double *x,double *y, int *key, int len)
07597 {
07598         ENTERFUNC;
07599         int k, i;
07600         for(i=0; i<len; i++) key[i]=i+1;
07601 
07602         for(i = 0; i<len;i++){
07603                 k = rand()%len;
07604                 std::swap(key[k], key[i]);
07605                 std::swap(x[k], x[i]);
07606                 std::swap(y[k], y[i]);
07607         }
07608         EXITFUNC;
07609 }
07610 
07611 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07612 {
07613         ENTERFUNC;
07614         double costheta,sintheta,cosphi,sinphi;
07615         for(int i = 0;  i<len;  i++)
07616         {
07617                 cosphi = cos(y[i]*dgr_to_rad);
07618                 sinphi = sin(y[i]*dgr_to_rad);
07619                 if(fabs(x[i]-90.0)< 1.0e-5){
07620                         x[i] = cosphi;
07621                         y[i] = sinphi;
07622                         z[i] = 0.0;
07623                 }
07624                 else{
07625                         costheta = cos(x[i]*dgr_to_rad);
07626                         sintheta = sin(x[i]*dgr_to_rad);
07627                         x[i] = cosphi*sintheta;
07628                         y[i] = sinphi*sintheta;
07629                         z[i] = costheta;
07630                 }
07631         }
07632         EXITFUNC;
07633 }
07634 
07635 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07636 {
07637         ENTERFUNC;
07638         int i = k;
07639         while( i == k )  i = rand()%len;
07640         std::swap(key[i], key[k]);
07641         std::swap(x[i], x[k]);
07642         std::swap(y[i], y[k]);
07643         std::swap(z[i], z[k]);
07644         EXITFUNC;
07645 }
07646 
07647 
07648 #undef  mymax
07649 #undef  mymin
07650 #undef  sign
07651 #undef  quadpi
07652 #undef  dgr_to_rad
07653 #undef  deg_to_rad
07654 #undef  rad_to_deg
07655 #undef  rad_to_dgr
07656 #undef  TRUE
07657 #undef  FALSE
07658 #undef  theta
07659 #undef  phi
07660 #undef  weight
07661 #undef  lband
07662 #undef  ts
07663 #undef  thetast
07664 #undef  key
07665 
07666 
07667 /*################################################################################################
07668 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07669 ######   You must link the resulting object file with the libraries: #############################
07670 ####################    -lf2c -lm   (in that order)   ############################################
07671 ################################################################################################*/
07672 
07673 /* Common Block Declarations */
07674 
07675 
07676 #define TRUE_ (1)
07677 #define FALSE_ (0)
07678 #define abs(x) ((x) >= 0 ? (x) : -(x))
07679 
07680 struct stcom_{
07681     double y;
07682 };
07683 stcom_ stcom_1;
07684 #ifdef KR_headers
07685 double floor();
07686 int i_dnnt(x) double *x;
07687 #else
07688 int i_dnnt(double *x)
07689 #endif
07690 {
07691         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07692 }
07693 
07694 
07695 
07696 
07697 /* ____________________STRID______________________________________ */
07698 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07699         double *y, double *z__, int *n, int *list, int *
07700         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07701         int *near__, int *next, double *dist, int *ier)
07702 {
07703     /* System generated locals */
07704     int i__1, i__2;
07705 
07706     /* Local variables */
07707     static double d__;
07708     static int i__, j;
07709     static double d1, d2, d3;
07710     static int i0, lp, kt, ku, lpl, nku;
07711     extern long int left_(double *, double *, double *, double
07712             *, double *, double *, double *, double *,
07713             double *);
07714     static int nexti;
07715     extern /* Subroutine */ int addnod_(int *, int *, double *,
07716             double *, double *, int *, int *, int *,
07717             int *, int *);
07718 
07719 
07720 /* *********************************************************** */
07721 
07722 /*                                              From STRIPACK */
07723 /*                                            Robert J. Renka */
07724 /*                                  Dept. of Computer Science */
07725 /*                                       Univ. of North Texas */
07726 /*                                           renka@cs.unt.edu */
07727 /*                                                   01/20/03 */
07728 
07729 /*   This is an alternative to TRMESH with the inclusion of */
07730 /* an efficient means of removing duplicate or nearly dupli- */
07731 /* cate nodes. */
07732 
07733 /*   This subroutine creates a Delaunay triangulation of a */
07734 /* set of N arbitrarily distributed points, referred to as */
07735 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07736 /* routine TRMESH for definitions and a list of additional */
07737 /* subroutines.  This routine is an alternative to TRMESH */
07738 /* with the inclusion of an efficient means of removing dup- */
07739 /* licate or nearly duplicate nodes. */
07740 
07741 /*   The algorithm has expected time complexity O(N*log(N)) */
07742 /* for random nodal distributions. */
07743 
07744 
07745 /* On input: */
07746 
07747 /*       N0 = Number of nodes, possibly including duplicates. */
07748 /*            N0 .GE. 3. */
07749 
07750 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07751 /*             bound on the deviation from 1 of the cosine of */
07752 /*             the angle between the nodes.  Note that */
07753 /*             |1-cos(A)| is approximately A*A/2. */
07754 
07755 /* The above parameters are not altered by this routine. */
07756 
07757 /*       X,Y,Z = Arrays of length at least N0 containing the */
07758 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07759 /*               Z(K)) is referred to as node K, and K is re- */
07760 /*               ferred to as a nodal index.  It is required */
07761 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
07762 /*               K.  The first three nodes must not be col- */
07763 /*               linear (lie on a common great circle). */
07764 
07765 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
07766 
07767 /*       LEND = Array of length at least N0. */
07768 
07769 /*       INDX = Array of length at least N0. */
07770 
07771 /*       LCNT = Array of length at least N0 (length N is */
07772 /*              sufficient). */
07773 
07774 /*       NEAR,NEXT,DIST = Work space arrays of length at */
07775 /*                        least N0.  The space is used to */
07776 /*                        efficiently determine the nearest */
07777 /*                        triangulation node to each un- */
07778 /*                        processed node for use by ADDNOD. */
07779 
07780 /* On output: */
07781 
07782 /*       N = Number of nodes in the triangulation.  3 .LE. N */
07783 /*           .LE. N0, or N = 0 if IER < 0. */
07784 
07785 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
07786 /*               of the triangulation nodes in the first N */
07787 /*               locations.  The original array elements are */
07788 /*               shifted down as necessary to eliminate dup- */
07789 /*               licate nodes. */
07790 
07791 /*       LIST = Set of nodal indexes which, along with LPTR, */
07792 /*              LEND, and LNEW, define the triangulation as a */
07793 /*              set of N adjacency lists -- counterclockwise- */
07794 /*              ordered sequences of neighboring nodes such */
07795 /*              that the first and last neighbors of a bound- */
07796 /*              ary node are boundary nodes (the first neigh- */
07797 /*              bor of an interior node is arbitrary).  In */
07798 /*              order to distinguish between interior and */
07799 /*              boundary nodes, the last neighbor of each */
07800 /*              boundary node is represented by the negative */
07801 /*              of its index. */
07802 
07803 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
07804 /*              correspondence with the elements of LIST. */
07805 /*              LIST(LPTR(I)) indexes the node which follows */
07806 /*              LIST(I) in cyclical counterclockwise order */
07807 /*              (the first neighbor follows the last neigh- */
07808 /*              bor). */
07809 
07810 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
07811 /*              points to the last neighbor of node K for */
07812 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
07813 /*              only if K is a boundary node. */
07814 
07815 /*       LNEW = Pointer to the first empty location in LIST */
07816 /*              and LPTR (list length plus one).  LIST, LPTR, */
07817 /*              LEND, and LNEW are not altered if IER < 0, */
07818 /*              and are incomplete if IER > 0. */
07819 
07820 /*       INDX = Array of output (triangulation) nodal indexes */
07821 /*              associated with input nodes.  For I = 1 to */
07822 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
07823 /*              the triangulation node with the same (or */
07824 /*              nearly the same) coordinates as input node I. */
07825 
07826 /*       LCNT = Array of int weights (counts) associated */
07827 /*              with the triangulation nodes.  For I = 1 to */
07828 /*              N, LCNT(I) is the number of occurrences of */
07829 /*              node I in the input node set, and thus the */
07830 /*              number of duplicates is LCNT(I)-1. */
07831 
07832 /*       NEAR,NEXT,DIST = Garbage. */
07833 
07834 /*       IER = Error indicator: */
07835 /*             IER =  0 if no errors were encountered. */
07836 /*             IER = -1 if N0 < 3 on input. */
07837 /*             IER = -2 if the first three nodes are */
07838 /*                      collinear. */
07839 /*             IER = -3 if Subroutine ADDNOD returns an error */
07840 /*                      flag.  This should not occur. */
07841 
07842 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
07843 /*                                INSERT, INTADD, JRAND, */
07844 /*                                LEFT, LSTPTR, STORE, SWAP, */
07845 /*                                SWPTST, TRFIND */
07846 
07847 /* Intrinsic function called by TRMSH3:  ABS */
07848 
07849 /* *********************************************************** */
07850 
07851 
07852 /* Local parameters: */
07853 
07854 /* D =        (Negative cosine of) distance from node KT to */
07855 /*              node I */
07856 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
07857 /*              respectively */
07858 /* I,J =      Nodal indexes */
07859 /* I0 =       Index of the node preceding I in a sequence of */
07860 /*              unprocessed nodes:  I = NEXT(I0) */
07861 /* KT =       Index of a triangulation node */
07862 /* KU =       Index of an unprocessed node and DO-loop index */
07863 /* LP =       LIST index (pointer) of a neighbor of KT */
07864 /* LPL =      Pointer to the last neighbor of KT */
07865 /* NEXTI =    NEXT(I) */
07866 /* NKU =      NEAR(KU) */
07867 
07868     /* Parameter adjustments */
07869     --dist;
07870     --next;
07871     --near__;
07872     --indx;
07873     --lend;
07874     --z__;
07875     --y;
07876     --x;
07877     --list;
07878     --lptr;
07879     --lcnt;
07880 
07881     /* Function Body */
07882     if (*n0 < 3) {
07883         *n = 0;
07884         *ier = -1;
07885         return 0;
07886     }
07887 
07888 /* Store the first triangle in the linked list. */
07889 
07890     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07891             z__[3])) {
07892 
07893 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
07894 
07895         list[1] = 3;
07896         lptr[1] = 2;
07897         list[2] = -2;
07898         lptr[2] = 1;
07899         lend[1] = 2;
07900 
07901         list[3] = 1;
07902         lptr[3] = 4;
07903         list[4] = -3;
07904         lptr[4] = 3;
07905         lend[2] = 4;
07906 
07907         list[5] = 2;
07908         lptr[5] = 6;
07909         list[6] = -1;
07910         lptr[6] = 5;
07911         lend[3] = 6;
07912 
07913     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07914             y[3], &z__[3])) {
07915 
07916 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
07917 /*     i.e., node 3 lies in the left hemisphere defined by */
07918 /*     arc 1->2. */
07919 
07920         list[1] = 2;
07921         lptr[1] = 2;
07922         list[2] = -3;
07923         lptr[2] = 1;
07924         lend[1] = 2;
07925 
07926         list[3] = 3;
07927         lptr[3] = 4;
07928         list[4] = -1;
07929         lptr[4] = 3;
07930         lend[2] = 4;
07931 
07932         list[5] = 1;
07933         lptr[5] = 6;
07934         list[6] = -2;
07935         lptr[6] = 5;
07936         lend[3] = 6;
07937 
07938 
07939     } else {
07940 
07941 /*   The first three nodes are collinear. */
07942 
07943         *n = 0;
07944         *ier = -2;
07945         return 0;
07946     }
07947 
07948     //printf("pass check colinear\n");
07949 
07950 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
07951 
07952     *lnew = 7;
07953     indx[1] = 1;
07954     indx[2] = 2;
07955     indx[3] = 3;
07956     lcnt[1] = 1;
07957     lcnt[2] = 1;
07958     lcnt[3] = 1;
07959     if (*n0 == 3) {
07960         *n = 3;
07961         *ier = 0;
07962         return 0;
07963     }
07964 
07965 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
07966 /*   used to obtain an expected-time (N*log(N)) incremental */
07967 /*   algorithm by enabling constant search time for locating */
07968 /*   each new node in the triangulation. */
07969 
07970 /* For each unprocessed node KU, NEAR(KU) is the index of the */
07971 /*   triangulation node closest to KU (used as the starting */
07972 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
07973 /*   is an increasing function of the arc length (angular */
07974 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
07975 /*   arc length a. */
07976 
07977 /* Since it is necessary to efficiently find the subset of */
07978 /*   unprocessed nodes associated with each triangulation */
07979 /*   node J (those that have J as their NEAR entries), the */
07980 /*   subsets are stored in NEAR and NEXT as follows:  for */
07981 /*   each node J in the triangulation, I = NEAR(J) is the */
07982 /*   first unprocessed node in J's set (with I = 0 if the */
07983 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
07984 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
07985 /*   set are initially ordered by increasing indexes (which */
07986 /*   maximizes efficiency) but that ordering is not main- */
07987 /*   tained as the data structure is updated. */
07988 
07989 /* Initialize the data structure for the single triangle. */
07990 
07991     near__[1] = 0;
07992     near__[2] = 0;
07993     near__[3] = 0;
07994     for (ku = *n0; ku >= 4; --ku) {
07995         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
07996         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
07997         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
07998         if (d1 <= d2 && d1 <= d3) {
07999             near__[ku] = 1;
08000             dist[ku] = d1;
08001             next[ku] = near__[1];
08002             near__[1] = ku;
08003         } else if (d2 <= d1 && d2 <= d3) {
08004             near__[ku] = 2;
08005             dist[ku] = d2;
08006             next[ku] = near__[2];
08007             near__[2] = ku;
08008         } else {
08009             near__[ku] = 3;
08010             dist[ku] = d3;
08011             next[ku] = near__[3];
08012             near__[3] = ku;
08013         }
08014 /* L1: */
08015     }
08016 
08017 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08018 /*   in the triangulation, and NKU = NEAR(KU). */
08019 
08020     kt = 3;
08021     i__1 = *n0;
08022     for (ku = 4; ku <= i__1; ++ku) {
08023         nku = near__[ku];
08024 
08025 /* Remove KU from the set of unprocessed nodes associated */
08026 /*   with NEAR(KU). */
08027         i__ = nku;
08028         if (near__[i__] == ku) {
08029             near__[i__] = next[ku];
08030         } else {
08031             i__ = near__[i__];
08032 L2:
08033             i0 = i__;
08034             i__ = next[i0];
08035             if (i__ != ku) {
08036                 goto L2;
08037             }
08038             next[i0] = next[ku];
08039         }
08040         near__[ku] = 0;
08041 
08042 /* Bypass duplicate nodes. */
08043 
08044         if (dist[ku] <= *tol - 1.) {
08045             indx[ku] = -nku;
08046             ++lcnt[nku];
08047             goto L6;
08048         }
08049 
08050 
08051 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08052         ++kt;
08053         x[kt] = x[ku];
08054         y[kt] = y[ku];
08055         z__[kt] = z__[ku];
08056         indx[ku] = kt;
08057         lcnt[kt] = 1;
08058         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08059                 , lnew, ier);
08060         if (*ier != 0) {
08061             *n = 0;
08062             *ier = -3;
08063             return 0;
08064         }
08065 
08066 /* Loop on neighbors J of node KT. */
08067 
08068         lpl = lend[kt];
08069         lp = lpl;
08070 L3:
08071         lp = lptr[lp];
08072         j = (i__2 = list[lp], abs(i__2));
08073 
08074 /* Loop on elements I in the sequence of unprocessed nodes */
08075 /*   associated with J:  KT is a candidate for replacing J */
08076 /*   as the nearest triangulation node to I.  The next value */
08077 /*   of I in the sequence, NEXT(I), must be saved before I */
08078 /*   is moved because it is altered by adding I to KT's set. */
08079 
08080         i__ = near__[j];
08081 L4:
08082         if (i__ == 0) {
08083             goto L5;
08084         }
08085         nexti = next[i__];
08086 
08087 /* Test for the distance from I to KT less than the distance */
08088 /*   from I to J. */
08089 
08090         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08091         if (d__ < dist[i__]) {
08092 
08093 /* Replace J by KT as the nearest triangulation node to I: */
08094 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08095 /*   of unprocessed nodes and add it to KT's set. */
08096 
08097             near__[i__] = kt;
08098             dist[i__] = d__;
08099             if (i__ == near__[j]) {
08100                 near__[j] = nexti;
08101             } else {
08102                 next[i0] = nexti;
08103             }
08104             next[i__] = near__[kt];
08105             near__[kt] = i__;
08106         } else {
08107             i0 = i__;
08108         }
08109 
08110 /* Bottom of loop on I. */
08111 
08112         i__ = nexti;
08113         goto L4;
08114 
08115 /* Bottom of loop on neighbors J. */
08116 
08117 L5:
08118         if (lp != lpl) {
08119             goto L3;
08120         }
08121 L6:
08122         ;
08123     }
08124     *n = kt;
08125     *ier = 0;
08126     return 0;
08127 } /* trmsh3_ */
08128 
08129 /* stripack.dbl sent by Robert on 06/03/03 */
08130 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08131         double *y, double *z__, int *list, int *lptr, int
08132         *lend, int *lnew, int *ier)
08133 {
08134     /* Initialized data */
08135 
08136     static double tol = 0.;
08137 
08138     /* System generated locals */
08139     int i__1;
08140 
08141     /* Local variables */
08142     static int l;
08143     static double p[3], b1, b2, b3;
08144     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08145     extern /* Subroutine */ int swap_(int *, int *, int *,
08146             int *, int *, int *, int *, int *);
08147     static int lpo1s;
08148     extern /* Subroutine */ int bdyadd_(int *, int *, int *,
08149             int *, int *, int *, int *), intadd_(int *,
08150             int *, int *, int *, int *, int *, int *,
08151             int *), trfind_(int *, double *, int *,
08152             double *, double *, double *, int *, int *,
08153             int *, double *, double *, double *, int *,
08154             int *, int *), covsph_(int *, int *, int *,
08155             int *, int *, int *);
08156     extern int lstptr_(int *, int *, int *, int *);
08157     extern long int swptst_(int *, int *, int *, int *,
08158             double *, double *, double *);
08159 
08160 
08161 /* *********************************************************** */
08162 
08163 /*                                              From STRIPACK */
08164 /*                                            Robert J. Renka */
08165 /*                                  Dept. of Computer Science */
08166 /*                                       Univ. of North Texas */
08167 /*                                           renka@cs.unt.edu */
08168 /*                                                   01/08/03 */
08169 
08170 /*   This subroutine adds node K to a triangulation of the */
08171 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08172 /* of the convex hull of nodes 1,...,K. */
08173 
08174 /*   The algorithm consists of the following steps:  node K */
08175 /* is located relative to the triangulation (TRFIND), its */
08176 /* index is added to the data structure (INTADD or BDYADD), */
08177 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08178 /* the arcs opposite K so that all arcs incident on node K */
08179 /* and opposite node K are locally optimal (satisfy the cir- */
08180 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08181 /* input, a Delaunay triangulation will result. */
08182 
08183 
08184 /* On input: */
08185 
08186 /*       NST = Index of a node at which TRFIND begins its */
08187 /*             search.  Search time depends on the proximity */
08188 /*             of this node to K.  If NST < 1, the search is */
08189 /*             begun at node K-1. */
08190 
08191 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08192 /*           new node to be added.  K .GE. 4. */
08193 
08194 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08195 /*               tesian coordinates of the nodes. */
08196 /*               (X(I),Y(I),Z(I)) defines node I for */
08197 /*               I = 1,...,K. */
08198 
08199 /* The above parameters are not altered by this routine. */
08200 
08201 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08202 /*                             the triangulation of nodes 1 */
08203 /*                             to K-1.  The array lengths are */
08204 /*                             assumed to be large enough to */
08205 /*                             add node K.  Refer to Subrou- */
08206 /*                             tine TRMESH. */
08207 
08208 /* On output: */
08209 
08210 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08211 /*                             the addition of node K as the */
08212 /*                             last entry unless IER .NE. 0 */
08213 /*                             and IER .NE. -3, in which case */
08214 /*                             the arrays are not altered. */
08215 
08216 /*       IER = Error indicator: */
08217 /*             IER =  0 if no errors were encountered. */
08218 /*             IER = -1 if K is outside its valid range */
08219 /*                      on input. */
08220 /*             IER = -2 if all nodes (including K) are col- */
08221 /*                      linear (lie on a common geodesic). */
08222 /*             IER =  L if nodes L and K coincide for some */
08223 /*                      L < K.  Refer to TOL below. */
08224 
08225 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08226 /*                                INTADD, JRAND, LSTPTR, */
08227 /*                                STORE, SWAP, SWPTST, */
08228 /*                                TRFIND */
08229 
08230 /* Intrinsic function called by ADDNOD:  ABS */
08231 
08232 /* *********************************************************** */
08233 
08234 
08235 /* Local parameters: */
08236 
08237 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08238 /*              by TRFIND. */
08239 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08240 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08241 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08242 /*              counterclockwise order. */
08243 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08244 /*              be tested for a swap */
08245 /* IST =      Index of node at which TRFIND begins its search */
08246 /* KK =       Local copy of K */
08247 /* KM1 =      K-1 */
08248 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08249 /*              if node K coincides with a vertex */
08250 /* LP =       LIST pointer */
08251 /* LPF =      LIST pointer to the first neighbor of K */
08252 /* LPO1 =     LIST pointer to IO1 */
08253 /* LPO1S =    Saved value of LPO1 */
08254 /* P =        Cartesian coordinates of node K */
08255 /* TOL =      Tolerance defining coincident nodes:  bound on */
08256 /*              the deviation from 1 of the cosine of the */
08257 /*              angle between the nodes. */
08258 /*              Note that |1-cos(A)| is approximately A*A/2. */
08259 
08260     /* Parameter adjustments */
08261     --lend;
08262     --z__;
08263     --y;
08264     --x;
08265     --list;
08266     --lptr;
08267 
08268     /* Function Body */
08269 
08270     kk = *k;
08271     if (kk < 4) {
08272         goto L3;
08273     }
08274 
08275 /* Initialization: */
08276     km1 = kk - 1;
08277     ist = *nst;
08278     if (ist < 1) {
08279         ist = km1;
08280     }
08281     p[0] = x[kk];
08282     p[1] = y[kk];
08283     p[2] = z__[kk];
08284 
08285 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08286 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08287 /*   from node K. */
08288     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08289             , &b1, &b2, &b3, &i1, &i2, &i3);
08290 
08291 /*   Test for collinear or (nearly) duplicate nodes. */
08292 
08293     if (i1 == 0) {
08294         goto L4;
08295     }
08296     l = i1;
08297     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08298         goto L5;
08299     }
08300     l = i2;
08301     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08302         goto L5;
08303     }
08304     if (i3 != 0) {
08305         l = i3;
08306         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08307             goto L5;
08308         }
08309         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08310     } else {
08311         if (i1 != i2) {
08312             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08313         } else {
08314             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08315         }
08316     }
08317     *ier = 0;
08318 
08319 /* Initialize variables for optimization of the */
08320 /*   triangulation. */
08321     lp = lend[kk];
08322     lpf = lptr[lp];
08323     io2 = list[lpf];
08324     lpo1 = lptr[lpf];
08325     io1 = (i__1 = list[lpo1], abs(i__1));
08326 
08327 /* Begin loop:  find the node opposite K. */
08328 
08329 L1:
08330     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08331     if (list[lp] < 0) {
08332         goto L2;
08333     }
08334     lp = lptr[lp];
08335     in1 = (i__1 = list[lp], abs(i__1));
08336 
08337 /* Swap test:  if a swap occurs, two new arcs are */
08338 /*             opposite K and must be tested. */
08339 
08340     lpo1s = lpo1;
08341     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08342         goto L2;
08343     }
08344     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08345     if (lpo1 == 0) {
08346 
08347 /*   A swap is not possible because KK and IN1 are already */
08348 /*     adjacent.  This error in SWPTST only occurs in the */
08349 /*     neutral case and when there are nearly duplicate */
08350 /*     nodes. */
08351 
08352         lpo1 = lpo1s;
08353         goto L2;
08354     }
08355     io1 = in1;
08356     goto L1;
08357 
08358 /* No swap occurred.  Test for termination and reset */
08359 /*   IO2 and IO1. */
08360 
08361 L2:
08362     if (lpo1 == lpf || list[lpo1] < 0) {
08363         return 0;
08364     }
08365     io2 = io1;
08366     lpo1 = lptr[lpo1];
08367     io1 = (i__1 = list[lpo1], abs(i__1));
08368     goto L1;
08369 
08370 /* KK < 4. */
08371 
08372 L3:
08373     *ier = -1;
08374     return 0;
08375 
08376 /* All nodes are collinear. */
08377 
08378 L4:
08379     *ier = -2;
08380     return 0;
08381 
08382 /* Nodes L and K coincide. */
08383 
08384 L5:
08385     *ier = l;
08386     return 0;
08387 } /* addnod_ */
08388 
08389 double angle_(double *v1, double *v2, double *v3)
08390 {
08391     /* System generated locals */
08392     double ret_val;
08393 
08394     /* Builtin functions */
08395     //double sqrt(double), acos(double);
08396 
08397     /* Local variables */
08398     static double a;
08399     static int i__;
08400     static double ca, s21, s23, u21[3], u23[3];
08401     extern long int left_(double *, double *, double *, double
08402             *, double *, double *, double *, double *,
08403             double *);
08404 
08405 
08406 /* *********************************************************** */
08407 
08408 /*                                              From STRIPACK */
08409 /*                                            Robert J. Renka */
08410 /*                                  Dept. of Computer Science */
08411 /*                                       Univ. of North Texas */
08412 /*                                           renka@cs.unt.edu */
08413 /*                                                   06/03/03 */
08414 
08415 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08416 /* face of the unit sphere, this function returns the */
08417 /* interior angle at V2 -- the dihedral angle between the */
08418 /* plane defined by V2 and V3 (and the origin) and the plane */
08419 /* defined by V2 and V1 or, equivalently, the angle between */
08420 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08421 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08422 /* wise.  The surface area of a spherical polygon with CCW- */
08423 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08424 /* Asum is the sum of the m interior angles computed from the */
08425 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08426 /* (Vm-1,Vm,V1). */
08427 
08428 
08429 /* On input: */
08430 
08431 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08432 /*                  sian coordinates of unit vectors.  These */
08433 /*                  vectors, if nonzero, are implicitly */
08434 /*                  scaled to have length 1. */
08435 
08436 /* Input parameters are not altered by this function. */
08437 
08438 /* On output: */
08439 
08440 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08441 /*               V2 X V3 = 0. */
08442 
08443 /* Module required by ANGLE:  LEFT */
08444 
08445 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08446 
08447 /* *********************************************************** */
08448 
08449 
08450 /* Local parameters: */
08451 
08452 /* A =       Interior angle at V2 */
08453 /* CA =      cos(A) */
08454 /* I =       DO-loop index and index for U21 and U23 */
08455 /* S21,S23 = Sum of squared components of U21 and U23 */
08456 /* U21,U23 = Unit normal vectors to the planes defined by */
08457 /*             pairs of triangle vertices */
08458 
08459 
08460 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08461 
08462     /* Parameter adjustments */
08463     --v3;
08464     --v2;
08465     --v1;
08466 
08467     /* Function Body */
08468     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08469     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08470     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08471 
08472     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08473     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08474     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08475 
08476 /* Normalize U21 and U23 to unit vectors. */
08477 
08478     s21 = 0.;
08479     s23 = 0.;
08480     for (i__ = 1; i__ <= 3; ++i__) {
08481         s21 += u21[i__ - 1] * u21[i__ - 1];
08482         s23 += u23[i__ - 1] * u23[i__ - 1];
08483 /* L1: */
08484     }
08485 
08486 /* Test for a degenerate triangle associated with collinear */
08487 /*   vertices. */
08488 
08489     if (s21 == 0. || s23 == 0.) {
08490         ret_val = 0.;
08491         return ret_val;
08492     }
08493     s21 = sqrt(s21);
08494     s23 = sqrt(s23);
08495     for (i__ = 1; i__ <= 3; ++i__) {
08496         u21[i__ - 1] /= s21;
08497         u23[i__ - 1] /= s23;
08498 /* L2: */
08499     }
08500 
08501 /* Compute the angle A between normals: */
08502 
08503 /*   CA = cos(A) = <U21,U23> */
08504 
08505     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08506     if (ca < -1.) {
08507         ca = -1.;
08508     }
08509     if (ca > 1.) {
08510         ca = 1.;
08511     }
08512     a = acos(ca);
08513 
08514 /* Adjust A to the interior angle:  A > Pi iff */
08515 /*   V3 Right V1->V2. */
08516 
08517     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08518             , &v3[3])) {
08519         a = acos(-1.) * 2. - a;
08520     }
08521     ret_val = a;
08522     return ret_val;
08523 } /* angle_ */
08524 
08525 double areas_(double *v1, double *v2, double *v3)
08526 {
08527     /* System generated locals */
08528     double ret_val;
08529 
08530     /* Builtin functions */
08531     //double sqrt(double), acos(double);
08532 
08533     /* Local variables */
08534     static int i__;
08535     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08536             ca2, ca3;
08537 
08538 
08539 /* *********************************************************** */
08540 
08541 /*                                              From STRIPACK */
08542 /*                                            Robert J. Renka */
08543 /*                                  Dept. of Computer Science */
08544 /*                                       Univ. of North Texas */
08545 /*                                           renka@cs.unt.edu */
08546 /*                                                   06/22/98 */
08547 
08548 /*   This function returns the area of a spherical triangle */
08549 /* on the unit sphere. */
08550 
08551 
08552 /* On input: */
08553 
08554 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08555 /*                  sian coordinates of unit vectors (the */
08556 /*                  three triangle vertices in any order). */
08557 /*                  These vectors, if nonzero, are implicitly */
08558 /*                  scaled to have length 1. */
08559 
08560 /* Input parameters are not altered by this function. */
08561 
08562 /* On output: */
08563 
08564 /*       AREAS = Area of the spherical triangle defined by */
08565 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08566 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08567 /*               if and only if V1, V2, and V3 lie in (or */
08568 /*               close to) a plane containing the origin. */
08569 
08570 /* Modules required by AREAS:  None */
08571 
08572 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08573 
08574 /* *********************************************************** */
08575 
08576 
08577 /* Local parameters: */
08578 
08579 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08580 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08581 /* I =           DO-loop index and index for Uij */
08582 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08583 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08584 /*                 pairs of triangle vertices */
08585 
08586 
08587 /* Compute cross products Uij = Vi X Vj. */
08588 
08589     /* Parameter adjustments */
08590     --v3;
08591     --v2;
08592     --v1;
08593 
08594     /* Function Body */
08595     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08596     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08597     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08598 
08599     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08600     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08601     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08602 
08603     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08604     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08605     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08606 
08607 /* Normalize Uij to unit vectors. */
08608 
08609     s12 = 0.;
08610     s23 = 0.;
08611     s31 = 0.;
08612     for (i__ = 1; i__ <= 3; ++i__) {
08613         s12 += u12[i__ - 1] * u12[i__ - 1];
08614         s23 += u23[i__ - 1] * u23[i__ - 1];
08615         s31 += u31[i__ - 1] * u31[i__ - 1];
08616 /* L2: */
08617     }
08618 
08619 /* Test for a degenerate triangle associated with collinear */
08620 /*   vertices. */
08621 
08622     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08623         ret_val = 0.;
08624         return ret_val;
08625     }
08626     s12 = sqrt(s12);
08627     s23 = sqrt(s23);
08628     s31 = sqrt(s31);
08629     for (i__ = 1; i__ <= 3; ++i__) {
08630         u12[i__ - 1] /= s12;
08631         u23[i__ - 1] /= s23;
08632         u31[i__ - 1] /= s31;
08633 /* L3: */
08634     }
08635 
08636 /* Compute interior angles Ai as the dihedral angles between */
08637 /*   planes: */
08638 /*           CA1 = cos(A1) = -<U12,U31> */
08639 /*           CA2 = cos(A2) = -<U23,U12> */
08640 /*           CA3 = cos(A3) = -<U31,U23> */
08641 
08642     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08643     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08644     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08645     if (ca1 < -1.) {
08646         ca1 = -1.;
08647     }
08648     if (ca1 > 1.) {
08649         ca1 = 1.;
08650     }
08651     if (ca2 < -1.) {
08652         ca2 = -1.;
08653     }
08654     if (ca2 > 1.) {
08655         ca2 = 1.;
08656     }
08657     if (ca3 < -1.) {
08658         ca3 = -1.;
08659     }
08660     if (ca3 > 1.) {
08661         ca3 = 1.;
08662     }
08663     a1 = acos(ca1);
08664     a2 = acos(ca2);
08665     a3 = acos(ca3);
08666 
08667 /* Compute AREAS = A1 + A2 + A3 - PI. */
08668 
08669     ret_val = a1 + a2 + a3 - acos(-1.);
08670     if (ret_val < 0.) {
08671         ret_val = 0.;
08672     }
08673     return ret_val;
08674 } /* areas_ */
08675 
08676 double Util::areav_(int *k, int *n, double *x, double *y,
08677         double *z__, int *list, int *lptr, int *lend, int
08678         *ier)
08679 {
08680     /* Initialized data */
08681 
08682     static double amax = 6.28;
08683 
08684     /* System generated locals */
08685     double ret_val;
08686 
08687     /* Local variables */
08688     static double a, c0[3], c2[3], c3[3];
08689     static int n1, n2, n3;
08690     static double v1[3], v2[3], v3[3];
08691     static int lp, lpl, ierr;
08692     static double asum;
08693     extern double areas_(double *, double *, double *);
08694     static long int first;
08695     extern /* Subroutine */ int circum_(double *, double *,
08696             double *, double *, int *);
08697 
08698 
08699 /* *********************************************************** */
08700 
08701 /*                                            Robert J. Renka */
08702 /*                                  Dept. of Computer Science */
08703 /*                                       Univ. of North Texas */
08704 /*                                           renka@cs.unt.edu */
08705 /*                                                   10/25/02 */
08706 
08707 /*   Given a Delaunay triangulation and the index K of an */
08708 /* interior node, this subroutine returns the (surface) area */
08709 /* of the Voronoi region associated with node K.  The Voronoi */
08710 /* region is the polygon whose vertices are the circumcenters */
08711 /* of the triangles that contain node K, where a triangle */
08712 /* circumcenter is the point (unit vector) lying at the same */
08713 /* angular distance from the three vertices and contained in */
08714 /* the same hemisphere as the vertices. */
08715 
08716 
08717 /* On input: */
08718 
08719 /*       K = Nodal index in the range 1 to N. */
08720 
08721 /*       N = Number of nodes in the triangulation.  N > 3. */
08722 
08723 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08724 /*               coordinates of the nodes (unit vectors). */
08725 
08726 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08727 /*                        gulation.  Refer to Subroutine */
08728 /*                        TRMESH. */
08729 
08730 /* Input parameters are not altered by this function. */
08731 
08732 /* On output: */
08733 
08734 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08735 /*               in which case AREAV = 0. */
08736 
08737 /*       IER = Error indicator: */
08738 /*             IER = 0 if no errors were encountered. */
08739 /*             IER = 1 if K or N is outside its valid range */
08740 /*                     on input. */
08741 /*             IER = 2 if K indexes a boundary node. */
08742 /*             IER = 3 if an error flag is returned by CIRCUM */
08743 /*                     (null triangle). */
08744 /*             IER = 4 if AREAS returns a value greater than */
08745 /*                     AMAX (defined below). */
08746 
08747 /* Modules required by AREAV:  AREAS, CIRCUM */
08748 
08749 /* *********************************************************** */
08750 
08751 
08752 /* Maximum valid triangle area is less than 2*Pi: */
08753 
08754     /* Parameter adjustments */
08755     --lend;
08756     --z__;
08757     --y;
08758     --x;
08759     --list;
08760     --lptr;
08761 
08762     /* Function Body */
08763 
08764 /* Test for invalid input. */
08765 
08766     if (*k < 1 || *k > *n || *n <= 3) {
08767         goto L11;
08768     }
08769 
08770 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08771 /*   FIRST = TRUE only for the first triangle. */
08772 /*   The Voronoi region area is accumulated in ASUM. */
08773 
08774     n1 = *k;
08775     v1[0] = x[n1];
08776     v1[1] = y[n1];
08777     v1[2] = z__[n1];
08778     lpl = lend[n1];
08779     n3 = list[lpl];
08780     if (n3 < 0) {
08781         goto L12;
08782     }
08783     lp = lpl;
08784     first = TRUE_;
08785     asum = 0.;
08786 
08787 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08788 
08789 L1:
08790     n2 = n3;
08791     lp = lptr[lp];
08792     n3 = list[lp];
08793     v2[0] = x[n2];
08794     v2[1] = y[n2];
08795     v2[2] = z__[n2];
08796     v3[0] = x[n3];
08797     v3[1] = y[n3];
08798     v3[2] = z__[n3];
08799     if (first) {
08800 
08801 /* First triangle:  compute the circumcenter C3 and save a */
08802 /*   copy in C0. */
08803 
08804         circum_(v1, v2, v3, c3, &ierr);
08805         if (ierr != 0) {
08806             goto L13;
08807         }
08808         c0[0] = c3[0];
08809         c0[1] = c3[1];
08810         c0[2] = c3[2];
08811         first = FALSE_;
08812     } else {
08813 
08814 /* Set C2 to C3, compute the new circumcenter C3, and compute */
08815 /*   the area A of triangle (V1,C2,C3). */
08816 
08817         c2[0] = c3[0];
08818         c2[1] = c3[1];
08819         c2[2] = c3[2];
08820         circum_(v1, v2, v3, c3, &ierr);
08821         if (ierr != 0) {
08822             goto L13;
08823         }
08824         a = areas_(v1, c2, c3);
08825         if (a > amax) {
08826             goto L14;
08827         }
08828         asum += a;
08829     }
08830 
08831 /* Bottom on loop on neighbors of K. */
08832 
08833     if (lp != lpl) {
08834         goto L1;
08835     }
08836 
08837 /* Compute the area of triangle (V1,C3,C0). */
08838 
08839     a = areas_(v1, c3, c0);
08840     if (a > amax) {
08841         goto L14;
08842     }
08843     asum += a;
08844 
08845 /* No error encountered. */
08846 
08847     *ier = 0;
08848     ret_val = asum;
08849     return ret_val;
08850 
08851 /* Invalid input. */
08852 
08853 L11:
08854     *ier = 1;
08855     ret_val = 0.;
08856     return ret_val;
08857 
08858 /* K indexes a boundary node. */
08859 
08860 L12:
08861     *ier = 2;
08862     ret_val = 0.;
08863     return ret_val;
08864 
08865 /* Error in CIRCUM. */
08866 
08867 L13:
08868     *ier = 3;
08869     ret_val = 0.;
08870     return ret_val;
08871 
08872 /* AREAS value larger than AMAX. */
08873 
08874 L14:
08875     *ier = 4;
08876     ret_val = 0.;
08877     return ret_val;
08878 } /* areav_ */
08879 
08880 double areav_new__(int *k, int *n, double *x, double *y,
08881         double *z__, int *list, int *lptr, int *lend, int
08882         *ier)
08883 {
08884     /* System generated locals */
08885     double ret_val = 0;
08886 
08887     /* Builtin functions */
08888     //double acos(double);
08889 
08890     /* Local variables */
08891     static int m;
08892     static double c1[3], c2[3], c3[3];
08893     static int n1, n2, n3;
08894     static double v1[3], v2[3], v3[3];
08895     static int lp;
08896     static double c1s[3], c2s[3];
08897     static int lpl, ierr;
08898     static double asum;
08899     extern double angle_(double *, double *, double *);
08900     static float areav;
08901     extern /* Subroutine */ int circum_(double *, double *,
08902             double *, double *, int *);
08903 
08904 
08905 /* *********************************************************** */
08906 
08907 /*                                            Robert J. Renka */
08908 /*                                  Dept. of Computer Science */
08909 /*                                       Univ. of North Texas */
08910 /*                                           renka@cs.unt.edu */
08911 /*                                                   06/03/03 */
08912 
08913 /*   Given a Delaunay triangulation and the index K of an */
08914 /* interior node, this subroutine returns the (surface) area */
08915 /* of the Voronoi region associated with node K.  The Voronoi */
08916 /* region is the polygon whose vertices are the circumcenters */
08917 /* of the triangles that contain node K, where a triangle */
08918 /* circumcenter is the point (unit vector) lying at the same */
08919 /* angular distance from the three vertices and contained in */
08920 /* the same hemisphere as the vertices.  The Voronoi region */
08921 /* area is computed as Asum-(m-2)*Pi, where m is the number */
08922 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
08923 /* of interior angles at the vertices. */
08924 
08925 
08926 /* On input: */
08927 
08928 /*       K = Nodal index in the range 1 to N. */
08929 
08930 /*       N = Number of nodes in the triangulation.  N > 3. */
08931 
08932 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08933 /*               coordinates of the nodes (unit vectors). */
08934 
08935 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08936 /*                        gulation.  Refer to Subroutine */
08937 /*                        TRMESH. */
08938 
08939 /* Input parameters are not altered by this function. */
08940 
08941 /* On output: */
08942 
08943 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08944 /*               in which case AREAV = 0. */
08945 
08946 /*       IER = Error indicator: */
08947 /*             IER = 0 if no errors were encountered. */
08948 /*             IER = 1 if K or N is outside its valid range */
08949 /*                     on input. */
08950 /*             IER = 2 if K indexes a boundary node. */
08951 /*             IER = 3 if an error flag is returned by CIRCUM */
08952 /*                     (null triangle). */
08953 
08954 /* Modules required by AREAV:  ANGLE, CIRCUM */
08955 
08956 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
08957 
08958 /* *********************************************************** */
08959 
08960 
08961 /* Test for invalid input. */
08962 
08963     /* Parameter adjustments */
08964     --lend;
08965     --z__;
08966     --y;
08967     --x;
08968     --list;
08969     --lptr;
08970 
08971     /* Function Body */
08972     if (*k < 1 || *k > *n || *n <= 3) {
08973         goto L11;
08974     }
08975 
08976 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08977 /*   The number of neighbors and the sum of interior angles */
08978 /*   are accumulated in M and ASUM, respectively. */
08979 
08980     n1 = *k;
08981     v1[0] = x[n1];
08982     v1[1] = y[n1];
08983     v1[2] = z__[n1];
08984     lpl = lend[n1];
08985     n3 = list[lpl];
08986     if (n3 < 0) {
08987         goto L12;
08988     }
08989     lp = lpl;
08990     m = 0;
08991     asum = 0.;
08992 
08993 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08994 
08995 L1:
08996     ++m;
08997     n2 = n3;
08998     lp = lptr[lp];
08999     n3 = list[lp];
09000     v2[0] = x[n2];
09001     v2[1] = y[n2];
09002     v2[2] = z__[n2];
09003     v3[0] = x[n3];
09004     v3[1] = y[n3];
09005     v3[2] = z__[n3];
09006     if (m == 1) {
09007 
09008 /* First triangle:  compute the circumcenter C2 and save a */
09009 /*   copy in C1S. */
09010 
09011         circum_(v1, v2, v3, c2, &ierr);
09012         if (ierr != 0) {
09013             goto L13;
09014         }
09015         c1s[0] = c2[0];
09016         c1s[1] = c2[1];
09017         c1s[2] = c2[2];
09018     } else if (m == 2) {
09019 
09020 /* Second triangle:  compute the circumcenter C3 and save a */
09021 /*   copy in C2S. */
09022 
09023         circum_(v1, v2, v3, c3, &ierr);
09024         if (ierr != 0) {
09025             goto L13;
09026         }
09027         c2s[0] = c3[0];
09028         c2s[1] = c3[1];
09029         c2s[2] = c3[2];
09030     } else {
09031 
09032 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09033 /*   C3, and compute the interior angle at C2 from the */
09034 /*   sequence of vertices (C1,C2,C3). */
09035 
09036         c1[0] = c2[0];
09037         c1[1] = c2[1];
09038         c1[2] = c2[2];
09039         c2[0] = c3[0];
09040         c2[1] = c3[1];
09041         c2[2] = c3[2];
09042         circum_(v1, v2, v3, c3, &ierr);
09043         if (ierr != 0) {
09044             goto L13;
09045         }
09046         asum += angle_(c1, c2, c3);
09047     }
09048 
09049 /* Bottom on loop on neighbors of K. */
09050 
09051     if (lp != lpl) {
09052         goto L1;
09053     }
09054 
09055 /* C3 is the last vertex.  Compute its interior angle from */
09056 /*   the sequence (C2,C3,C1S). */
09057 
09058     asum += angle_(c2, c3, c1s);
09059 
09060 /* Compute the interior angle at C1S from */
09061 /*   the sequence (C3,C1S,C2S). */
09062 
09063     asum += angle_(c3, c1s, c2s);
09064 
09065 /* No error encountered. */
09066 
09067     *ier = 0;
09068     ret_val = asum - (double) (m - 2) * acos(-1.);
09069     return ret_val;
09070 
09071 /* Invalid input. */
09072 
09073 L11:
09074     *ier = 1;
09075     areav = 0.f;
09076     return ret_val;
09077 
09078 /* K indexes a boundary node. */
09079 
09080 L12:
09081     *ier = 2;
09082     areav = 0.f;
09083     return ret_val;
09084 
09085 /* Error in CIRCUM. */
09086 
09087 L13:
09088     *ier = 3;
09089     areav = 0.f;
09090     return ret_val;
09091 } /* areav_new__ */
09092 
09093 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09094         list, int *lptr, int *lend, int *lnew)
09095 {
09096     static int k, n1, n2, lp, lsav, nsav, next;
09097     extern /* Subroutine */ int insert_(int *, int *, int *,
09098             int *, int *);
09099 
09100 
09101 /* *********************************************************** */
09102 
09103 /*                                              From STRIPACK */
09104 /*                                            Robert J. Renka */
09105 /*                                  Dept. of Computer Science */
09106 /*                                       Univ. of North Texas */
09107 /*                                           renka@cs.unt.edu */
09108 /*                                                   07/11/96 */
09109 
09110 /*   This subroutine adds a boundary node to a triangulation */
09111 /* of a set of KK-1 points on the unit sphere.  The data */
09112 /* structure is updated with the insertion of node KK, but no */
09113 /* optimization is performed. */
09114 
09115 /*   This routine is identical to the similarly named routine */
09116 /* in TRIPACK. */
09117 
09118 
09119 /* On input: */
09120 
09121 /*       KK = Index of a node to be connected to the sequence */
09122 /*            of all visible boundary nodes.  KK .GE. 1 and */
09123 /*            KK must not be equal to I1 or I2. */
09124 
09125 /*       I1 = First (rightmost as viewed from KK) boundary */
09126 /*            node in the triangulation that is visible from */
09127 /*            node KK (the line segment KK-I1 intersects no */
09128 /*            arcs. */
09129 
09130 /*       I2 = Last (leftmost) boundary node that is visible */
09131 /*            from node KK.  I1 and I2 may be determined by */
09132 /*            Subroutine TRFIND. */
09133 
09134 /* The above parameters are not altered by this routine. */
09135 
09136 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09137 /*                             created by Subroutine TRMESH. */
09138 /*                             Nodes I1 and I2 must be in- */
09139 /*                             cluded in the triangulation. */
09140 
09141 /* On output: */
09142 
09143 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09144 /*                             the addition of node KK.  Node */
09145 /*                             KK is connected to I1, I2, and */
09146 /*                             all boundary nodes in between. */
09147 
09148 /* Module required by BDYADD:  INSERT */
09149 
09150 /* *********************************************************** */
09151 
09152 
09153 /* Local parameters: */
09154 
09155 /* K =     Local copy of KK */
09156 /* LP =    LIST pointer */
09157 /* LSAV =  LIST pointer */
09158 /* N1,N2 = Local copies of I1 and I2, respectively */
09159 /* NEXT =  Boundary node visible from K */
09160 /* NSAV =  Boundary node visible from K */
09161 
09162     /* Parameter adjustments */
09163     --lend;
09164     --lptr;
09165     --list;
09166 
09167     /* Function Body */
09168     k = *kk;
09169     n1 = *i1;
09170     n2 = *i2;
09171 
09172 /* Add K as the last neighbor of N1. */
09173 
09174     lp = lend[n1];
09175     lsav = lptr[lp];
09176     lptr[lp] = *lnew;
09177     list[*lnew] = -k;
09178     lptr[*lnew] = lsav;
09179     lend[n1] = *lnew;
09180     ++(*lnew);
09181     next = -list[lp];
09182     list[lp] = next;
09183     nsav = next;
09184 
09185 /* Loop on the remaining boundary nodes between N1 and N2, */
09186 /*   adding K as the first neighbor. */
09187 
09188 L1:
09189     lp = lend[next];
09190     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09191     if (next == n2) {
09192         goto L2;
09193     }
09194     next = -list[lp];
09195     list[lp] = next;
09196     goto L1;
09197 
09198 /* Add the boundary nodes between N1 and N2 as neighbors */
09199 /*   of node K. */
09200 
09201 L2:
09202     lsav = *lnew;
09203     list[*lnew] = n1;
09204     lptr[*lnew] = *lnew + 1;
09205     ++(*lnew);
09206     next = nsav;
09207 
09208 L3:
09209     if (next == n2) {
09210         goto L4;
09211     }
09212     list[*lnew] = next;
09213     lptr[*lnew] = *lnew + 1;
09214     ++(*lnew);
09215     lp = lend[next];
09216     next = list[lp];
09217     goto L3;
09218 
09219 L4:
09220     list[*lnew] = -n2;
09221     lptr[*lnew] = lsav;
09222     lend[k] = *lnew;
09223     ++(*lnew);
09224     return 0;
09225 } /* bdyadd_ */
09226 
09227 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09228         int *lend, int *nodes, int *nb, int *na, int *nt)
09229 {
09230     /* System generated locals */
09231     int i__1;
09232 
09233     /* Local variables */
09234     static int k, n0, lp, nn, nst;
09235 
09236 
09237 /* *********************************************************** */
09238 
09239 /*                                              From STRIPACK */
09240 /*                                            Robert J. Renka */
09241 /*                                  Dept. of Computer Science */
09242 /*                                       Univ. of North Texas */
09243 /*                                           renka@cs.unt.edu */
09244 /*                                                   06/26/96 */
09245 
09246 /*   Given a triangulation of N nodes on the unit sphere */
09247 /* created by Subroutine TRMESH, this subroutine returns an */
09248 /* array containing the indexes (if any) of the counterclock- */
09249 /* wise-ordered sequence of boundary nodes -- the nodes on */
09250 /* the boundary of the convex hull of the set of nodes.  (The */
09251 /* boundary is empty if the nodes do not lie in a single */
09252 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09253 /* triangles are also returned. */
09254 
09255 
09256 /* On input: */
09257 
09258 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09259 
09260 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09261 /*                        gulation.  Refer to Subroutine */
09262 /*                        TRMESH. */
09263 
09264 /* The above parameters are not altered by this routine. */
09265 
09266 /*       NODES = int array of length at least NB */
09267 /*               (NB .LE. N). */
09268 
09269 /* On output: */
09270 
09271 /*       NODES = Ordered sequence of boundary node indexes */
09272 /*               in the range 1 to N (in the first NB loca- */
09273 /*               tions). */
09274 
09275 /*       NB = Number of boundary nodes. */
09276 
09277 /*       NA,NT = Number of arcs and triangles, respectively, */
09278 /*               in the triangulation. */
09279 
09280 /* Modules required by BNODES:  None */
09281 
09282 /* *********************************************************** */
09283 
09284 
09285 /* Local parameters: */
09286 
09287 /* K =   NODES index */
09288 /* LP =  LIST pointer */
09289 /* N0 =  Boundary node to be added to NODES */
09290 /* NN =  Local copy of N */
09291 /* NST = First element of nodes (arbitrarily chosen to be */
09292 /*         the one with smallest index) */
09293 
09294     /* Parameter adjustments */
09295     --lend;
09296     --list;
09297     --lptr;
09298     --nodes;
09299 
09300     /* Function Body */
09301     nn = *n;
09302 
09303 /* Search for a boundary node. */
09304 
09305     i__1 = nn;
09306     for (nst = 1; nst <= i__1; ++nst) {
09307         lp = lend[nst];
09308         if (list[lp] < 0) {
09309             goto L2;
09310         }
09311 /* L1: */
09312     }
09313 
09314 /* The triangulation contains no boundary nodes. */
09315 
09316     *nb = 0;
09317     *na = (nn - 2) * 3;
09318     *nt = nn - (2<<1);
09319     return 0;
09320 
09321 /* NST is the first boundary node encountered.  Initialize */
09322 /*   for traversal of the boundary. */
09323 
09324 L2:
09325     nodes[1] = nst;
09326     k = 1;
09327     n0 = nst;
09328 
09329 /* Traverse the boundary in counterclockwise order. */
09330 
09331 L3:
09332     lp = lend[n0];
09333     lp = lptr[lp];
09334     n0 = list[lp];
09335     if (n0 == nst) {
09336         goto L4;
09337     }
09338     ++k;
09339     nodes[k] = n0;
09340     goto L3;
09341 
09342 /* Store the counts. */
09343 
09344 L4:
09345     *nb = k;
09346     *nt = (*n << 1) - *nb - 2;
09347     *na = *nt + *n - 1;
09348     return 0;
09349 } /* bnodes_ */
09350 
09351 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09352         int *ier)
09353 {
09354     /* System generated locals */
09355     int i__1;
09356 
09357     /* Builtin functions */
09358     //double atan(double), cos(double), sin(double);
09359 
09360     /* Local variables */
09361     static double a, c__;
09362     static int i__;
09363     static double s;
09364     static int k2, k3;
09365     static double x0, y0;
09366     static int kk, np1;
09367 
09368 
09369 /* *********************************************************** */
09370 
09371 /*                                              From STRIPACK */
09372 /*                                            Robert J. Renka */
09373 /*                                  Dept. of Computer Science */
09374 /*                                       Univ. of North Texas */
09375 /*                                           renka@cs.unt.edu */
09376 /*                                                   04/06/90 */
09377 
09378 /*   This subroutine computes the coordinates of a sequence */
09379 /* of N equally spaced points on the unit circle centered at */
09380 /* (0,0).  An N-sided polygonal approximation to the circle */
09381 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09382 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09383 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09384 /* is 2*PI*R, where R is the radius of the circle in device */
09385 /* coordinates. */
09386 
09387 
09388 /* On input: */
09389 
09390 /*       K = Number of points in each quadrant, defining N as */
09391 /*           4K.  K .GE. 1. */
09392 
09393 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09394 
09395 /* K is not altered by this routine. */
09396 
09397 /* On output: */
09398 
09399 /*       XC,YC = Cartesian coordinates of the points on the */
09400 /*               unit circle in the first N+1 locations. */
09401 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09402 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09403 /*               and YC(N+1) = YC(1). */
09404 
09405 /*       IER = Error indicator: */
09406 /*             IER = 0 if no errors were encountered. */
09407 /*             IER = 1 if K < 1 on input. */
09408 
09409 /* Modules required by CIRCLE:  None */
09410 
09411 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09412 /*                                          SIN */
09413 
09414 /* *********************************************************** */
09415 
09416 
09417 /* Local parameters: */
09418 
09419 /* I =     DO-loop index and index for XC and YC */
09420 /* KK =    Local copy of K */
09421 /* K2 =    K*2 */
09422 /* K3 =    K*3 */
09423 /* NP1 =   N+1 = 4*K + 1 */
09424 /* A =     Angular separation between adjacent points */
09425 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09426 /*           rotation through angle A */
09427 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09428 /*           circle in the first quadrant */
09429 
09430     /* Parameter adjustments */
09431     --yc;
09432     --xc;
09433 
09434     /* Function Body */
09435     kk = *k;
09436     k2 = kk << 1;
09437     k3 = kk * 3;
09438     np1 = (kk << 2) + 1;
09439 
09440 /* Test for invalid input, compute A, C, and S, and */
09441 /*   initialize (X0,Y0) to (1,0). */
09442 
09443     if (kk < 1) {
09444         goto L2;
09445     }
09446     a = atan(1.) * 2. / (double) kk;
09447     c__ = cos(a);
09448     s = sin(a);
09449     x0 = 1.;
09450     y0 = 0.;
09451 
09452 /* Loop on points (X0,Y0) in the first quadrant, storing */
09453 /*   the point and its reflections about the x axis, the */
09454 /*   y axis, and the line y = -x. */
09455 
09456     i__1 = kk;
09457     for (i__ = 1; i__ <= i__1; ++i__) {
09458         xc[i__] = x0;
09459         yc[i__] = y0;
09460         xc[i__ + kk] = -y0;
09461         yc[i__ + kk] = x0;
09462         xc[i__ + k2] = -x0;
09463         yc[i__ + k2] = -y0;
09464         xc[i__ + k3] = y0;
09465         yc[i__ + k3] = -x0;
09466 
09467 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09468 
09469         x0 = c__ * x0 - s * y0;
09470         y0 = s * x0 + c__ * y0;
09471 /* L1: */
09472     }
09473 
09474 /* Store the coordinates of the first point as the last */
09475 /*   point. */
09476 
09477     xc[np1] = xc[1];
09478     yc[np1] = yc[1];
09479     *ier = 0;
09480     return 0;
09481 
09482 /* K < 1. */
09483 
09484 L2:
09485     *ier = 1;
09486     return 0;
09487 } /* circle_ */
09488 
09489 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09490         double *c__, int *ier)
09491 {
09492     /* Builtin functions */
09493     //double sqrt(double);
09494 
09495     /* Local variables */
09496     static int i__;
09497     static double e1[3], e2[3], cu[3], cnorm;
09498 
09499 
09500 /* *********************************************************** */
09501 
09502 /*                                              From STRIPACK */
09503 /*                                            Robert J. Renka */
09504 /*                                  Dept. of Computer Science */
09505 /*                                       Univ. of North Texas */
09506 /*                                           renka@cs.unt.edu */
09507 /*                                                   10/27/02 */
09508 
09509 /*   This subroutine returns the circumcenter of a spherical */
09510 /* triangle on the unit sphere:  the point on the sphere sur- */
09511 /* face that is equally distant from the three triangle */
09512 /* vertices and lies in the same hemisphere, where distance */
09513 /* is taken to be arc-length on the sphere surface. */
09514 
09515 
09516 /* On input: */
09517 
09518 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09519 /*                  sian coordinates of the three triangle */
09520 /*                  vertices (unit vectors) in CCW order. */
09521 
09522 /* The above parameters are not altered by this routine. */
09523 
09524 /*       C = Array of length 3. */
09525 
09526 /* On output: */
09527 
09528 /*       C = Cartesian coordinates of the circumcenter unless */
09529 /*           IER > 0, in which case C is not defined.  C = */
09530 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09531 
09532 /*       IER = Error indicator: */
09533 /*             IER = 0 if no errors were encountered. */
09534 /*             IER = 1 if V1, V2, and V3 lie on a common */
09535 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09536 /*             (The vertices are not tested for validity.) */
09537 
09538 /* Modules required by CIRCUM:  None */
09539 
09540 /* Intrinsic function called by CIRCUM:  SQRT */
09541 
09542 /* *********************************************************** */
09543 
09544 
09545 /* Local parameters: */
09546 
09547 /* CNORM = Norm of CU:  used to compute C */
09548 /* CU =    Scalar multiple of C:  E1 X E2 */
09549 /* E1,E2 = Edges of the underlying planar triangle: */
09550 /*           V2-V1 and V3-V1, respectively */
09551 /* I =     DO-loop index */
09552 
09553     /* Parameter adjustments */
09554     --c__;
09555     --v3;
09556     --v2;
09557     --v1;
09558 
09559     /* Function Body */
09560     for (i__ = 1; i__ <= 3; ++i__) {
09561         e1[i__ - 1] = v2[i__] - v1[i__];
09562         e2[i__ - 1] = v3[i__] - v1[i__];
09563 /* L1: */
09564     }
09565 
09566 /* Compute CU = E1 X E2 and CNORM**2. */
09567 
09568     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09569     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09570     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09571     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09572 
09573 /* The vertices lie on a common line if and only if CU is */
09574 /*   the zero vector. */
09575 
09576     if (cnorm != 0.) {
09577 
09578 /*   No error:  compute C. */
09579 
09580         cnorm = sqrt(cnorm);
09581         for (i__ = 1; i__ <= 3; ++i__) {
09582             c__[i__] = cu[i__ - 1] / cnorm;
09583 /* L2: */
09584         }
09585 
09586 /* If the vertices are nearly identical, the problem is */
09587 /*   ill-conditioned and it is possible for the computed */
09588 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09589 /*   when it should be positive. */
09590 
09591         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09592             c__[1] = -c__[1];
09593             c__[2] = -c__[2];
09594             c__[3] = -c__[3];
09595         }
09596         *ier = 0;
09597     } else {
09598 
09599 /*   CU = 0. */
09600 
09601         *ier = 1;
09602     }
09603     return 0;
09604 } /* circum_ */
09605 
09606 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09607         *lptr, int *lend, int *lnew)
09608 {
09609     static int k, lp, nst, lsav, next;
09610     extern /* Subroutine */ int insert_(int *, int *, int *,
09611             int *, int *);
09612 
09613 
09614 /* *********************************************************** */
09615 
09616 /*                                              From STRIPACK */
09617 /*                                            Robert J. Renka */
09618 /*                                  Dept. of Computer Science */
09619 /*                                       Univ. of North Texas */
09620 /*                                           renka@cs.unt.edu */
09621 /*                                                   07/17/96 */
09622 
09623 /*   This subroutine connects an exterior node KK to all */
09624 /* boundary nodes of a triangulation of KK-1 points on the */
09625 /* unit sphere, producing a triangulation that covers the */
09626 /* sphere.  The data structure is updated with the addition */
09627 /* of node KK, but no optimization is performed.  All boun- */
09628 /* dary nodes must be visible from node KK. */
09629 
09630 
09631 /* On input: */
09632 
09633 /*       KK = Index of the node to be connected to the set of */
09634 /*            all boundary nodes.  KK .GE. 4. */
09635 
09636 /*       N0 = Index of a boundary node (in the range 1 to */
09637 /*            KK-1).  N0 may be determined by Subroutine */
09638 /*            TRFIND. */
09639 
09640 /* The above parameters are not altered by this routine. */
09641 
09642 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09643 /*                             created by Subroutine TRMESH. */
09644 /*                             Node N0 must be included in */
09645 /*                             the triangulation. */
09646 
09647 /* On output: */
09648 
09649 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09650 /*                             the addition of node KK as the */
09651 /*                             last entry.  The updated */
09652 /*                             triangulation contains no */
09653 /*                             boundary nodes. */
09654 
09655 /* Module required by COVSPH:  INSERT */
09656 
09657 /* *********************************************************** */
09658 
09659 
09660 /* Local parameters: */
09661 
09662 /* K =     Local copy of KK */
09663 /* LP =    LIST pointer */
09664 /* LSAV =  LIST pointer */
09665 /* NEXT =  Boundary node visible from K */
09666 /* NST =   Local copy of N0 */
09667 
09668     /* Parameter adjustments */
09669     --lend;
09670     --lptr;
09671     --list;
09672 
09673     /* Function Body */
09674     k = *kk;
09675     nst = *n0;
09676 
09677 /* Traverse the boundary in clockwise order, inserting K as */
09678 /*   the first neighbor of each boundary node, and converting */
09679 /*   the boundary node to an interior node. */
09680 
09681     next = nst;
09682 L1:
09683     lp = lend[next];
09684     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09685     next = -list[lp];
09686     list[lp] = next;
09687     if (next != nst) {
09688         goto L1;
09689     }
09690 
09691 /* Traverse the boundary again, adding each node to K's */
09692 /*   adjacency list. */
09693 
09694     lsav = *lnew;
09695 L2:
09696     lp = lend[next];
09697     list[*lnew] = next;
09698     lptr[*lnew] = *lnew + 1;
09699     ++(*lnew);
09700     next = list[lp];
09701     if (next != nst) {
09702         goto L2;
09703     }
09704 
09705     lptr[*lnew - 1] = lsav;
09706     lend[k] = *lnew - 1;
09707     return 0;
09708 } /* covsph_ */
09709 
09710 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09711         double *y, double *z__, int *list, int *lend, int
09712         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09713         double *xc, double *yc, double *zc, double *rc,
09714         int *ier)
09715 {
09716     /* System generated locals */
09717     int i__1, i__2;
09718 
09719     /* Builtin functions */
09720     //double acos(double);
09721 
09722     /* Local variables */
09723     static double c__[3], t;
09724     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09725     static double v1[3], v2[3], v3[3];
09726     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09727              lpn;
09728     static long int swp;
09729     static int ierr;
09730     extern /* Subroutine */ int circum_(double *, double *,
09731             double *, double *, int *);
09732     extern int lstptr_(int *, int *, int *, int *);
09733     extern long int swptst_(int *, int *, int *, int *,
09734             double *, double *, double *);
09735 
09736 
09737 /* *********************************************************** */
09738 
09739 /*                                              From STRIPACK */
09740 /*                                            Robert J. Renka */
09741 /*                                  Dept. of Computer Science */
09742 /*                                       Univ. of North Texas */
09743 /*                                           renka@cs.unt.edu */
09744 /*                                                   03/05/03 */
09745 
09746 /*   Given a Delaunay triangulation of nodes on the surface */
09747 /* of the unit sphere, this subroutine returns the set of */
09748 /* triangle circumcenters corresponding to Voronoi vertices, */
09749 /* along with the circumradii and a list of triangle indexes */
09750 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09751 /* entries. */
09752 
09753 /*   A triangle circumcenter is the point (unit vector) lying */
09754 /* at the same angular distance from the three vertices and */
09755 /* contained in the same hemisphere as the vertices.  (Note */
09756 /* that the negative of a circumcenter is also equidistant */
09757 /* from the vertices.)  If the triangulation covers the sur- */
09758 /* face, the Voronoi vertices are the circumcenters of the */
09759 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09760 /* LNEW are not altered in this case. */
09761 
09762 /*   On the other hand, if the nodes are contained in a sin- */
09763 /* gle hemisphere, the triangulation is implicitly extended */
09764 /* to the entire surface by adding pseudo-arcs (of length */
09765 /* greater than 180 degrees) between boundary nodes forming */
09766 /* pseudo-triangles whose 'circumcenters' are included in the */
09767 /* list.  This extension to the triangulation actually con- */
09768 /* sists of a triangulation of the set of boundary nodes in */
09769 /* which the swap test is reversed (a non-empty circumcircle */
09770 /* test).  The negative circumcenters are stored as the */
09771 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
09772 /* LNEW contain a data structure corresponding to the ex- */
09773 /* tended triangulation (Voronoi diagram), but LIST is not */
09774 /* altered in this case.  Thus, if it is necessary to retain */
09775 /* the original (unextended) triangulation data structure, */
09776 /* copies of LPTR and LNEW must be saved before calling this */
09777 /* routine. */
09778 
09779 
09780 /* On input: */
09781 
09782 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09783 /*           Note that, if N = 3, there are only two Voronoi */
09784 /*           vertices separated by 180 degrees, and the */
09785 /*           Voronoi regions are not well defined. */
09786 
09787 /*       NCOL = Number of columns reserved for LTRI.  This */
09788 /*              must be at least NB-2, where NB is the number */
09789 /*              of boundary nodes. */
09790 
09791 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09792 /*               coordinates of the nodes (unit vectors). */
09793 
09794 /*       LIST = int array containing the set of adjacency */
09795 /*              lists.  Refer to Subroutine TRMESH. */
09796 
09797 /*       LEND = Set of pointers to ends of adjacency lists. */
09798 /*              Refer to Subroutine TRMESH. */
09799 
09800 /* The above parameters are not altered by this routine. */
09801 
09802 /*       LPTR = Array of pointers associated with LIST.  Re- */
09803 /*              fer to Subroutine TRMESH. */
09804 
09805 /*       LNEW = Pointer to the first empty location in LIST */
09806 /*              and LPTR (list length plus one). */
09807 
09808 /*       LTRI = int work space array dimensioned 6 by */
09809 /*              NCOL, or unused dummy parameter if NB = 0. */
09810 
09811 /*       LISTC = int array of length at least 3*NT, where */
09812 /*               NT = 2*N-4 is the number of triangles in the */
09813 /*               triangulation (after extending it to cover */
09814 /*               the entire surface if necessary). */
09815 
09816 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
09817 
09818 /* On output: */
09819 
09820 /*       LPTR = Array of pointers associated with LISTC: */
09821 /*              updated for the addition of pseudo-triangles */
09822 /*              if the original triangulation contains */
09823 /*              boundary nodes (NB > 0). */
09824 
09825 /*       LNEW = Pointer to the first empty location in LISTC */
09826 /*              and LPTR (list length plus one).  LNEW is not */
09827 /*              altered if NB = 0. */
09828 
09829 /*       LTRI = Triangle list whose first NB-2 columns con- */
09830 /*              tain the indexes of a clockwise-ordered */
09831 /*              sequence of vertices (first three rows) */
09832 /*              followed by the LTRI column indexes of the */
09833 /*              triangles opposite the vertices (or 0 */
09834 /*              denoting the exterior region) in the last */
09835 /*              three rows.  This array is not generally of */
09836 /*              any use. */
09837 
09838 /*       LISTC = Array containing triangle indexes (indexes */
09839 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
09840 /*               pondence with LIST/LPTR entries (or entries */
09841 /*               that would be stored in LIST for the */
09842 /*               extended triangulation):  the index of tri- */
09843 /*               angle (N1,N2,N3) is stored in LISTC(K), */
09844 /*               LISTC(L), and LISTC(M), where LIST(K), */
09845 /*               LIST(L), and LIST(M) are the indexes of N2 */
09846 /*               as a neighbor of N1, N3 as a neighbor of N2, */
09847 /*               and N1 as a neighbor of N3.  The Voronoi */
09848 /*               region associated with a node is defined by */
09849 /*               the CCW-ordered sequence of circumcenters in */
09850 /*               one-to-one correspondence with its adjacency */
09851 /*               list (in the extended triangulation). */
09852 
09853 /*       NB = Number of boundary nodes unless IER = 1. */
09854 
09855 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
09856 /*                  nates of the triangle circumcenters */
09857 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
09858 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
09859 /*                  correspond to pseudo-triangles if NB > 0. */
09860 
09861 /*       RC = Array containing circumradii (the arc lengths */
09862 /*            or angles between the circumcenters and associ- */
09863 /*            ated triangle vertices) in 1-1 correspondence */
09864 /*            with circumcenters. */
09865 
09866 /*       IER = Error indicator: */
09867 /*             IER = 0 if no errors were encountered. */
09868 /*             IER = 1 if N < 3. */
09869 /*             IER = 2 if NCOL < NB-2. */
09870 /*             IER = 3 if a triangle is degenerate (has ver- */
09871 /*                     tices lying on a common geodesic). */
09872 
09873 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
09874 
09875 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
09876 
09877 /* *********************************************************** */
09878 
09879 
09880 /* Local parameters: */
09881 
09882 /* C =         Circumcenter returned by Subroutine CIRCUM */
09883 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
09884 /* I4 =        LTRI row index in the range 1 to 3 */
09885 /* IERR =      Error flag for calls to CIRCUM */
09886 /* KT =        Triangle index */
09887 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
09888 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
09889 /*               and N2 as vertices of KT1 */
09890 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
09891 /*               and N2 as vertices of KT2 */
09892 /* LP,LPN =    LIST pointers */
09893 /* LPL =       LIST pointer of the last neighbor of N1 */
09894 /* N0 =        Index of the first boundary node (initial */
09895 /*               value of N1) in the loop on boundary nodes */
09896 /*               used to store the pseudo-triangle indexes */
09897 /*               in LISTC */
09898 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
09899 /*               or pseudo-triangle (clockwise order) */
09900 /* N4 =        Index of the node opposite N2 -> N1 */
09901 /* NM2 =       N-2 */
09902 /* NN =        Local copy of N */
09903 /* NT =        Number of pseudo-triangles:  NB-2 */
09904 /* SWP =       long int variable set to TRUE in each optimiza- */
09905 /*               tion loop (loop on pseudo-arcs) iff a swap */
09906 /*               is performed */
09907 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
09908 /*               Subroutine CIRCUM */
09909 
09910     /* Parameter adjustments */
09911     --lend;
09912     --z__;
09913     --y;
09914     --x;
09915     ltri -= 7;
09916     --list;
09917     --lptr;
09918     --listc;
09919     --xc;
09920     --yc;
09921     --zc;
09922     --rc;
09923 
09924     /* Function Body */
09925     nn = *n;
09926     *nb = 0;
09927     nt = 0;
09928     if (nn < 3) {
09929         goto L21;
09930     }
09931 
09932 /* Search for a boundary node N1. */
09933 
09934     i__1 = nn;
09935     for (n1 = 1; n1 <= i__1; ++n1) {
09936         lp = lend[n1];
09937         if (list[lp] < 0) {
09938             goto L2;
09939         }
09940 /* L1: */
09941     }
09942 
09943 /* The triangulation already covers the sphere. */
09944 
09945     goto L9;
09946 
09947 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
09948 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
09949 /*   boundary nodes to which it is not already adjacent. */
09950 
09951 /*   Set N3 and N2 to the first and last neighbors, */
09952 /*     respectively, of N1. */
09953 
09954 L2:
09955     n2 = -list[lp];
09956     lp = lptr[lp];
09957     n3 = list[lp];
09958 
09959 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
09960 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
09961 /*     along with the indexes of the triangles opposite */
09962 /*     the vertices. */
09963 
09964 L3:
09965     ++nt;
09966     if (nt <= *ncol) {
09967         ltri[nt * 6 + 1] = n1;
09968         ltri[nt * 6 + 2] = n2;
09969         ltri[nt * 6 + 3] = n3;
09970         ltri[nt * 6 + 4] = nt + 1;
09971         ltri[nt * 6 + 5] = nt - 1;
09972         ltri[nt * 6 + 6] = 0;
09973     }
09974     n1 = n2;
09975     lp = lend[n1];
09976     n2 = -list[lp];
09977     if (n2 != n3) {
09978         goto L3;
09979     }
09980 
09981     *nb = nt + 2;
09982     if (*ncol < nt) {
09983         goto L22;
09984     }
09985     ltri[nt * 6 + 4] = 0;
09986     if (nt == 1) {
09987         goto L7;
09988     }
09989 
09990 /* Optimize the exterior triangulation (set of pseudo- */
09991 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
09992 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
09993 /*   The loop on pseudo-arcs is repeated until no swaps are */
09994 /*   performed. */
09995 
09996 L4:
09997     swp = FALSE_;
09998     i__1 = nt - 1;
09999     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10000         for (i3 = 1; i3 <= 3; ++i3) {
10001             kt2 = ltri[i3 + 3 + kt1 * 6];
10002             if (kt2 <= kt1) {
10003                 goto L5;
10004             }
10005 
10006 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10007 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10008 
10009             if (i3 == 1) {
10010                 i1 = 2;
10011                 i2 = 3;
10012             } else if (i3 == 2) {
10013                 i1 = 3;
10014                 i2 = 1;
10015             } else {
10016                 i1 = 1;
10017                 i2 = 2;
10018             }
10019             n1 = ltri[i1 + kt1 * 6];
10020             n2 = ltri[i2 + kt1 * 6];
10021             n3 = ltri[i3 + kt1 * 6];
10022 
10023 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10024 /*     LTRI(I+3,KT2) = KT1. */
10025 
10026             if (ltri[kt2 * 6 + 4] == kt1) {
10027                 i4 = 1;
10028             } else if (ltri[kt2 * 6 + 5] == kt1) {
10029                 i4 = 2;
10030             } else {
10031                 i4 = 3;
10032             }
10033             n4 = ltri[i4 + kt2 * 6];
10034 
10035 /*   The empty circumcircle test is reversed for the pseudo- */
10036 /*     triangles.  The reversal is implicit in the clockwise */
10037 /*     ordering of the vertices. */
10038 
10039             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10040                 goto L5;
10041             }
10042 
10043 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10044 /*     Nj as a vertex of KTi. */
10045 
10046             swp = TRUE_;
10047             kt11 = ltri[i1 + 3 + kt1 * 6];
10048             kt12 = ltri[i2 + 3 + kt1 * 6];
10049             if (i4 == 1) {
10050                 i2 = 2;
10051                 i1 = 3;
10052             } else if (i4 == 2) {
10053                 i2 = 3;
10054                 i1 = 1;
10055             } else {
10056                 i2 = 1;
10057                 i1 = 2;
10058             }
10059             kt21 = ltri[i1 + 3 + kt2 * 6];
10060             kt22 = ltri[i2 + 3 + kt2 * 6];
10061             ltri[kt1 * 6 + 1] = n4;
10062             ltri[kt1 * 6 + 2] = n3;
10063             ltri[kt1 * 6 + 3] = n1;
10064             ltri[kt1 * 6 + 4] = kt12;
10065             ltri[kt1 * 6 + 5] = kt22;
10066             ltri[kt1 * 6 + 6] = kt2;
10067             ltri[kt2 * 6 + 1] = n3;
10068             ltri[kt2 * 6 + 2] = n4;
10069             ltri[kt2 * 6 + 3] = n2;
10070             ltri[kt2 * 6 + 4] = kt21;
10071             ltri[kt2 * 6 + 5] = kt11;
10072             ltri[kt2 * 6 + 6] = kt1;
10073 
10074 /*   Correct the KT11 and KT22 entries that changed. */
10075 
10076             if (kt11 != 0) {
10077                 i4 = 4;
10078                 if (ltri[kt11 * 6 + 4] != kt1) {
10079                     i4 = 5;
10080                     if (ltri[kt11 * 6 + 5] != kt1) {
10081                         i4 = 6;
10082                     }
10083                 }
10084                 ltri[i4 + kt11 * 6] = kt2;
10085             }
10086             if (kt22 != 0) {
10087                 i4 = 4;
10088                 if (ltri[kt22 * 6 + 4] != kt2) {
10089                     i4 = 5;
10090                     if (ltri[kt22 * 6 + 5] != kt2) {
10091                         i4 = 6;
10092                     }
10093                 }
10094                 ltri[i4 + kt22 * 6] = kt1;
10095             }
10096 L5:
10097             ;
10098         }
10099 /* L6: */
10100     }
10101     if (swp) {
10102         goto L4;
10103     }
10104 
10105 /* Compute and store the negative circumcenters and radii of */
10106 /*   the pseudo-triangles in the first NT positions. */
10107 
10108 L7:
10109     i__1 = nt;
10110     for (kt = 1; kt <= i__1; ++kt) {
10111         n1 = ltri[kt * 6 + 1];
10112         n2 = ltri[kt * 6 + 2];
10113         n3 = ltri[kt * 6 + 3];
10114         v1[0] = x[n1];
10115         v1[1] = y[n1];
10116         v1[2] = z__[n1];
10117         v2[0] = x[n2];
10118         v2[1] = y[n2];
10119         v2[2] = z__[n2];
10120         v3[0] = x[n3];
10121         v3[1] = y[n3];
10122         v3[2] = z__[n3];
10123         circum_(v2, v1, v3, c__, &ierr);
10124         if (ierr != 0) {
10125             goto L23;
10126         }
10127 
10128 /*   Store the negative circumcenter and radius (computed */
10129 /*     from <V1,C>). */
10130 
10131         xc[kt] = -c__[0];
10132         yc[kt] = -c__[1];
10133         zc[kt] = -c__[2];
10134         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10135         if (t < -1.) {
10136             t = -1.;
10137         }
10138         if (t > 1.) {
10139             t = 1.;
10140         }
10141         rc[kt] = acos(t);
10142 /* L8: */
10143     }
10144 
10145 /* Compute and store the circumcenters and radii of the */
10146 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10147 /*   Also, store the triangle indexes KT in the appropriate */
10148 /*   LISTC positions. */
10149 
10150 L9:
10151     kt = nt;
10152 
10153 /*   Loop on nodes N1. */
10154 
10155     nm2 = nn - 2;
10156     i__1 = nm2;
10157     for (n1 = 1; n1 <= i__1; ++n1) {
10158         lpl = lend[n1];
10159         lp = lpl;
10160         n3 = list[lp];
10161 
10162 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10163 /*     and N3 > N1. */
10164 
10165 L10:
10166         lp = lptr[lp];
10167         n2 = n3;
10168         n3 = (i__2 = list[lp], abs(i__2));
10169         if (n2 <= n1 || n3 <= n1) {
10170             goto L11;
10171         }
10172         ++kt;
10173 
10174 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10175 
10176         v1[0] = x[n1];
10177         v1[1] = y[n1];
10178         v1[2] = z__[n1];
10179         v2[0] = x[n2];
10180         v2[1] = y[n2];
10181         v2[2] = z__[n2];
10182         v3[0] = x[n3];
10183         v3[1] = y[n3];
10184         v3[2] = z__[n3];
10185         circum_(v1, v2, v3, c__, &ierr);
10186         if (ierr != 0) {
10187             goto L23;
10188         }
10189 
10190 /*   Store the circumcenter, radius and triangle index. */
10191 
10192         xc[kt] = c__[0];
10193         yc[kt] = c__[1];
10194         zc[kt] = c__[2];
10195         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10196         if (t < -1.) {
10197             t = -1.;
10198         }
10199         if (t > 1.) {
10200             t = 1.;
10201         }
10202         rc[kt] = acos(t);
10203 
10204 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10205 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10206 /*     of N2, and N1 as a neighbor of N3. */
10207 
10208         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10209         listc[lpn] = kt;
10210         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10211         listc[lpn] = kt;
10212         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10213         listc[lpn] = kt;
10214 L11:
10215         if (lp != lpl) {
10216             goto L10;
10217         }
10218 /* L12: */
10219     }
10220     if (nt == 0) {
10221         goto L20;
10222     }
10223 
10224 /* Store the first NT triangle indexes in LISTC. */
10225 
10226 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10227 /*     boundary arc opposite N3. */
10228 
10229     kt1 = 0;
10230 L13:
10231     ++kt1;
10232     if (ltri[kt1 * 6 + 4] == 0) {
10233         i1 = 2;
10234         i2 = 3;
10235         i3 = 1;
10236         goto L14;
10237     } else if (ltri[kt1 * 6 + 5] == 0) {
10238         i1 = 3;
10239         i2 = 1;
10240         i3 = 2;
10241         goto L14;
10242     } else if (ltri[kt1 * 6 + 6] == 0) {
10243         i1 = 1;
10244         i2 = 2;
10245         i3 = 3;
10246         goto L14;
10247     }
10248     goto L13;
10249 L14:
10250     n1 = ltri[i1 + kt1 * 6];
10251     n0 = n1;
10252 
10253 /*   Loop on boundary nodes N1 in CCW order, storing the */
10254 /*     indexes of the clockwise-ordered sequence of triangles */
10255 /*     that contain N1.  The first triangle overwrites the */
10256 /*     last neighbor position, and the remaining triangles, */
10257 /*     if any, are appended to N1's adjacency list. */
10258 
10259 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10260 
10261 L15:
10262     lp = lend[n1];
10263     lpn = lptr[lp];
10264     listc[lp] = kt1;
10265 
10266 /*   Loop on triangles KT2 containing N1. */
10267 
10268 L16:
10269     kt2 = ltri[i2 + 3 + kt1 * 6];
10270     if (kt2 != 0) {
10271 
10272 /*   Append KT2 to N1's triangle list. */
10273 
10274         lptr[lp] = *lnew;
10275         lp = *lnew;
10276         listc[lp] = kt2;
10277         ++(*lnew);
10278 
10279 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10280 /*     LTRI(I1,KT1) = N1. */
10281 
10282         kt1 = kt2;
10283         if (ltri[kt1 * 6 + 1] == n1) {
10284             i1 = 1;
10285             i2 = 2;
10286             i3 = 3;
10287         } else if (ltri[kt1 * 6 + 2] == n1) {
10288             i1 = 2;
10289             i2 = 3;
10290             i3 = 1;
10291         } else {
10292             i1 = 3;
10293             i2 = 1;
10294             i3 = 2;
10295         }
10296         goto L16;
10297     }
10298 
10299 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10300 /*     N1 to the next boundary node, test for termination, */
10301 /*     and permute the indexes:  the last triangle containing */
10302 /*     a boundary node is the first triangle containing the */
10303 /*     next boundary node. */
10304 
10305     lptr[lp] = lpn;
10306     n1 = ltri[i3 + kt1 * 6];
10307     if (n1 != n0) {
10308         i4 = i3;
10309         i3 = i2;
10310         i2 = i1;
10311         i1 = i4;
10312         goto L15;
10313     }
10314 
10315 /* No errors encountered. */
10316 
10317 L20:
10318     *ier = 0;
10319     return 0;
10320 
10321 /* N < 3. */
10322 
10323 L21:
10324     *ier = 1;
10325     return 0;
10326 
10327 /* Insufficient space reserved for LTRI. */
10328 
10329 L22:
10330     *ier = 2;
10331     return 0;
10332 
10333 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10334 
10335 L23:
10336     *ier = 3;
10337     return 0;
10338 } /* crlist_ */
10339 
10340 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10341         list, int *lptr, int *lend, int *lnew, int *ier)
10342 {
10343     /* System generated locals */
10344     int i__1;
10345 
10346     /* Local variables */
10347     static int n1, n2, n3, lp, lph, lpl;
10348     extern /* Subroutine */ int delnb_(int *, int *, int *,
10349             int *, int *, int *, int *, int *);
10350     extern int lstptr_(int *, int *, int *, int *);
10351 
10352 
10353 /* *********************************************************** */
10354 
10355 /*                                              From STRIPACK */
10356 /*                                            Robert J. Renka */
10357 /*                                  Dept. of Computer Science */
10358 /*                                       Univ. of North Texas */
10359 /*                                           renka@cs.unt.edu */
10360 /*                                                   07/17/96 */
10361 
10362 /*   This subroutine deletes a boundary arc from a triangula- */
10363 /* tion.  It may be used to remove a null triangle from the */
10364 /* convex hull boundary.  Note, however, that if the union of */
10365 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10366 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10367 /* NEARND should not be called following an arc deletion. */
10368 
10369 /*   This routine is identical to the similarly named routine */
10370 /* in TRIPACK. */
10371 
10372 
10373 /* On input: */
10374 
10375 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10376 
10377 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10378 /*                 adjacent boundary nodes defining the arc */
10379 /*                 to be removed. */
10380 
10381 /* The above parameters are not altered by this routine. */
10382 
10383 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10384 /*                             created by Subroutine TRMESH. */
10385 
10386 /* On output: */
10387 
10388 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10389 /*                             the removal of arc IO1-IO2 */
10390 /*                             unless IER > 0. */
10391 
10392 /*       IER = Error indicator: */
10393 /*             IER = 0 if no errors were encountered. */
10394 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10395 /*                     range, or IO1 = IO2. */
10396 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10397 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10398 /*                     ready a boundary node, and thus IO1 */
10399 /*                     or IO2 has only two neighbors or a */
10400 /*                     deletion would result in two triangu- */
10401 /*                     lations sharing a single node. */
10402 /*             IER = 4 if one of the nodes is a neighbor of */
10403 /*                     the other, but not vice versa, imply- */
10404 /*                     ing an invalid triangulation data */
10405 /*                     structure. */
10406 
10407 /* Module required by DELARC:  DELNB, LSTPTR */
10408 
10409 /* Intrinsic function called by DELARC:  ABS */
10410 
10411 /* *********************************************************** */
10412 
10413 
10414 /* Local parameters: */
10415 
10416 /* LP =       LIST pointer */
10417 /* LPH =      LIST pointer or flag returned by DELNB */
10418 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10419 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10420 /*              is the directed boundary edge associated */
10421 /*              with IO1-IO2 */
10422 
10423     /* Parameter adjustments */
10424     --lend;
10425     --list;
10426     --lptr;
10427 
10428     /* Function Body */
10429     n1 = *io1;
10430     n2 = *io2;
10431 
10432 /* Test for errors, and set N1->N2 to the directed boundary */
10433 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10434 /*   for some N3. */
10435 
10436     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10437         *ier = 1;
10438         return 0;
10439     }
10440 
10441     lpl = lend[n2];
10442     if (-list[lpl] != n1) {
10443         n1 = n2;
10444         n2 = *io1;
10445         lpl = lend[n2];
10446         if (-list[lpl] != n1) {
10447             *ier = 2;
10448             return 0;
10449         }
10450     }
10451 
10452 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10453 /*   of N1), and test for error 3 (N3 already a boundary */
10454 /*   node). */
10455 
10456     lpl = lend[n1];
10457     lp = lptr[lpl];
10458     lp = lptr[lp];
10459     n3 = (i__1 = list[lp], abs(i__1));
10460     lpl = lend[n3];
10461     if (list[lpl] <= 0) {
10462         *ier = 3;
10463         return 0;
10464     }
10465 
10466 /* Delete N2 as a neighbor of N1, making N3 the first */
10467 /*   neighbor, and test for error 4 (N2 not a neighbor */
10468 /*   of N1).  Note that previously computed pointers may */
10469 /*   no longer be valid following the call to DELNB. */
10470 
10471     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10472     if (lph < 0) {
10473         *ier = 4;
10474         return 0;
10475     }
10476 
10477 /* Delete N1 as a neighbor of N2, making N3 the new last */
10478 /*   neighbor. */
10479 
10480     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10481 
10482 /* Make N3 a boundary node with first neighbor N2 and last */
10483 /*   neighbor N1. */
10484 
10485     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10486     lend[n3] = lp;
10487     list[lp] = -n1;
10488 
10489 /* No errors encountered. */
10490 
10491     *ier = 0;
10492     return 0;
10493 } /* delarc_ */
10494 
10495 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10496         list, int *lptr, int *lend, int *lnew, int *lph)
10497 {
10498     /* System generated locals */
10499     int i__1;
10500 
10501     /* Local variables */
10502     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10503 
10504 
10505 /* *********************************************************** */
10506 
10507 /*                                              From STRIPACK */
10508 /*                                            Robert J. Renka */
10509 /*                                  Dept. of Computer Science */
10510 /*                                       Univ. of North Texas */
10511 /*                                           renka@cs.unt.edu */
10512 /*                                                   07/29/98 */
10513 
10514 /*   This subroutine deletes a neighbor NB from the adjacency */
10515 /* list of node N0 (but N0 is not deleted from the adjacency */
10516 /* list of NB) and, if NB is a boundary node, makes N0 a */
10517 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10518 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10519 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10520 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10521 /* ted.  This requires a search of LEND and LPTR entailing an */
10522 /* expected operation count of O(N). */
10523 
10524 /*   This routine is identical to the similarly named routine */
10525 /* in TRIPACK. */
10526 
10527 
10528 /* On input: */
10529 
10530 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10531 /*               nodes such that NB is a neighbor of N0. */
10532 /*               (N0 need not be a neighbor of NB.) */
10533 
10534 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10535 
10536 /* The above parameters are not altered by this routine. */
10537 
10538 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10539 /*                             triangulation. */
10540 
10541 /* On output: */
10542 
10543 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10544 /*                             the removal of NB from the ad- */
10545 /*                             jacency list of N0 unless */
10546 /*                             LPH < 0. */
10547 
10548 /*       LPH = List pointer to the hole (NB as a neighbor of */
10549 /*             N0) filled in by the values at LNEW-1 or error */
10550 /*             indicator: */
10551 /*             LPH > 0 if no errors were encountered. */
10552 /*             LPH = -1 if N0, NB, or N is outside its valid */
10553 /*                      range. */
10554 /*             LPH = -2 if NB is not a neighbor of N0. */
10555 
10556 /* Modules required by DELNB:  None */
10557 
10558 /* Intrinsic function called by DELNB:  ABS */
10559 
10560 /* *********************************************************** */
10561 
10562 
10563 /* Local parameters: */
10564 
10565 /* I =   DO-loop index */
10566 /* LNW = LNEW-1 (output value of LNEW) */
10567 /* LP =  LIST pointer of the last neighbor of NB */
10568 /* LPB = Pointer to NB as a neighbor of N0 */
10569 /* LPL = Pointer to the last neighbor of N0 */
10570 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10571 /* NN =  Local copy of N */
10572 
10573     /* Parameter adjustments */
10574     --lend;
10575     --list;
10576     --lptr;
10577 
10578     /* Function Body */
10579     nn = *n;
10580 
10581 /* Test for error 1. */
10582 
10583     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10584         *lph = -1;
10585         return 0;
10586     }
10587 
10588 /*   Find pointers to neighbors of N0: */
10589 
10590 /*     LPL points to the last neighbor, */
10591 /*     LPP points to the neighbor NP preceding NB, and */
10592 /*     LPB points to NB. */
10593 
10594     lpl = lend[*n0];
10595     lpp = lpl;
10596     lpb = lptr[lpp];
10597 L1:
10598     if (list[lpb] == *nb) {
10599         goto L2;
10600     }
10601     lpp = lpb;
10602     lpb = lptr[lpp];
10603     if (lpb != lpl) {
10604         goto L1;
10605     }
10606 
10607 /*   Test for error 2 (NB not found). */
10608 
10609     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10610         *lph = -2;
10611         return 0;
10612     }
10613 
10614 /*   NB is the last neighbor of N0.  Make NP the new last */
10615 /*     neighbor and, if NB is a boundary node, then make N0 */
10616 /*     a boundary node. */
10617 
10618     lend[*n0] = lpp;
10619     lp = lend[*nb];
10620     if (list[lp] < 0) {
10621         list[lpp] = -list[lpp];
10622     }
10623     goto L3;
10624 
10625 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10626 /*     node and N0 is not, then make N0 a boundary node with */
10627 /*     last neighbor NP. */
10628 
10629 L2:
10630     lp = lend[*nb];
10631     if (list[lp] < 0 && list[lpl] > 0) {
10632         lend[*n0] = lpp;
10633         list[lpp] = -list[lpp];
10634     }
10635 
10636 /*   Update LPTR so that the neighbor following NB now fol- */
10637 /*     lows NP, and fill in the hole at location LPB. */
10638 
10639 L3:
10640     lptr[lpp] = lptr[lpb];
10641     lnw = *lnew - 1;
10642     list[lpb] = list[lnw];
10643     lptr[lpb] = lptr[lnw];
10644     for (i__ = nn; i__ >= 1; --i__) {
10645         if (lend[i__] == lnw) {
10646             lend[i__] = lpb;
10647             goto L5;
10648         }
10649 /* L4: */
10650     }
10651 
10652 L5:
10653     i__1 = lnw - 1;
10654     for (i__ = 1; i__ <= i__1; ++i__) {
10655         if (lptr[i__] == lnw) {
10656             lptr[i__] = lpb;
10657         }
10658 /* L6: */
10659     }
10660 
10661 /* No errors encountered. */
10662 
10663     *lnew = lnw;
10664     *lph = lpb;
10665     return 0;
10666 } /* delnb_ */
10667 
10668 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10669         double *y, double *z__, int *list, int *lptr, int
10670         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10671 {
10672     /* System generated locals */
10673     int i__1;
10674 
10675     /* Local variables */
10676     static int i__, j, n1, n2;
10677     static double x1, x2, y1, y2, z1, z2;
10678     static int nl, lp, nn, nr;
10679     static double xl, yl, zl, xr, yr, zr;
10680     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10681     extern long int left_(double *, double *, double *, double
10682             *, double *, double *, double *, double *,
10683             double *);
10684     static long int bdry;
10685     static int ierr, lwkl;
10686     extern /* Subroutine */ int swap_(int *, int *, int *,
10687             int *, int *, int *, int *, int *), delnb_(
10688             int *, int *, int *, int *, int *, int *,
10689             int *, int *);
10690     extern int nbcnt_(int *, int *);
10691     extern /* Subroutine */ int optim_(double *, double *, double
10692             *, int *, int *, int *, int *, int *, int
10693             *, int *);
10694     static int nfrst;
10695     extern int lstptr_(int *, int *, int *, int *);
10696 
10697 
10698 /* *********************************************************** */
10699 
10700 /*                                              From STRIPACK */
10701 /*                                            Robert J. Renka */
10702 /*                                  Dept. of Computer Science */
10703 /*                                       Univ. of North Texas */
10704 /*                                           renka@cs.unt.edu */
10705 /*                                                   11/30/99 */
10706 
10707 /*   This subroutine deletes node K (along with all arcs */
10708 /* incident on node K) from a triangulation of N nodes on the */
10709 /* unit sphere, and inserts arcs as necessary to produce a */
10710 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10711 /* triangulation is input, a Delaunay triangulation will */
10712 /* result, and thus, DELNOD reverses the effect of a call to */
10713 /* Subroutine ADDNOD. */
10714 
10715 
10716 /* On input: */
10717 
10718 /*       K = Index (for X, Y, and Z) of the node to be */
10719 /*           deleted.  1 .LE. K .LE. N. */
10720 
10721 /* K is not altered by this routine. */
10722 
10723 /*       N = Number of nodes in the triangulation on input. */
10724 /*           N .GE. 4.  Note that N will be decremented */
10725 /*           following the deletion. */
10726 
10727 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10728 /*               coordinates of the nodes in the triangula- */
10729 /*               tion. */
10730 
10731 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10732 /*                             triangulation.  Refer to Sub- */
10733 /*                             routine TRMESH. */
10734 
10735 /*       LWK = Number of columns reserved for IWK.  LWK must */
10736 /*             be at least NNB-3, where NNB is the number of */
10737 /*             neighbors of node K, including an extra */
10738 /*             pseudo-node if K is a boundary node. */
10739 
10740 /*       IWK = int work array dimensioned 2 by LWK (or */
10741 /*             array of length .GE. 2*LWK). */
10742 
10743 /* On output: */
10744 
10745 /*       N = Number of nodes in the triangulation on output. */
10746 /*           The input value is decremented unless 1 .LE. IER */
10747 /*           .LE. 4. */
10748 
10749 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10750 /*               (with elements K+1,...,N+1 shifted up one */
10751 /*               position, thus overwriting element K) unless */
10752 /*               1 .LE. IER .LE. 4. */
10753 
10754 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10755 /*                             structure reflecting the dele- */
10756 /*                             tion unless 1 .LE. IER .LE. 4. */
10757 /*                             Note that the data structure */
10758 /*                             may have been altered if IER > */
10759 /*                             3. */
10760 
10761 /*       LWK = Number of IWK columns required unless IER = 1 */
10762 /*             or IER = 3. */
10763 
10764 /*       IWK = Indexes of the endpoints of the new arcs added */
10765 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10766 /*             are associated with columns, or pairs of */
10767 /*             adjacent elements if IWK is declared as a */
10768 /*             singly-subscripted array.) */
10769 
10770 /*       IER = Error indicator: */
10771 /*             IER = 0 if no errors were encountered. */
10772 /*             IER = 1 if K or N is outside its valid range */
10773 /*                     or LWK < 0 on input. */
10774 /*             IER = 2 if more space is required in IWK. */
10775 /*                     Refer to LWK. */
10776 /*             IER = 3 if the triangulation data structure is */
10777 /*                     invalid on input. */
10778 /*             IER = 4 if K indexes an interior node with */
10779 /*                     four or more neighbors, none of which */
10780 /*                     can be swapped out due to collineari- */
10781 /*                     ty, and K cannot therefore be deleted. */
10782 /*             IER = 5 if an error flag (other than IER = 1) */
10783 /*                     was returned by OPTIM.  An error */
10784 /*                     message is written to the standard */
10785 /*                     output unit in this case. */
10786 /*             IER = 6 if error flag 1 was returned by OPTIM. */
10787 /*                     This is not necessarily an error, but */
10788 /*                     the arcs may not be optimal. */
10789 
10790 /*   Note that the deletion may result in all remaining nodes */
10791 /* being collinear.  This situation is not flagged. */
10792 
10793 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
10794 /*                                OPTIM, SWAP, SWPTST */
10795 
10796 /* Intrinsic function called by DELNOD:  ABS */
10797 
10798 /* *********************************************************** */
10799 
10800 
10801 /* Local parameters: */
10802 
10803 /* BDRY =    long int variable with value TRUE iff N1 is a */
10804 /*             boundary node */
10805 /* I,J =     DO-loop indexes */
10806 /* IERR =    Error flag returned by OPTIM */
10807 /* IWL =     Number of IWK columns containing arcs */
10808 /* LNW =     Local copy of LNEW */
10809 /* LP =      LIST pointer */
10810 /* LP21 =    LIST pointer returned by SWAP */
10811 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
10812 /* LPH =     Pointer (or flag) returned by DELNB */
10813 /* LPL2 =    Pointer to the last neighbor of N2 */
10814 /* LPN =     Pointer to a neighbor of N1 */
10815 /* LWKL =    Input value of LWK */
10816 /* N1 =      Local copy of K */
10817 /* N2 =      Neighbor of N1 */
10818 /* NFRST =   First neighbor of N1:  LIST(LPF) */
10819 /* NIT =     Number of iterations in OPTIM */
10820 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
10821 /*             following (to the left of) N2, respectively */
10822 /* NN =      Number of nodes in the triangulation */
10823 /* NNB =     Number of neighbors of N1 (including a pseudo- */
10824 /*             node representing the boundary if N1 is a */
10825 /*             boundary node) */
10826 /* X1,Y1,Z1 = Coordinates of N1 */
10827 /* X2,Y2,Z2 = Coordinates of N2 */
10828 /* XL,YL,ZL = Coordinates of NL */
10829 /* XR,YR,ZR = Coordinates of NR */
10830 
10831 
10832 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
10833 /*   one if N1 is a boundary node), and test for errors.  LPF */
10834 /*   and LPL are LIST indexes of the first and last neighbors */
10835 /*   of N1, IWL is the number of IWK columns containing arcs, */
10836 /*   and BDRY is TRUE iff N1 is a boundary node. */
10837 
10838     /* Parameter adjustments */
10839     iwk -= 3;
10840     --lend;
10841     --lptr;
10842     --list;
10843     --z__;
10844     --y;
10845     --x;
10846 
10847     /* Function Body */
10848     n1 = *k;
10849     nn = *n;
10850     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10851         goto L21;
10852     }
10853     lpl = lend[n1];
10854     lpf = lptr[lpl];
10855     nnb = nbcnt_(&lpl, &lptr[1]);
10856     bdry = list[lpl] < 0;
10857     if (bdry) {
10858         ++nnb;
10859     }
10860     if (nnb < 3) {
10861         goto L23;
10862     }
10863     lwkl = *lwk;
10864     *lwk = nnb - 3;
10865     if (lwkl < *lwk) {
10866         goto L22;
10867     }
10868     iwl = 0;
10869     if (nnb == 3) {
10870         goto L3;
10871     }
10872 
10873 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
10874 /*   beginning with the second neighbor.  NR and NL are the */
10875 /*   neighbors preceding and following N2, respectively, and */
10876 /*   LP indexes NL.  The loop is exited when all possible */
10877 /*   swaps have been applied to arcs incident on N1. */
10878 
10879     x1 = x[n1];
10880     y1 = y[n1];
10881     z1 = z__[n1];
10882     nfrst = list[lpf];
10883     nr = nfrst;
10884     xr = x[nr];
10885     yr = y[nr];
10886     zr = z__[nr];
10887     lp = lptr[lpf];
10888     n2 = list[lp];
10889     x2 = x[n2];
10890     y2 = y[n2];
10891     z2 = z__[n2];
10892     lp = lptr[lp];
10893 
10894 /* Top of loop:  set NL to the neighbor following N2. */
10895 
10896 L1:
10897     nl = (i__1 = list[lp], abs(i__1));
10898     if (nl == nfrst && bdry) {
10899         goto L3;
10900     }
10901     xl = x[nl];
10902     yl = y[nl];
10903     zl = z__[nl];
10904 
10905 /*   Test for a convex quadrilateral.  To avoid an incorrect */
10906 /*     test caused by collinearity, use the fact that if N1 */
10907 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
10908 /*     a boundary node, then N2 LEFT NL->NR. */
10909 
10910     lpl2 = lend[n2];
10911     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10912             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10913             z2)))) {
10914 
10915 /*   Nonconvex quadrilateral -- no swap is possible. */
10916 
10917         nr = n2;
10918         xr = x2;
10919         yr = y2;
10920         zr = z2;
10921         goto L2;
10922     }
10923 
10924 /*   The quadrilateral defined by adjacent triangles */
10925 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
10926 /*     NL-NR and store it in IWK unless NL and NR are */
10927 /*     already adjacent, in which case the swap is not */
10928 /*     possible.  Indexes larger than N1 must be decremented */
10929 /*     since N1 will be deleted from X, Y, and Z. */
10930 
10931     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
10932     if (lp21 == 0) {
10933         nr = n2;
10934         xr = x2;
10935         yr = y2;
10936         zr = z2;
10937         goto L2;
10938     }
10939     ++iwl;
10940     if (nl <= n1) {
10941         iwk[(iwl << 1) + 1] = nl;
10942     } else {
10943         iwk[(iwl << 1) + 1] = nl - 1;
10944     }
10945     if (nr <= n1) {
10946         iwk[(iwl << 1) + 2] = nr;
10947     } else {
10948         iwk[(iwl << 1) + 2] = nr - 1;
10949     }
10950 
10951 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
10952 
10953     lpl = lend[n1];
10954     --nnb;
10955     if (nnb == 3) {
10956         goto L3;
10957     }
10958     lpf = lptr[lpl];
10959     nfrst = list[lpf];
10960     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
10961     if (nr == nfrst) {
10962         goto L2;
10963     }
10964 
10965 /*   NR is not the first neighbor of N1. */
10966 /*     Back up and test N1-NR for a swap again:  Set N2 to */
10967 /*     NR and NR to the previous neighbor of N1 -- the */
10968 /*     neighbor of NR which follows N1.  LP21 points to NL */
10969 /*     as a neighbor of NR. */
10970 
10971     n2 = nr;
10972     x2 = xr;
10973     y2 = yr;
10974     z2 = zr;
10975     lp21 = lptr[lp21];
10976     lp21 = lptr[lp21];
10977     nr = (i__1 = list[lp21], abs(i__1));
10978     xr = x[nr];
10979     yr = y[nr];
10980     zr = z__[nr];
10981     goto L1;
10982 
10983 /*   Bottom of loop -- test for termination of loop. */
10984 
10985 L2:
10986     if (n2 == nfrst) {
10987         goto L3;
10988     }
10989     n2 = nl;
10990     x2 = xl;
10991     y2 = yl;
10992     z2 = zl;
10993     lp = lptr[lp];
10994     goto L1;
10995 
10996 /* Delete N1 and all its incident arcs.  If N1 is an interior */
10997 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
10998 /*   then N1 must be separated from its neighbors by a plane */
10999 /*   containing the origin -- its removal reverses the effect */
11000 /*   of a call to COVSPH, and all its neighbors become */
11001 /*   boundary nodes.  This is achieved by treating it as if */
11002 /*   it were a boundary node (setting BDRY to TRUE, changing */
11003 /*   a sign in LIST, and incrementing NNB). */
11004 
11005 L3:
11006     if (! bdry) {
11007         if (nnb > 3) {
11008             bdry = TRUE_;
11009         } else {
11010             lpf = lptr[lpl];
11011             nr = list[lpf];
11012             lp = lptr[lpf];
11013             n2 = list[lp];
11014             nl = list[lpl];
11015             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11016                     x[n2], &y[n2], &z__[n2]);
11017         }
11018         if (bdry) {
11019 
11020 /*   IF a boundary node already exists, then N1 and its */
11021 /*     neighbors cannot be converted to boundary nodes. */
11022 /*     (They must be collinear.)  This is a problem if */
11023 /*     NNB > 3. */
11024 
11025             i__1 = nn;
11026             for (i__ = 1; i__ <= i__1; ++i__) {
11027                 if (list[lend[i__]] < 0) {
11028                     bdry = FALSE_;
11029                     goto L5;
11030                 }
11031 /* L4: */
11032             }
11033             list[lpl] = -list[lpl];
11034             ++nnb;
11035         }
11036     }
11037 L5:
11038     if (! bdry && nnb > 3) {
11039         goto L24;
11040     }
11041 
11042 /* Initialize for loop on neighbors.  LPL points to the last */
11043 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11044 
11045     lp = lpl;
11046     lnw = *lnew;
11047 
11048 /* Loop on neighbors N2 of N1, beginning with the first. */
11049 
11050 L6:
11051     lp = lptr[lp];
11052     n2 = (i__1 = list[lp], abs(i__1));
11053     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11054     if (lph < 0) {
11055         goto L23;
11056     }
11057 
11058 /*   LP and LPL may require alteration. */
11059 
11060     if (lpl == lnw) {
11061         lpl = lph;
11062     }
11063     if (lp == lnw) {
11064         lp = lph;
11065     }
11066     if (lp != lpl) {
11067         goto L6;
11068     }
11069 
11070 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11071 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11072 /*   which are larger than N1 must be decremented. */
11073 
11074     --nn;
11075     if (n1 > nn) {
11076         goto L9;
11077     }
11078     i__1 = nn;
11079     for (i__ = n1; i__ <= i__1; ++i__) {
11080         x[i__] = x[i__ + 1];
11081         y[i__] = y[i__ + 1];
11082         z__[i__] = z__[i__ + 1];
11083         lend[i__] = lend[i__ + 1];
11084 /* L7: */
11085     }
11086 
11087     i__1 = lnw - 1;
11088     for (i__ = 1; i__ <= i__1; ++i__) {
11089         if (list[i__] > n1) {
11090             --list[i__];
11091         }
11092         if (list[i__] < -n1) {
11093             ++list[i__];
11094         }
11095 /* L8: */
11096     }
11097 
11098 /*   For LPN = first to last neighbors of N1, delete the */
11099 /*     preceding neighbor (indexed by LP). */
11100 
11101 /*   Each empty LIST,LPTR location LP is filled in with the */
11102 /*     values at LNW-1, and LNW is decremented.  All pointers */
11103 /*     (including those in LPTR and LEND) with value LNW-1 */
11104 /*     must be changed to LP. */
11105 
11106 /*  LPL points to the last neighbor of N1. */
11107 
11108 L9:
11109     if (bdry) {
11110         --nnb;
11111     }
11112     lpn = lpl;
11113     i__1 = nnb;
11114     for (j = 1; j <= i__1; ++j) {
11115         --lnw;
11116         lp = lpn;
11117         lpn = lptr[lp];
11118         list[lp] = list[lnw];
11119         lptr[lp] = lptr[lnw];
11120         if (lptr[lpn] == lnw) {
11121             lptr[lpn] = lp;
11122         }
11123         if (lpn == lnw) {
11124             lpn = lp;
11125         }
11126         for (i__ = nn; i__ >= 1; --i__) {
11127             if (lend[i__] == lnw) {
11128                 lend[i__] = lp;
11129                 goto L11;
11130             }
11131 /* L10: */
11132         }
11133 
11134 L11:
11135         for (i__ = lnw - 1; i__ >= 1; --i__) {
11136             if (lptr[i__] == lnw) {
11137                 lptr[i__] = lp;
11138             }
11139 /* L12: */
11140         }
11141 /* L13: */
11142     }
11143 
11144 /* Update N and LNEW, and optimize the patch of triangles */
11145 /*   containing K (on input) by applying swaps to the arcs */
11146 /*   in IWK. */
11147 
11148     *n = nn;
11149     *lnew = lnw;
11150     if (iwl > 0) {
11151         nit = iwl << 2;
11152         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11153                 nit, &iwk[3], &ierr);
11154         if (ierr != 0 && ierr != 1) {
11155             goto L25;
11156         }
11157         if (ierr == 1) {
11158             goto L26;
11159         }
11160     }
11161 
11162 /* Successful termination. */
11163 
11164     *ier = 0;
11165     return 0;
11166 
11167 /* Invalid input parameter. */
11168 
11169 L21:
11170     *ier = 1;
11171     return 0;
11172 
11173 /* Insufficient space reserved for IWK. */
11174 
11175 L22:
11176     *ier = 2;
11177     return 0;
11178 
11179 /* Invalid triangulation data structure.  NNB < 3 on input or */
11180 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11181 
11182 L23:
11183     *ier = 3;
11184     return 0;
11185 
11186 /* N1 is interior but NNB could not be reduced to 3. */
11187 
11188 L24:
11189     *ier = 4;
11190     return 0;
11191 
11192 /* Error flag (other than 1) returned by OPTIM. */
11193 
11194 L25:
11195     *ier = 5;
11196 /*      WRITE (*,100) NIT, IERR */
11197 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11198 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11199     return 0;
11200 
11201 /* Error flag 1 returned by OPTIM. */
11202 
11203 L26:
11204     *ier = 6;
11205     return 0;
11206 } /* delnod_ */
11207 
11208 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11209         double *tol, int *nseg)
11210 {
11211     /* System generated locals */
11212     int i__1;
11213     double d__1;
11214 
11215     /* Builtin functions */
11216     //double sqrt(double);
11217 
11218     /* Local variables */
11219     static int i__, k;
11220     static double s, p1[3], p2[3], u1, u2, v1, v2;
11221     static int na;
11222     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11223 
11224 
11225 /* *********************************************************** */
11226 
11227 /*                                              From STRIPACK */
11228 /*                                            Robert J. Renka */
11229 /*                                  Dept. of Computer Science */
11230 /*                                       Univ. of North Texas */
11231 /*                                           renka@cs.unt.edu */
11232 /*                                                   03/04/03 */
11233 
11234 /*   Given unit vectors P and Q corresponding to northern */
11235 /* hemisphere points (with positive third components), this */
11236 /* subroutine draws a polygonal line which approximates the */
11237 /* projection of arc P-Q onto the plane containing the */
11238 /* equator. */
11239 
11240 /*   The line segment is drawn by writing a sequence of */
11241 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11242 /* is assumed that an open file is attached to the unit, */
11243 /* header comments have been written to the file, a window- */
11244 /* to-viewport mapping has been established, etc. */
11245 
11246 /* On input: */
11247 
11248 /*       LUN = long int unit number in the range 0 to 99. */
11249 
11250 /*       P,Q = Arrays of length 3 containing the endpoints of */
11251 /*             the arc to be drawn. */
11252 
11253 /*       TOL = Maximum distance in world coordinates between */
11254 /*             the projected arc and polygonal line. */
11255 
11256 /* Input parameters are not altered by this routine. */
11257 
11258 /* On output: */
11259 
11260 /*       NSEG = Number of line segments in the polygonal */
11261 /*              approximation to the projected arc.  This is */
11262 /*              a decreasing function of TOL.  NSEG = 0 and */
11263 /*              no drawing is performed if P = Q or P = -Q */
11264 /*              or an error is encountered in writing to unit */
11265 /*              LUN. */
11266 
11267 /* STRIPACK modules required by DRWARC:  None */
11268 
11269 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11270 
11271 /* *********************************************************** */
11272 
11273 
11274 /* Local parameters: */
11275 
11276 /* DP =    (Q-P)/NSEG */
11277 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11278 /*           onto the projection plane */
11279 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11280 /* ERR =   Orthogonal distance from the projected midpoint */
11281 /*           PM' to the line defined by P' and Q': */
11282 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11283 /* I,K =   DO-loop indexes */
11284 /* NA =    Number of arcs (segments) in the partition of P-Q */
11285 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11286 /*           arc P-Q into NSEG segments; obtained by normal- */
11287 /*           izing PM values */
11288 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11289 /*           uniform partition of the line segment P-Q into */
11290 /*           NSEG segments */
11291 /* S =     Scale factor 1/NA */
11292 /* U1,V1 = Components of P' */
11293 /* U2,V2 = Components of Q' */
11294 /* UM,VM = Components of the midpoint PM' */
11295 
11296 
11297 /* Compute the midpoint PM of arc P-Q. */
11298 
11299     /* Parameter adjustments */
11300     --q;
11301     --p;
11302 
11303     /* Function Body */
11304     enrm = 0.;
11305     for (i__ = 1; i__ <= 3; ++i__) {
11306         pm[i__ - 1] = p[i__] + q[i__];
11307         enrm += pm[i__ - 1] * pm[i__ - 1];
11308 /* L1: */
11309     }
11310     if (enrm == 0.) {
11311         goto L5;
11312     }
11313     enrm = sqrt(enrm);
11314     pm[0] /= enrm;
11315     pm[1] /= enrm;
11316     pm[2] /= enrm;
11317 
11318 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11319 /*   PM' = (UM,VM), respectively. */
11320 
11321     u1 = p[1];
11322     v1 = p[2];
11323     u2 = q[1];
11324     v2 = q[2];
11325     um = pm[0];
11326     vm = pm[1];
11327 
11328 /* Compute the orthogonal distance ERR from PM' to the line */
11329 /*   defined by P' and Q'.  This is the maximum deviation */
11330 /*   between the projected arc and the line segment.  It is */
11331 /*   undefined if P' = Q'. */
11332 
11333     du = u2 - u1;
11334     dv = v2 - v1;
11335     enrm = du * du + dv * dv;
11336     if (enrm == 0.) {
11337         goto L5;
11338     }
11339     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11340 
11341 /* Compute the number of arcs into which P-Q will be parti- */
11342 /*   tioned (the number of line segments to be drawn): */
11343 /*   NA = ERR/TOL. */
11344 
11345     na = (int) (err / *tol + 1.);
11346 
11347 /* Initialize for loop on arcs P1-P2, where the intermediate */
11348 /*   points are obtained by normalizing PM = P + k*DP for */
11349 /*   DP = (Q-P)/NA */
11350 
11351     s = 1. / (double) na;
11352     for (i__ = 1; i__ <= 3; ++i__) {
11353         dp[i__ - 1] = s * (q[i__] - p[i__]);
11354         pm[i__ - 1] = p[i__];
11355         p1[i__ - 1] = p[i__];
11356 /* L2: */
11357     }
11358 
11359 /* Loop on arcs P1-P2, drawing the line segments associated */
11360 /*   with the projected endpoints. */
11361 
11362     i__1 = na - 1;
11363     for (k = 1; k <= i__1; ++k) {
11364         enrm = 0.;
11365         for (i__ = 1; i__ <= 3; ++i__) {
11366             pm[i__ - 1] += dp[i__ - 1];
11367             enrm += pm[i__ - 1] * pm[i__ - 1];
11368 /* L3: */
11369         }
11370         if (enrm == 0.) {
11371             goto L5;
11372         }
11373         enrm = sqrt(enrm);
11374         p2[0] = pm[0] / enrm;
11375         p2[1] = pm[1] / enrm;
11376         p2[2] = pm[2] / enrm;
11377 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11378 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11379         p1[0] = p2[0];
11380         p1[1] = p2[1];
11381         p1[2] = p2[2];
11382 /* L4: */
11383     }
11384 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11385 
11386 /* No error encountered. */
11387 
11388     *nseg = na;
11389     return 0;
11390 
11391 /* Invalid input value of P or Q. */
11392 
11393 L5:
11394     *nseg = 0;
11395     return 0;
11396 } /* drwarc_ */
11397 
11398 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11399         double *y, double *z__, int *lwk, int *iwk, int *
11400         list, int *lptr, int *lend, int *ier)
11401 {
11402     /* System generated locals */
11403     int i__1;
11404 
11405     /* Local variables */
11406     static int i__, n0, n1, n2;
11407     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11408     static int nl, lp, nr;
11409     static double dp12;
11410     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11411     static double dp1l, dp2l, dp1r, dp2r;
11412     extern long int left_(double *, double *, double *, double
11413             *, double *, double *, double *, double *,
11414             double *);
11415     static int ierr;
11416     extern /* Subroutine */ int swap_(int *, int *, int *,
11417             int *, int *, int *, int *, int *);
11418     static int next, iwcp1, n1lst, iwend;
11419     extern /* Subroutine */ int optim_(double *, double *, double
11420             *, int *, int *, int *, int *, int *, int
11421             *, int *);
11422     static int n1frst;
11423 
11424 
11425 /* *********************************************************** */
11426 
11427 /*                                              From STRIPACK */
11428 /*                                            Robert J. Renka */
11429 /*                                  Dept. of Computer Science */
11430 /*                                       Univ. of North Texas */
11431 /*                                           renka@cs.unt.edu */
11432 /*                                                   07/30/98 */
11433 
11434 /*   Given a triangulation of N nodes and a pair of nodal */
11435 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11436 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11437 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11438 /* lation is input, the resulting triangulation is as close */
11439 /* as possible to a Delaunay triangulation in the sense that */
11440 /* all arcs other than IN1-IN2 are locally optimal. */
11441 
11442 /*   A sequence of calls to EDGE may be used to force the */
11443 /* presence of a set of edges defining the boundary of a non- */
11444 /* convex and/or multiply connected region, or to introduce */
11445 /* barriers into the triangulation.  Note that Subroutine */
11446 /* GETNP will not necessarily return closest nodes if the */
11447 /* triangulation has been constrained by a call to EDGE. */
11448 /* However, this is appropriate in some applications, such */
11449 /* as triangle-based interpolation on a nonconvex domain. */
11450 
11451 
11452 /* On input: */
11453 
11454 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11455 /*                 N defining a pair of nodes to be connected */
11456 /*                 by an arc. */
11457 
11458 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11459 /*               coordinates of the nodes. */
11460 
11461 /* The above parameters are not altered by this routine. */
11462 
11463 /*       LWK = Number of columns reserved for IWK.  This must */
11464 /*             be at least NI -- the number of arcs that */
11465 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11466 
11467 /*       IWK = int work array of length at least 2*LWK. */
11468 
11469 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11470 /*                        gulation.  Refer to Subroutine */
11471 /*                        TRMESH. */
11472 
11473 /* On output: */
11474 
11475 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11476 /*             not more than the input value of LWK) unless */
11477 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11478 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11479 
11480 /*       IWK = Array containing the indexes of the endpoints */
11481 /*             of the new arcs other than IN1-IN2 unless */
11482 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11483 /*             IN1->IN2 are stored in the first K-1 columns */
11484 /*             (left portion of IWK), column K contains */
11485 /*             zeros, and new arcs to the right of IN1->IN2 */
11486 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11487 /*             mined by searching IWK for the zeros.) */
11488 
11489 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11490 /*                        to reflect the presence of an arc */
11491 /*                        connecting IN1 and IN2 unless IER > */
11492 /*                        0.  The data structure has been */
11493 /*                        altered if IER >= 4. */
11494 
11495 /*       IER = Error indicator: */
11496 /*             IER = 0 if no errors were encountered. */
11497 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11498 /*                     or LWK < 0 on input. */
11499 /*             IER = 2 if more space is required in IWK. */
11500 /*                     Refer to LWK. */
11501 /*             IER = 3 if IN1 and IN2 could not be connected */
11502 /*                     due to either an invalid data struc- */
11503 /*                     ture or collinear nodes (and floating */
11504 /*                     point error). */
11505 /*             IER = 4 if an error flag other than IER = 1 */
11506 /*                     was returned by OPTIM. */
11507 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11508 /*                     This is not necessarily an error, but */
11509 /*                     the arcs other than IN1-IN2 may not */
11510 /*                     be optimal. */
11511 
11512 /*   An error message is written to the standard output unit */
11513 /* in the case of IER = 3 or IER = 4. */
11514 
11515 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11516 /*                              SWPTST */
11517 
11518 /* Intrinsic function called by EDGE:  ABS */
11519 
11520 /* *********************************************************** */
11521 
11522 
11523 /* Local parameters: */
11524 
11525 /* DPij =     Dot product <Ni,Nj> */
11526 /* I =        DO-loop index and column index for IWK */
11527 /* IERR =     Error flag returned by Subroutine OPTIM */
11528 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11529 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11530 /* IWCP1 =    IWC + 1 */
11531 /* IWEND =    Input or output value of LWK */
11532 /* IWF =      IWK (column) index of the first (leftmost) arc */
11533 /*              which intersects IN1->IN2 */
11534 /* IWL =      IWK (column) index of the last (rightmost) are */
11535 /*              which intersects IN1->IN2 */
11536 /* LFT =      Flag used to determine if a swap results in the */
11537 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11538 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11539 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11540 /* LP =       List pointer (index for LIST and LPTR) */
11541 /* LP21 =     Unused parameter returned by SWAP */
11542 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11543 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11544 /* N1,N2 =    Local copies of IN1 and IN2 */
11545 /* N1FRST =   First neighbor of IN1 */
11546 /* N1LST =    (Signed) last neighbor of IN1 */
11547 /* NEXT =     Node opposite NL->NR */
11548 /* NIT =      Flag or number of iterations employed by OPTIM */
11549 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11550 /*              with NL LEFT IN1->IN2 */
11551 /* X0,Y0,Z0 = Coordinates of N0 */
11552 /* X1,Y1,Z1 = Coordinates of IN1 */
11553 /* X2,Y2,Z2 = Coordinates of IN2 */
11554 
11555 
11556 /* Store IN1, IN2, and LWK in local variables and test for */
11557 /*   errors. */
11558 
11559     /* Parameter adjustments */
11560     --lend;
11561     --lptr;
11562     --list;
11563     iwk -= 3;
11564     --z__;
11565     --y;
11566     --x;
11567 
11568     /* Function Body */
11569     n1 = *in1;
11570     n2 = *in2;
11571     iwend = *lwk;
11572     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11573         goto L31;
11574     }
11575 
11576 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11577 /*   neighbor of N1. */
11578 
11579     lpl = lend[n1];
11580     n0 = (i__1 = list[lpl], abs(i__1));
11581     lp = lpl;
11582 L1:
11583     if (n0 == n2) {
11584         goto L30;
11585     }
11586     lp = lptr[lp];
11587     n0 = list[lp];
11588     if (lp != lpl) {
11589         goto L1;
11590     }
11591 
11592 /* Initialize parameters. */
11593 
11594     iwl = 0;
11595     nit = 0;
11596 
11597 /* Store the coordinates of N1 and N2. */
11598 
11599 L2:
11600     x1 = x[n1];
11601     y1 = y[n1];
11602     z1 = z__[n1];
11603     x2 = x[n2];
11604     y2 = y[n2];
11605     z2 = z__[n2];
11606 
11607 /* Set NR and NL to adjacent neighbors of N1 such that */
11608 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11609 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11610 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11611 
11612 /*   Initialization:  Set N1FRST and N1LST to the first and */
11613 /*     (signed) last neighbors of N1, respectively, and */
11614 /*     initialize NL to N1FRST. */
11615 
11616     lpl = lend[n1];
11617     n1lst = list[lpl];
11618     lp = lptr[lpl];
11619     n1frst = list[lp];
11620     nl = n1frst;
11621     if (n1lst < 0) {
11622         goto L4;
11623     }
11624 
11625 /*   N1 is an interior node.  Set NL to the first candidate */
11626 /*     for NR (NL LEFT N2->N1). */
11627 
11628 L3:
11629     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11630         goto L4;
11631     }
11632     lp = lptr[lp];
11633     nl = list[lp];
11634     if (nl != n1frst) {
11635         goto L3;
11636     }
11637 
11638 /*   All neighbors of N1 are strictly left of N1->N2. */
11639 
11640     goto L5;
11641 
11642 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11643 /*     following neighbor of N1. */
11644 
11645 L4:
11646     nr = nl;
11647     lp = lptr[lp];
11648     nl = (i__1 = list[lp], abs(i__1));
11649     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11650 
11651 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11652 /*     are employed to avoid an error associated with */
11653 /*     collinear nodes. */
11654 
11655         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11656         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11657         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11658         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11659         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11660         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11661                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11662             goto L6;
11663         }
11664 
11665 /*   NL-NR does not intersect N1-N2.  However, there is */
11666 /*     another candidate for the first arc if NL lies on */
11667 /*     the line N1-N2. */
11668 
11669         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11670             goto L5;
11671         }
11672     }
11673 
11674 /*   Bottom of loop. */
11675 
11676     if (nl != n1frst) {
11677         goto L4;
11678     }
11679 
11680 /* Either the triangulation is invalid or N1-N2 lies on the */
11681 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11682 /*   intersecting N1-N2) was not found due to floating point */
11683 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11684 /*   has already been done. */
11685 
11686 L5:
11687     if (nit > 0) {
11688         goto L33;
11689     }
11690     nit = 1;
11691     n1 = n2;
11692     n2 = *in1;
11693     goto L2;
11694 
11695 /* Store the ordered sequence of intersecting edges NL->NR in */
11696 /*   IWK(1,IWL)->IWK(2,IWL). */
11697 
11698 L6:
11699     ++iwl;
11700     if (iwl > iwend) {
11701         goto L32;
11702     }
11703     iwk[(iwl << 1) + 1] = nl;
11704     iwk[(iwl << 1) + 2] = nr;
11705 
11706 /*   Set NEXT to the neighbor of NL which follows NR. */
11707 
11708     lpl = lend[nl];
11709     lp = lptr[lpl];
11710 
11711 /*   Find NR as a neighbor of NL.  The search begins with */
11712 /*     the first neighbor. */
11713 
11714 L7:
11715     if (list[lp] == nr) {
11716         goto L8;
11717     }
11718     lp = lptr[lp];
11719     if (lp != lpl) {
11720         goto L7;
11721     }
11722 
11723 /*   NR must be the last neighbor, and NL->NR cannot be a */
11724 /*     boundary edge. */
11725 
11726     if (list[lp] != nr) {
11727         goto L33;
11728     }
11729 
11730 /*   Set NEXT to the neighbor following NR, and test for */
11731 /*     termination of the store loop. */
11732 
11733 L8:
11734     lp = lptr[lp];
11735     next = (i__1 = list[lp], abs(i__1));
11736     if (next == n2) {
11737         goto L9;
11738     }
11739 
11740 /*   Set NL or NR to NEXT. */
11741 
11742     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11743         nl = next;
11744     } else {
11745         nr = next;
11746     }
11747     goto L6;
11748 
11749 /* IWL is the number of arcs which intersect N1-N2. */
11750 /*   Store LWK. */
11751 
11752 L9:
11753     *lwk = iwl;
11754     iwend = iwl;
11755 
11756 /* Initialize for edge swapping loop -- all possible swaps */
11757 /*   are applied (even if the new arc again intersects */
11758 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11759 /*   left portion of IWK, and arcs to the right are stored in */
11760 /*   the right portion.  IWF and IWL index the first and last */
11761 /*   intersecting arcs. */
11762 
11763     iwf = 1;
11764 
11765 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11766 /*   IWC points to the arc currently being processed.  LFT */
11767 /*   .LE. 0 iff N0 LEFT N1->N2. */
11768 
11769 L10:
11770     lft = 0;
11771     n0 = n1;
11772     x0 = x1;
11773     y0 = y1;
11774     z0 = z1;
11775     nl = iwk[(iwf << 1) + 1];
11776     nr = iwk[(iwf << 1) + 2];
11777     iwc = iwf;
11778 
11779 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
11780 /*     last arc. */
11781 
11782 L11:
11783     if (iwc == iwl) {
11784         goto L21;
11785     }
11786     iwcp1 = iwc + 1;
11787     next = iwk[(iwcp1 << 1) + 1];
11788     if (next != nl) {
11789         goto L16;
11790     }
11791     next = iwk[(iwcp1 << 1) + 2];
11792 
11793 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
11794 /*     swap. */
11795 
11796     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11797             z__[next])) {
11798         goto L14;
11799     }
11800     if (lft >= 0) {
11801         goto L12;
11802     }
11803     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11804             z__[next])) {
11805         goto L14;
11806     }
11807 
11808 /*   Replace NL->NR with N0->NEXT. */
11809 
11810     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11811     iwk[(iwc << 1) + 1] = n0;
11812     iwk[(iwc << 1) + 2] = next;
11813     goto L15;
11814 
11815 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
11816 /*     the left, and store N0-NEXT in the right portion of */
11817 /*     IWK. */
11818 
11819 L12:
11820     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11821     i__1 = iwl;
11822     for (i__ = iwcp1; i__ <= i__1; ++i__) {
11823         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11824         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11825 /* L13: */
11826     }
11827     iwk[(iwl << 1) + 1] = n0;
11828     iwk[(iwl << 1) + 2] = next;
11829     --iwl;
11830     nr = next;
11831     goto L11;
11832 
11833 /*   A swap is not possible.  Set N0 to NR. */
11834 
11835 L14:
11836     n0 = nr;
11837     x0 = x[n0];
11838     y0 = y[n0];
11839     z0 = z__[n0];
11840     lft = 1;
11841 
11842 /*   Advance to the next arc. */
11843 
11844 L15:
11845     nr = next;
11846     ++iwc;
11847     goto L11;
11848 
11849 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
11850 /*     Test for a possible swap. */
11851 
11852 L16:
11853     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11854             z__[next])) {
11855         goto L19;
11856     }
11857     if (lft <= 0) {
11858         goto L17;
11859     }
11860     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11861             z__[next])) {
11862         goto L19;
11863     }
11864 
11865 /*   Replace NL->NR with NEXT->N0. */
11866 
11867     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11868     iwk[(iwc << 1) + 1] = next;
11869     iwk[(iwc << 1) + 2] = n0;
11870     goto L20;
11871 
11872 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
11873 /*     the right, and store N0-NEXT in the left portion of */
11874 /*     IWK. */
11875 
11876 L17:
11877     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11878     i__1 = iwf;
11879     for (i__ = iwc - 1; i__ >= i__1; --i__) {
11880         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11881         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11882 /* L18: */
11883     }
11884     iwk[(iwf << 1) + 1] = n0;
11885     iwk[(iwf << 1) + 2] = next;
11886     ++iwf;
11887     goto L20;
11888 
11889 /*   A swap is not possible.  Set N0 to NL. */
11890 
11891 L19:
11892     n0 = nl;
11893     x0 = x[n0];
11894     y0 = y[n0];
11895     z0 = z__[n0];
11896     lft = -1;
11897 
11898 /*   Advance to the next arc. */
11899 
11900 L20:
11901     nl = next;
11902     ++iwc;
11903     goto L11;
11904 
11905 /*   N2 is opposite NL->NR (IWC = IWL). */
11906 
11907 L21:
11908     if (n0 == n1) {
11909         goto L24;
11910     }
11911     if (lft < 0) {
11912         goto L22;
11913     }
11914 
11915 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
11916 
11917     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11918         goto L10;
11919     }
11920 
11921 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
11922 /*     portion of IWK. */
11923 
11924     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11925     iwk[(iwl << 1) + 1] = n0;
11926     iwk[(iwl << 1) + 2] = n2;
11927     --iwl;
11928     goto L10;
11929 
11930 /*   N0 LEFT N1->N2.  Test for a possible swap. */
11931 
11932 L22:
11933     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
11934         goto L10;
11935     }
11936 
11937 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
11938 /*     right, and store N0-N2 in the left portion of IWK. */
11939 
11940     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11941     i__ = iwl;
11942 L23:
11943     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
11944     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
11945     --i__;
11946     if (i__ > iwf) {
11947         goto L23;
11948     }
11949     iwk[(iwf << 1) + 1] = n0;
11950     iwk[(iwf << 1) + 2] = n2;
11951     ++iwf;
11952     goto L10;
11953 
11954 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
11955 /*   store zeros in IWK. */
11956 
11957 L24:
11958     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11959     iwk[(iwc << 1) + 1] = 0;
11960     iwk[(iwc << 1) + 2] = 0;
11961 
11962 /* Optimization procedure -- */
11963 
11964     *ier = 0;
11965     if (iwc > 1) {
11966 
11967 /*   Optimize the set of new arcs to the left of IN1->IN2. */
11968 
11969         nit = iwc - (1<<2);
11970         i__1 = iwc - 1;
11971         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
11972                 nit, &iwk[3], &ierr);
11973         if (ierr != 0 && ierr != 1) {
11974             goto L34;
11975         }
11976         if (ierr == 1) {
11977             *ier = 5;
11978         }
11979     }
11980     if (iwc < iwend) {
11981 
11982 /*   Optimize the set of new arcs to the right of IN1->IN2. */
11983 
11984         nit = iwend - (iwc<<2);
11985         i__1 = iwend - iwc;
11986         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
11987                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
11988         if (ierr != 0 && ierr != 1) {
11989             goto L34;
11990         }
11991         if (ierr == 1) {
11992             goto L35;
11993         }
11994     }
11995     if (*ier == 5) {
11996         goto L35;
11997     }
11998 
11999 /* Successful termination (IER = 0). */
12000 
12001     return 0;
12002 
12003 /* IN1 and IN2 were adjacent on input. */
12004 
12005 L30:
12006     *ier = 0;
12007     return 0;
12008 
12009 /* Invalid input parameter. */
12010 
12011 L31:
12012     *ier = 1;
12013     return 0;
12014 
12015 /* Insufficient space reserved for IWK. */
12016 
12017 L32:
12018     *ier = 2;
12019     return 0;
12020 
12021 /* Invalid triangulation data structure or collinear nodes */
12022 /*   on convex hull boundary. */
12023 
12024 L33:
12025     *ier = 3;
12026 /*      WRITE (*,130) IN1, IN2 */
12027 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12028 /*     .        'tion or null triangles on boundary'/ */
12029 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12030     return 0;
12031 
12032 /* Error flag (other than 1) returned by OPTIM. */
12033 
12034 L34:
12035     *ier = 4;
12036 /*      WRITE (*,140) NIT, IERR */
12037 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12038 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12039     return 0;
12040 
12041 /* Error flag 1 returned by OPTIM. */
12042 
12043 L35:
12044     *ier = 5;
12045     return 0;
12046 } /* edge_ */
12047 
12048 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12049         int *list, int *lptr, int *lend, int *l, int *
12050         npts, double *df, int *ier)
12051 {
12052     /* System generated locals */
12053     int i__1, i__2;
12054 
12055     /* Local variables */
12056     static int i__, n1;
12057     static double x1, y1, z1;
12058     static int nb, ni, lp, np, lm1;
12059     static double dnb, dnp;
12060     static int lpl;
12061 
12062 
12063 /* *********************************************************** */
12064 
12065 /*                                              From STRIPACK */
12066 /*                                            Robert J. Renka */
12067 /*                                  Dept. of Computer Science */
12068 /*                                       Univ. of North Texas */
12069 /*                                           renka@cs.unt.edu */
12070 /*                                                   07/28/98 */
12071 
12072 /*   Given a Delaunay triangulation of N nodes on the unit */
12073 /* sphere and an array NPTS containing the indexes of L-1 */
12074 /* nodes ordered by angular distance from NPTS(1), this sub- */
12075 /* routine sets NPTS(L) to the index of the next node in the */
12076 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12077 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12078 /* of K closest nodes to N1 (including N1) may be determined */
12079 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12080 /* for K .GE. 2. */
12081 
12082 /*   The algorithm uses the property of a Delaunay triangula- */
12083 /* tion that the K-th closest node to N1 is a neighbor of one */
12084 /* of the K-1 closest nodes to N1. */
12085 
12086 
12087 /* On input: */
12088 
12089 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12090 /*               coordinates of the nodes. */
12091 
12092 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12093 /*                        fer to Subroutine TRMESH. */
12094 
12095 /*       L = Number of nodes in the sequence on output.  2 */
12096 /*           .LE. L .LE. N. */
12097 
12098 /* The above parameters are not altered by this routine. */
12099 
12100 /*       NPTS = Array of length .GE. L containing the indexes */
12101 /*              of the L-1 closest nodes to NPTS(1) in the */
12102 /*              first L-1 locations. */
12103 
12104 /* On output: */
12105 
12106 /*       NPTS = Array updated with the index of the L-th */
12107 /*              closest node to NPTS(1) in position L unless */
12108 /*              IER = 1. */
12109 
12110 /*       DF = Value of an increasing function (negative cos- */
12111 /*            ine) of the angular distance between NPTS(1) */
12112 /*            and NPTS(L) unless IER = 1. */
12113 
12114 /*       IER = Error indicator: */
12115 /*             IER = 0 if no errors were encountered. */
12116 /*             IER = 1 if L < 2. */
12117 
12118 /* Modules required by GETNP:  None */
12119 
12120 /* Intrinsic function called by GETNP:  ABS */
12121 
12122 /* *********************************************************** */
12123 
12124 
12125 /* Local parameters: */
12126 
12127 /* DNB,DNP =  Negative cosines of the angular distances from */
12128 /*              N1 to NB and to NP, respectively */
12129 /* I =        NPTS index and DO-loop index */
12130 /* LM1 =      L-1 */
12131 /* LP =       LIST pointer of a neighbor of NI */
12132 /* LPL =      Pointer to the last neighbor of NI */
12133 /* N1 =       NPTS(1) */
12134 /* NB =       Neighbor of NI and candidate for NP */
12135 /* NI =       NPTS(I) */
12136 /* NP =       Candidate for NPTS(L) */
12137 /* X1,Y1,Z1 = Coordinates of N1 */
12138 
12139     /* Parameter adjustments */
12140     --x;
12141     --y;
12142     --z__;
12143     --list;
12144     --lptr;
12145     --lend;
12146     --npts;
12147 
12148     /* Function Body */
12149     lm1 = *l - 1;
12150     if (lm1 < 1) {
12151         goto L6;
12152     }
12153     *ier = 0;
12154 
12155 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12156 
12157     n1 = npts[1];
12158     x1 = x[n1];
12159     y1 = y[n1];
12160     z1 = z__[n1];
12161     i__1 = lm1;
12162     for (i__ = 1; i__ <= i__1; ++i__) {
12163         ni = npts[i__];
12164         lend[ni] = -lend[ni];
12165 /* L1: */
12166     }
12167 
12168 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12169 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12170 /*   (the maximum distance). */
12171 
12172     dnp = 2.;
12173 
12174 /* Loop on nodes NI in NPTS. */
12175 
12176     i__1 = lm1;
12177     for (i__ = 1; i__ <= i__1; ++i__) {
12178         ni = npts[i__];
12179         lpl = -lend[ni];
12180         lp = lpl;
12181 
12182 /* Loop on neighbors NB of NI. */
12183 
12184 L2:
12185         nb = (i__2 = list[lp], abs(i__2));
12186         if (lend[nb] < 0) {
12187             goto L3;
12188         }
12189 
12190 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12191 /*   closer to N1. */
12192 
12193         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12194         if (dnb >= dnp) {
12195             goto L3;
12196         }
12197         np = nb;
12198         dnp = dnb;
12199 L3:
12200         lp = lptr[lp];
12201         if (lp != lpl) {
12202             goto L2;
12203         }
12204 /* L4: */
12205     }
12206     npts[*l] = np;
12207     *df = dnp;
12208 
12209 /* Unmark the elements of NPTS. */
12210 
12211     i__1 = lm1;
12212     for (i__ = 1; i__ <= i__1; ++i__) {
12213         ni = npts[i__];
12214         lend[ni] = -lend[ni];
12215 /* L5: */
12216     }
12217     return 0;
12218 
12219 /* L is outside its valid range. */
12220 
12221 L6:
12222     *ier = 1;
12223     return 0;
12224 } /* getnp_ */
12225 
12226 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12227         lptr, int *lnew)
12228 {
12229     static int lsav;
12230 
12231 
12232 /* *********************************************************** */
12233 
12234 /*                                              From STRIPACK */
12235 /*                                            Robert J. Renka */
12236 /*                                  Dept. of Computer Science */
12237 /*                                       Univ. of North Texas */
12238 /*                                           renka@cs.unt.edu */
12239 /*                                                   07/17/96 */
12240 
12241 /*   This subroutine inserts K as a neighbor of N1 following */
12242 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12243 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12244 /* become the first neighbor (even if N1 is a boundary node). */
12245 
12246 /*   This routine is identical to the similarly named routine */
12247 /* in TRIPACK. */
12248 
12249 
12250 /* On input: */
12251 
12252 /*       K = Index of the node to be inserted. */
12253 
12254 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12255 
12256 /* The above parameters are not altered by this routine. */
12257 
12258 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12259 /*                        gulation.  Refer to Subroutine */
12260 /*                        TRMESH. */
12261 
12262 /* On output: */
12263 
12264 /*       LIST,LPTR,LNEW = Data structure updated with the */
12265 /*                        addition of node K. */
12266 
12267 /* Modules required by INSERT:  None */
12268 
12269 /* *********************************************************** */
12270 
12271 
12272     /* Parameter adjustments */
12273     --lptr;
12274     --list;
12275 
12276     /* Function Body */
12277     lsav = lptr[*lp];
12278     lptr[*lp] = *lnew;
12279     list[*lnew] = *k;
12280     lptr[*lnew] = lsav;
12281     ++(*lnew);
12282     return 0;
12283 } /* insert_ */
12284 
12285 long int inside_(double *p, int *lv, double *xv, double *yv,
12286         double *zv, int *nv, int *listv, int *ier)
12287 {
12288     /* Initialized data */
12289 
12290     static double eps = .001;
12291 
12292     /* System generated locals */
12293     int i__1;
12294     long int ret_val = 0;
12295 
12296     /* Builtin functions */
12297     //double sqrt(double);
12298 
12299     /* Local variables */
12300     static double b[3], d__;
12301     static int k, n;
12302     static double q[3];
12303     static int i1, i2, k0;
12304     static double v1[3], v2[3], cn[3], bp, bq;
12305     static int ni;
12306     static double pn[3], qn[3], vn[3];
12307     static int imx;
12308     static long int lft1, lft2, even;
12309     static int ierr;
12310     static long int pinr, qinr;
12311     static double qnrm, vnrm;
12312     extern /* Subroutine */ int intrsc_(double *, double *,
12313             double *, double *, int *);
12314 
12315 
12316 /* *********************************************************** */
12317 
12318 /*                                              From STRIPACK */
12319 /*                                            Robert J. Renka */
12320 /*                                  Dept. of Computer Science */
12321 /*                                       Univ. of North Texas */
12322 /*                                           renka@cs.unt.edu */
12323 /*                                                   12/27/93 */
12324 
12325 /*   This function locates a point P relative to a polygonal */
12326 /* region R on the surface of the unit sphere, returning */
12327 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12328 /* defined by a cyclically ordered sequence of vertices which */
12329 /* form a positively-oriented simple closed curve.  Adjacent */
12330 /* vertices need not be distinct but the curve must not be */
12331 /* self-intersecting.  Also, while polygon edges are by defi- */
12332 /* nition restricted to a single hemisphere, R is not so */
12333 /* restricted.  Its interior is the region to the left as the */
12334 /* vertices are traversed in order. */
12335 
12336 /*   The algorithm consists of selecting a point Q in R and */
12337 /* then finding all points at which the great circle defined */
12338 /* by P and Q intersects the boundary of R.  P lies inside R */
12339 /* if and only if there is an even number of intersection */
12340 /* points between Q and P.  Q is taken to be a point immedi- */
12341 /* ately to the left of a directed boundary edge -- the first */
12342 /* one that results in no consistency-check failures. */
12343 
12344 /*   If P is close to the polygon boundary, the problem is */
12345 /* ill-conditioned and the decision may be incorrect.  Also, */
12346 /* an incorrect decision may result from a poor choice of Q */
12347 /* (if, for example, a boundary edge lies on the great cir- */
12348 /* cle defined by P and Q).  A more reliable result could be */
12349 /* obtained by a sequence of calls to INSIDE with the ver- */
12350 /* tices cyclically permuted before each call (to alter the */
12351 /* choice of Q). */
12352 
12353 
12354 /* On input: */
12355 
12356 /*       P = Array of length 3 containing the Cartesian */
12357 /*           coordinates of the point (unit vector) to be */
12358 /*           located. */
12359 
12360 /*       LV = Length of arrays XV, YV, and ZV. */
12361 
12362 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12363 /*                  sian coordinates of unit vectors (points */
12364 /*                  on the unit sphere).  These values are */
12365 /*                  not tested for validity. */
12366 
12367 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12368 /*            .LE. LV. */
12369 
12370 /*       LISTV = Array of length NV containing the indexes */
12371 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12372 /*               (and CCW-ordered) sequence of vertices that */
12373 /*               define R.  The last vertex (indexed by */
12374 /*               LISTV(NV)) is followed by the first (indexed */
12375 /*               by LISTV(1)).  LISTV entries must be in the */
12376 /*               range 1 to LV. */
12377 
12378 /* Input parameters are not altered by this function. */
12379 
12380 /* On output: */
12381 
12382 /*       INSIDE = TRUE if and only if P lies inside R unless */
12383 /*                IER .NE. 0, in which case the value is not */
12384 /*                altered. */
12385 
12386 /*       IER = Error indicator: */
12387 /*             IER = 0 if no errors were encountered. */
12388 /*             IER = 1 if LV or NV is outside its valid */
12389 /*                     range. */
12390 /*             IER = 2 if a LISTV entry is outside its valid */
12391 /*                     range. */
12392 /*             IER = 3 if the polygon boundary was found to */
12393 /*                     be self-intersecting.  This error will */
12394 /*                     not necessarily be detected. */
12395 /*             IER = 4 if every choice of Q (one for each */
12396 /*                     boundary edge) led to failure of some */
12397 /*                     internal consistency check.  The most */
12398 /*                     likely cause of this error is invalid */
12399 /*                     input:  P = (0,0,0), a null or self- */
12400 /*                     intersecting polygon, etc. */
12401 
12402 /* Module required by INSIDE:  INTRSC */
12403 
12404 /* Intrinsic function called by INSIDE:  SQRT */
12405 
12406 /* *********************************************************** */
12407 
12408 
12409 /* Local parameters: */
12410 
12411 /* B =         Intersection point between the boundary and */
12412 /*               the great circle defined by P and Q */
12413 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12414 /*               intersection points B that lie between P and */
12415 /*               Q (on the shorter arc) -- used to find the */
12416 /*               closest intersection points to P and Q */
12417 /* CN =        Q X P = normal to the plane of P and Q */
12418 /* D =         Dot product <B,P> or <B,Q> */
12419 /* EPS =       Parameter used to define Q as the point whose */
12420 /*               orthogonal distance to (the midpoint of) */
12421 /*               boundary edge V1->V2 is approximately EPS/ */
12422 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12423 /* EVEN =      TRUE iff an even number of intersection points */
12424 /*               lie between P and Q (on the shorter arc) */
12425 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12426 /*               boundary vertices (endpoints of a boundary */
12427 /*               edge) */
12428 /* IERR =      Error flag for calls to INTRSC (not tested) */
12429 /* IMX =       Local copy of LV and maximum value of I1 and */
12430 /*               I2 */
12431 /* K =         DO-loop index and LISTV index */
12432 /* K0 =        LISTV index of the first endpoint of the */
12433 /*               boundary edge used to compute Q */
12434 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12435 /*               the boundary traversal:  TRUE iff the vertex */
12436 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12437 /* N =         Local copy of NV */
12438 /* NI =        Number of intersections (between the boundary */
12439 /*               curve and the great circle P-Q) encountered */
12440 /* PINR =      TRUE iff P is to the left of the directed */
12441 /*               boundary edge associated with the closest */
12442 /*               intersection point to P that lies between P */
12443 /*               and Q (a left-to-right intersection as */
12444 /*               viewed from Q), or there is no intersection */
12445 /*               between P and Q (on the shorter arc) */
12446 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12447 /*               locate intersections B relative to arc Q->P */
12448 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12449 /*               the boundary edge indexed by LISTV(K0) -> */
12450 /*               LISTV(K0+1) */
12451 /* QINR =      TRUE iff Q is to the left of the directed */
12452 /*               boundary edge associated with the closest */
12453 /*               intersection point to Q that lies between P */
12454 /*               and Q (a right-to-left intersection as */
12455 /*               viewed from Q), or there is no intersection */
12456 /*               between P and Q (on the shorter arc) */
12457 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12458 /*               compute (normalize) Q */
12459 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12460 /*               traversal */
12461 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12462 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12463 /* VNRM =      Euclidean norm of VN */
12464 
12465     /* Parameter adjustments */
12466     --p;
12467     --zv;
12468     --yv;
12469     --xv;
12470     --listv;
12471 
12472     /* Function Body */
12473 
12474 /* Store local parameters, test for error 1, and initialize */
12475 /*   K0. */
12476 
12477     imx = *lv;
12478     n = *nv;
12479     if (n < 3 || n > imx) {
12480         goto L11;
12481     }
12482     k0 = 0;
12483     i1 = listv[1];
12484     if (i1 < 1 || i1 > imx) {
12485         goto L12;
12486     }
12487 
12488 /* Increment K0 and set Q to a point immediately to the left */
12489 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12490 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12491 
12492 L1:
12493     ++k0;
12494     if (k0 > n) {
12495         goto L14;
12496     }
12497     i1 = listv[k0];
12498     if (k0 < n) {
12499         i2 = listv[k0 + 1];
12500     } else {
12501         i2 = listv[1];
12502     }
12503     if (i2 < 1 || i2 > imx) {
12504         goto L12;
12505     }
12506     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12507     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12508     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12509     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12510     if (vnrm == 0.) {
12511         goto L1;
12512     }
12513     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12514     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12515     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12516     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12517     q[0] /= qnrm;
12518     q[1] /= qnrm;
12519     q[2] /= qnrm;
12520 
12521 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12522 
12523     cn[0] = q[1] * p[3] - q[2] * p[2];
12524     cn[1] = q[2] * p[1] - q[0] * p[3];
12525     cn[2] = q[0] * p[2] - q[1] * p[1];
12526     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12527         goto L1;
12528     }
12529     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12530     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12531     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12532     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12533     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12534     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12535 
12536 /* Initialize parameters for the boundary traversal. */
12537 
12538     ni = 0;
12539     even = TRUE_;
12540     bp = -2.;
12541     bq = -2.;
12542     pinr = TRUE_;
12543     qinr = TRUE_;
12544     i2 = listv[n];
12545     if (i2 < 1 || i2 > imx) {
12546         goto L12;
12547     }
12548     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12549 
12550 /* Loop on boundary arcs I1->I2. */
12551 
12552     i__1 = n;
12553     for (k = 1; k <= i__1; ++k) {
12554         i1 = i2;
12555         lft1 = lft2;
12556         i2 = listv[k];
12557         if (i2 < 1 || i2 > imx) {
12558             goto L12;
12559         }
12560         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12561         if (lft1 == lft2) {
12562             goto L2;
12563         }
12564 
12565 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12566 /*     point of intersection B. */
12567 
12568         ++ni;
12569         v1[0] = xv[i1];
12570         v1[1] = yv[i1];
12571         v1[2] = zv[i1];
12572         v2[0] = xv[i2];
12573         v2[1] = yv[i2];
12574         v2[2] = zv[i2];
12575         intrsc_(v1, v2, cn, b, &ierr);
12576 
12577 /*   B is between Q and P (on the shorter arc) iff */
12578 /*     B Forward Q->P and B Forward P->Q       iff */
12579 /*     <B,QN> > 0 and <B,PN> > 0. */
12580 
12581         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12582                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12583 
12584 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12585 
12586             even = ! even;
12587             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12588             if (d__ > bq) {
12589                 bq = d__;
12590                 qinr = lft2;
12591             }
12592             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12593             if (d__ > bp) {
12594                 bp = d__;
12595                 pinr = lft1;
12596             }
12597         }
12598 L2:
12599         ;
12600     }
12601 
12602 /* Test for consistency:  NI must be even and QINR must be */
12603 /*   TRUE. */
12604 
12605     if (ni != ni / 2 << 1 || ! qinr) {
12606         goto L1;
12607     }
12608 
12609 /* Test for error 3:  different values of PINR and EVEN. */
12610 
12611     if (pinr != even) {
12612         goto L13;
12613     }
12614 
12615 /* No error encountered. */
12616 
12617     *ier = 0;
12618     ret_val = even;
12619     return ret_val;
12620 
12621 /* LV or NV is outside its valid range. */
12622 
12623 L11:
12624     *ier = 1;
12625     return ret_val;
12626 
12627 /* A LISTV entry is outside its valid range. */
12628 
12629 L12:
12630     *ier = 2;
12631     return ret_val;
12632 
12633 /* The polygon boundary is self-intersecting. */
12634 
12635 L13:
12636     *ier = 3;
12637     return ret_val;
12638 
12639 /* Consistency tests failed for all values of Q. */
12640 
12641 L14:
12642     *ier = 4;
12643     return ret_val;
12644 } /* inside_ */
12645 
12646 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12647         i3, int *list, int *lptr, int *lend, int *lnew)
12648 {
12649     static int k, n1, n2, n3, lp;
12650     extern /* Subroutine */ int insert_(int *, int *, int *,
12651             int *, int *);
12652     extern int lstptr_(int *, int *, int *, int *);
12653 
12654 
12655 /* *********************************************************** */
12656 
12657 /*                                              From STRIPACK */
12658 /*                                            Robert J. Renka */
12659 /*                                  Dept. of Computer Science */
12660 /*                                       Univ. of North Texas */
12661 /*                                           renka@cs.unt.edu */
12662 /*                                                   07/17/96 */
12663 
12664 /*   This subroutine adds an interior node to a triangulation */
12665 /* of a set of points on the unit sphere.  The data structure */
12666 /* is updated with the insertion of node KK into the triangle */
12667 /* whose vertices are I1, I2, and I3.  No optimization of the */
12668 /* triangulation is performed. */
12669 
12670 /*   This routine is identical to the similarly named routine */
12671 /* in TRIPACK. */
12672 
12673 
12674 /* On input: */
12675 
12676 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12677 /*            and KK must not be equal to I1, I2, or I3. */
12678 
12679 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12680 /*                  sequence of vertices of a triangle which */
12681 /*                  contains node KK. */
12682 
12683 /* The above parameters are not altered by this routine. */
12684 
12685 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12686 /*                             triangulation.  Refer to Sub- */
12687 /*                             routine TRMESH.  Triangle */
12688 /*                             (I1,I2,I3) must be included */
12689 /*                             in the triangulation. */
12690 
12691 /* On output: */
12692 
12693 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12694 /*                             the addition of node KK.  KK */
12695 /*                             will be connected to nodes I1, */
12696 /*                             I2, and I3. */
12697 
12698 /* Modules required by INTADD:  INSERT, LSTPTR */
12699 
12700 /* *********************************************************** */
12701 
12702 
12703 /* Local parameters: */
12704 
12705 /* K =        Local copy of KK */
12706 /* LP =       LIST pointer */
12707 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12708 
12709     /* Parameter adjustments */
12710     --lend;
12711     --lptr;
12712     --list;
12713 
12714     /* Function Body */
12715     k = *kk;
12716 
12717 /* Initialization. */
12718 
12719     n1 = *i1;
12720     n2 = *i2;
12721     n3 = *i3;
12722 
12723 /* Add K as a neighbor of I1, I2, and I3. */
12724 
12725     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12726     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12727     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12728     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12729     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12730     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12731 
12732 /* Add I1, I2, and I3 as neighbors of K. */
12733 
12734     list[*lnew] = n1;
12735     list[*lnew + 1] = n2;
12736     list[*lnew + 2] = n3;
12737     lptr[*lnew] = *lnew + 1;
12738     lptr[*lnew + 1] = *lnew + 2;
12739     lptr[*lnew + 2] = *lnew;
12740     lend[k] = *lnew + 2;
12741     *lnew += 3;
12742     return 0;
12743 } /* intadd_ */
12744 
12745 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12746         double *p, int *ier)
12747 {
12748     /* Builtin functions */
12749     //double sqrt(double);
12750 
12751     /* Local variables */
12752     static int i__;
12753     static double t, d1, d2, pp[3], ppn;
12754 
12755 
12756 /* *********************************************************** */
12757 
12758 /*                                              From STRIPACK */
12759 /*                                            Robert J. Renka */
12760 /*                                  Dept. of Computer Science */
12761 /*                                       Univ. of North Texas */
12762 /*                                           renka@cs.unt.edu */
12763 /*                                                   07/19/90 */
12764 
12765 /*   Given a great circle C and points P1 and P2 defining an */
12766 /* arc A on the surface of the unit sphere, where A is the */
12767 /* shorter of the two portions of the great circle C12 assoc- */
12768 /* iated with P1 and P2, this subroutine returns the point */
12769 /* of intersection P between C and C12 that is closer to A. */
12770 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12771 /* C, P is the point of intersection of C with A. */
12772 
12773 
12774 /* On input: */
12775 
12776 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
12777 /*               coordinates of unit vectors. */
12778 
12779 /*       CN = Array of length 3 containing the Cartesian */
12780 /*            coordinates of a nonzero vector which defines C */
12781 /*            as the intersection of the plane whose normal */
12782 /*            is CN with the unit sphere.  Thus, if C is to */
12783 /*            be the great circle defined by P and Q, CN */
12784 /*            should be P X Q. */
12785 
12786 /* The above parameters are not altered by this routine. */
12787 
12788 /*       P = Array of length 3. */
12789 
12790 /* On output: */
12791 
12792 /*       P = Point of intersection defined above unless IER */
12793 /*           .NE. 0, in which case P is not altered. */
12794 
12795 /*       IER = Error indicator. */
12796 /*             IER = 0 if no errors were encountered. */
12797 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
12798 /*                     iff P1 = P2 or CN = 0 or there are */
12799 /*                     two intersection points at the same */
12800 /*                     distance from A. */
12801 /*             IER = 2 if P2 = -P1 and the definition of A is */
12802 /*                     therefore ambiguous. */
12803 
12804 /* Modules required by INTRSC:  None */
12805 
12806 /* Intrinsic function called by INTRSC:  SQRT */
12807 
12808 /* *********************************************************** */
12809 
12810 
12811 /* Local parameters: */
12812 
12813 /* D1 =  <CN,P1> */
12814 /* D2 =  <CN,P2> */
12815 /* I =   DO-loop index */
12816 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
12817 /*         line defined by P1 and P2 */
12818 /* PPN = Norm of PP */
12819 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
12820 /*         in the plane of C */
12821 
12822     /* Parameter adjustments */
12823     --p;
12824     --cn;
12825     --p2;
12826     --p1;
12827 
12828     /* Function Body */
12829     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12830     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12831 
12832     if (d1 == d2) {
12833         *ier = 1;
12834         return 0;
12835     }
12836 
12837 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
12838 
12839     t = d1 / (d1 - d2);
12840     ppn = 0.;
12841     for (i__ = 1; i__ <= 3; ++i__) {
12842         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12843         ppn += pp[i__ - 1] * pp[i__ - 1];
12844 /* L1: */
12845     }
12846 
12847 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
12848 
12849     if (ppn == 0.) {
12850         *ier = 2;
12851         return 0;
12852     }
12853     ppn = sqrt(ppn);
12854 
12855 /* Compute P = PP/PPN. */
12856 
12857     for (i__ = 1; i__ <= 3; ++i__) {
12858         p[i__] = pp[i__ - 1] / ppn;
12859 /* L2: */
12860     }
12861     *ier = 0;
12862     return 0;
12863 } /* intrsc_ */
12864 
12865 int jrand_(int *n, int *ix, int *iy, int *iz)
12866 {
12867     /* System generated locals */
12868     int ret_val;
12869 
12870     /* Local variables */
12871     static float u, x;
12872 
12873 
12874 /* *********************************************************** */
12875 
12876 /*                                              From STRIPACK */
12877 /*                                            Robert J. Renka */
12878 /*                                  Dept. of Computer Science */
12879 /*                                       Univ. of North Texas */
12880 /*                                           renka@cs.unt.edu */
12881 /*                                                   07/28/98 */
12882 
12883 /*   This function returns a uniformly distributed pseudo- */
12884 /* random int in the range 1 to N. */
12885 
12886 
12887 /* On input: */
12888 
12889 /*       N = Maximum value to be returned. */
12890 
12891 /* N is not altered by this function. */
12892 
12893 /*       IX,IY,IZ = int seeds initialized to values in */
12894 /*                  the range 1 to 30,000 before the first */
12895 /*                  call to JRAND, and not altered between */
12896 /*                  subsequent calls (unless a sequence of */
12897 /*                  random numbers is to be repeated by */
12898 /*                  reinitializing the seeds). */
12899 
12900 /* On output: */
12901 
12902 /*       IX,IY,IZ = Updated int seeds. */
12903 
12904 /*       JRAND = Random int in the range 1 to N. */
12905 
12906 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
12907 /*             and Portable Pseudo-random Number Generator", */
12908 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
12909 /*             pp. 188-190. */
12910 
12911 /* Modules required by JRAND:  None */
12912 
12913 /* Intrinsic functions called by JRAND:  INT, MOD, float */
12914 
12915 /* *********************************************************** */
12916 
12917 
12918 /* Local parameters: */
12919 
12920 /* U = Pseudo-random number uniformly distributed in the */
12921 /*     interval (0,1). */
12922 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
12923 /*       tional part is U. */
12924 
12925     *ix = *ix * 171 % 30269;
12926     *iy = *iy * 172 % 30307;
12927     *iz = *iz * 170 % 30323;
12928     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
12929             30323.f;
12930     u = x - (int) x;
12931     ret_val = (int) ((float) (*n) * u + 1.f);
12932     return ret_val;
12933 } /* jrand_ */
12934 
12935 long int left_(double *x1, double *y1, double *z1, double *x2,
12936         double *y2, double *z2, double *x0, double *y0,
12937         double *z0)
12938 {
12939     /* System generated locals */
12940     long int ret_val;
12941 
12942 
12943 /* *********************************************************** */
12944 
12945 /*                                              From STRIPACK */
12946 /*                                            Robert J. Renka */
12947 /*                                  Dept. of Computer Science */
12948 /*                                       Univ. of North Texas */
12949 /*                                           renka@cs.unt.edu */
12950 /*                                                   07/15/96 */
12951 
12952 /*   This function determines whether node N0 is in the */
12953 /* (closed) left hemisphere defined by the plane containing */
12954 /* N1, N2, and the origin, where left is defined relative to */
12955 /* an observer at N1 facing N2. */
12956 
12957 
12958 /* On input: */
12959 
12960 /*       X1,Y1,Z1 = Coordinates of N1. */
12961 
12962 /*       X2,Y2,Z2 = Coordinates of N2. */
12963 
12964 /*       X0,Y0,Z0 = Coordinates of N0. */
12965 
12966 /* Input parameters are not altered by this function. */
12967 
12968 /* On output: */
12969 
12970 /*       LEFT = TRUE if and only if N0 is in the closed */
12971 /*              left hemisphere. */
12972 
12973 /* Modules required by LEFT:  None */
12974 
12975 /* *********************************************************** */
12976 
12977 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
12978 
12979     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
12980             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
12981 
12982 
12983     return ret_val;
12984 } /* left_ */
12985 
12986 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
12987 {
12988     /* System generated locals */
12989     int ret_val;
12990 
12991     /* Local variables */
12992     static int nd, lp;
12993 
12994 
12995 /* *********************************************************** */
12996 
12997 /*                                              From STRIPACK */
12998 /*                                            Robert J. Renka */
12999 /*                                  Dept. of Computer Science */
13000 /*                                       Univ. of North Texas */
13001 /*                                           renka@cs.unt.edu */
13002 /*                                                   07/15/96 */
13003 
13004 /*   This function returns the index (LIST pointer) of NB in */
13005 /* the adjacency list for N0, where LPL = LEND(N0). */
13006 
13007 /*   This function is identical to the similarly named */
13008 /* function in TRIPACK. */
13009 
13010 
13011 /* On input: */
13012 
13013 /*       LPL = LEND(N0) */
13014 
13015 /*       NB = Index of the node whose pointer is to be re- */
13016 /*            turned.  NB must be connected to N0. */
13017 
13018 /*       LIST,LPTR = Data structure defining the triangula- */
13019 /*                   tion.  Refer to Subroutine TRMESH. */
13020 
13021 /* Input parameters are not altered by this function. */
13022 
13023 /* On output: */
13024 
13025 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13026 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13027 /*                neighbor of N0, in which case LSTPTR = LPL. */
13028 
13029 /* Modules required by LSTPTR:  None */
13030 
13031 /* *********************************************************** */
13032 
13033 
13034 /* Local parameters: */
13035 
13036 /* LP = LIST pointer */
13037 /* ND = Nodal index */
13038 
13039     /* Parameter adjustments */
13040     --lptr;
13041     --list;
13042 
13043     /* Function Body */
13044     lp = lptr[*lpl];
13045 L1:
13046     nd = list[lp];
13047     if (nd == *nb) {
13048         goto L2;
13049     }
13050     lp = lptr[lp];
13051     if (lp != *lpl) {
13052         goto L1;
13053     }
13054 
13055 L2:
13056     ret_val = lp;
13057     return ret_val;
13058 } /* lstptr_ */
13059 
13060 int nbcnt_(int *lpl, int *lptr)
13061 {
13062     /* System generated locals */
13063     int ret_val;
13064 
13065     /* Local variables */
13066     static int k, 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 number of neighbors of a node */
13079 /* N0 in a triangulation created by Subroutine TRMESH. */
13080 
13081 /*   This function is identical to the similarly named */
13082 /* function in TRIPACK. */
13083 
13084 
13085 /* On input: */
13086 
13087 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13088 /*             LPL = LEND(N0). */
13089 
13090 /*       LPTR = Array of pointers associated with LIST. */
13091 
13092 /* Input parameters are not altered by this function. */
13093 
13094 /* On output: */
13095 
13096 /*       NBCNT = Number of neighbors of N0. */
13097 
13098 /* Modules required by NBCNT:  None */
13099 
13100 /* *********************************************************** */
13101 
13102 
13103 /* Local parameters: */
13104 
13105 /* K =  Counter for computing the number of neighbors */
13106 /* LP = LIST pointer */
13107 
13108     /* Parameter adjustments */
13109     --lptr;
13110 
13111     /* Function Body */
13112     lp = *lpl;
13113     k = 1;
13114 
13115 L1:
13116     lp = lptr[lp];
13117     if (lp == *lpl) {
13118         goto L2;
13119     }
13120     ++k;
13121     goto L1;
13122 
13123 L2:
13124     ret_val = k;
13125     return ret_val;
13126 } /* nbcnt_ */
13127 
13128 int nearnd_(double *p, int *ist, int *n, double *x,
13129         double *y, double *z__, int *list, int *lptr, int
13130         *lend, double *al)
13131 {
13132     /* System generated locals */
13133     int ret_val, i__1;
13134 
13135     /* Builtin functions */
13136     //double acos(double);
13137 
13138     /* Local variables */
13139     static int l;
13140     static double b1, b2, b3;
13141     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13142     static double ds1;
13143     static int lp1, lp2;
13144     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13145     static int lpl;
13146     static double dsr;
13147     static int nst, listp[25], lptrp[25];
13148     extern /* Subroutine */ int trfind_(int *, double *, int *,
13149             double *, double *, double *, int *, int *,
13150             int *, double *, double *, double *, int *,
13151             int *, int *);
13152     extern int lstptr_(int *, int *, int *, int *);
13153 
13154 
13155 /* *********************************************************** */
13156 
13157 /*                                              From STRIPACK */
13158 /*                                            Robert J. Renka */
13159 /*                                  Dept. of Computer Science */
13160 /*                                       Univ. of North Texas */
13161 /*                                           renka@cs.unt.edu */
13162 /*                                                   07/28/98 */
13163 
13164 /*   Given a point P on the surface of the unit sphere and a */
13165 /* Delaunay triangulation created by Subroutine TRMESH, this */
13166 /* function returns the index of the nearest triangulation */
13167 /* node to P. */
13168 
13169 /*   The algorithm consists of implicitly adding P to the */
13170 /* triangulation, finding the nearest neighbor to P, and */
13171 /* implicitly deleting P from the triangulation.  Thus, it */
13172 /* is based on the fact that, if P is a node in a Delaunay */
13173 /* triangulation, the nearest node to P is a neighbor of P. */
13174 
13175 
13176 /* On input: */
13177 
13178 /*       P = Array of length 3 containing the Cartesian coor- */
13179 /*           dinates of the point P to be located relative to */
13180 /*           the triangulation.  It is assumed without a test */
13181 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13182 
13183 /*       IST = Index of a node at which TRFIND begins the */
13184 /*             search.  Search time depends on the proximity */
13185 /*             of this node to P. */
13186 
13187 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13188 
13189 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13190 /*               coordinates of the nodes. */
13191 
13192 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13193 /*                        gulation.  Refer to TRMESH. */
13194 
13195 /* Input parameters are not altered by this function. */
13196 
13197 /* On output: */
13198 
13199 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13200 /*                if N < 3 or the triangulation data struc- */
13201 /*                ture is invalid. */
13202 
13203 /*       AL = Arc length (angular distance in radians) be- */
13204 /*            tween P and NEARND unless NEARND = 0. */
13205 
13206 /*       Note that the number of candidates for NEARND */
13207 /*       (neighbors of P) is limited to LMAX defined in */
13208 /*       the PARAMETER statement below. */
13209 
13210 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13211 
13212 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13213 
13214 /* *********************************************************** */
13215 
13216 
13217 /* Local parameters: */
13218 
13219 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13220 /*               by TRFIND */
13221 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13222 /* DSR =       (Negative cosine of the) distance from P to NR */
13223 /* DX1,..DZ3 = Components of vectors used by the swap test */
13224 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13225 /*               the rightmost (I1) and leftmost (I2) visible */
13226 /*               boundary nodes as viewed from P */
13227 /* L =         Length of LISTP/LPTRP and number of neighbors */
13228 /*               of P */
13229 /* LMAX =      Maximum value of L */
13230 /* LISTP =     Indexes of the neighbors of P */
13231 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13232 /*               LISTP elements */
13233 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13234 /*               pointer */
13235 /* LP1,LP2 =   LISTP indexes (pointers) */
13236 /* LPL =       Pointer to the last neighbor of N1 */
13237 /* N1 =        Index of a node visible from P */
13238 /* N2 =        Index of an endpoint of an arc opposite P */
13239 /* N3 =        Index of the node opposite N1->N2 */
13240 /* NN =        Local copy of N */
13241 /* NR =        Index of a candidate for the nearest node to P */
13242 /* NST =       Index of the node at which TRFIND begins the */
13243 /*               search */
13244 
13245 
13246 /* Store local parameters and test for N invalid. */
13247 
13248     /* Parameter adjustments */
13249     --p;
13250     --lend;
13251     --z__;
13252     --y;
13253     --x;
13254     --list;
13255     --lptr;
13256 
13257     /* Function Body */
13258     nn = *n;
13259     if (nn < 3) {
13260         goto L6;
13261     }
13262     nst = *ist;
13263     if (nst < 1 || nst > nn) {
13264         nst = 1;
13265     }
13266 
13267 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13268 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13269 /*   from P. */
13270 
13271     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13272             1], &b1, &b2, &b3, &i1, &i2, &i3);
13273 
13274 /* Test for collinear nodes. */
13275 
13276     if (i1 == 0) {
13277         goto L6;
13278     }
13279 
13280 /* Store the linked list of 'neighbors' of P in LISTP and */
13281 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13282 /*   the last neighbor if P is not contained in a triangle. */
13283 /*   L is the length of LISTP and LPTRP, and is limited to */
13284 /*   LMAX. */
13285 
13286     if (i3 != 0) {
13287         listp[0] = i1;
13288         lptrp[0] = 2;
13289         listp[1] = i2;
13290         lptrp[1] = 3;
13291         listp[2] = i3;
13292         lptrp[2] = 1;
13293         l = 3;
13294     } else {
13295         n1 = i1;
13296         l = 1;
13297         lp1 = 2;
13298         listp[l - 1] = n1;
13299         lptrp[l - 1] = lp1;
13300 
13301 /*   Loop on the ordered sequence of visible boundary nodes */
13302 /*     N1 from I1 to I2. */
13303 
13304 L1:
13305         lpl = lend[n1];
13306         n1 = -list[lpl];
13307         l = lp1;
13308         lp1 = l + 1;
13309         listp[l - 1] = n1;
13310         lptrp[l - 1] = lp1;
13311         if (n1 != i2 && lp1 < 25) {
13312             goto L1;
13313         }
13314         l = lp1;
13315         listp[l - 1] = 0;
13316         lptrp[l - 1] = 1;
13317     }
13318 
13319 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13320 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13321 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13322 /*   indexes of N1 and N2. */
13323 
13324     lp2 = 1;
13325     n2 = i1;
13326     lp1 = lptrp[0];
13327     n1 = listp[lp1 - 1];
13328 
13329 /* Begin loop:  find the node N3 opposite N1->N2. */
13330 
13331 L2:
13332     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13333     if (list[lp] < 0) {
13334         goto L3;
13335     }
13336     lp = lptr[lp];
13337     n3 = (i__1 = list[lp], abs(i__1));
13338 
13339 /* Swap test:  Exit the loop if L = LMAX. */
13340 
13341     if (l == 25) {
13342         goto L4;
13343     }
13344     dx1 = x[n1] - p[1];
13345     dy1 = y[n1] - p[2];
13346     dz1 = z__[n1] - p[3];
13347 
13348     dx2 = x[n2] - p[1];
13349     dy2 = y[n2] - p[2];
13350     dz2 = z__[n2] - p[3];
13351 
13352     dx3 = x[n3] - p[1];
13353     dy3 = y[n3] - p[2];
13354     dz3 = z__[n3] - p[3];
13355     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13356             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13357         goto L3;
13358     }
13359 
13360 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13361 /*        The two new arcs opposite P must be tested. */
13362 
13363     ++l;
13364     lptrp[lp2 - 1] = l;
13365     listp[l - 1] = n3;
13366     lptrp[l - 1] = lp1;
13367     lp1 = l;
13368     n1 = n3;
13369     goto L2;
13370 
13371 /* No swap:  Advance to the next arc and test for termination */
13372 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13373 
13374 L3:
13375     if (lp1 == 1) {
13376         goto L4;
13377     }
13378     lp2 = lp1;
13379     n2 = n1;
13380     lp1 = lptrp[lp1 - 1];
13381     n1 = listp[lp1 - 1];
13382     if (n1 == 0) {
13383         goto L4;
13384     }
13385     goto L2;
13386 
13387 /* Set NR and DSR to the index of the nearest node to P and */
13388 /*   an increasing function (negative cosine) of its distance */
13389 /*   from P, respectively. */
13390 
13391 L4:
13392     nr = i1;
13393     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13394     i__1 = l;
13395     for (lp = 2; lp <= i__1; ++lp) {
13396         n1 = listp[lp - 1];
13397         if (n1 == 0) {
13398             goto L5;
13399         }
13400         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13401         if (ds1 < dsr) {
13402             nr = n1;
13403             dsr = ds1;
13404         }
13405 L5:
13406         ;
13407     }
13408     dsr = -dsr;
13409     if (dsr > 1.) {
13410         dsr = 1.;
13411     }
13412     *al = acos(dsr);
13413     ret_val = nr;
13414     return ret_val;
13415 
13416 /* Invalid input. */
13417 
13418 L6:
13419     ret_val = 0;
13420     return ret_val;
13421 } /* nearnd_ */
13422 
13423 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13424         int *na, int *list, int *lptr, int *lend, int *
13425         nit, int *iwk, int *ier)
13426 {
13427     /* System generated locals */
13428     int i__1, i__2;
13429 
13430     /* Local variables */
13431     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13432     static long int swp;
13433     static int iter;
13434     extern /* Subroutine */ int swap_(int *, int *, int *,
13435             int *, int *, int *, int *, int *);
13436     static int maxit;
13437     extern long int swptst_(int *, int *, int *, int *,
13438             double *, double *, double *);
13439 
13440 
13441 /* *********************************************************** */
13442 
13443 /*                                              From STRIPACK */
13444 /*                                            Robert J. Renka */
13445 /*                                  Dept. of Computer Science */
13446 /*                                       Univ. of North Texas */
13447 /*                                           renka@cs.unt.edu */
13448 /*                                                   07/30/98 */
13449 
13450 /*   Given a set of NA triangulation arcs, this subroutine */
13451 /* optimizes the portion of the triangulation consisting of */
13452 /* the quadrilaterals (pairs of adjacent triangles) which */
13453 /* have the arcs as diagonals by applying the circumcircle */
13454 /* test and appropriate swaps to the arcs. */
13455 
13456 /*   An iteration consists of applying the swap test and */
13457 /* swaps to all NA arcs in the order in which they are */
13458 /* stored.  The iteration is repeated until no swap occurs */
13459 /* or NIT iterations have been performed.  The bound on the */
13460 /* number of iterations may be necessary to prevent an */
13461 /* infinite loop caused by cycling (reversing the effect of a */
13462 /* previous swap) due to floating point inaccuracy when four */
13463 /* or more nodes are nearly cocircular. */
13464 
13465 
13466 /* On input: */
13467 
13468 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13469 
13470 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13471 
13472 /* The above parameters are not altered by this routine. */
13473 
13474 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13475 /*                        gulation.  Refer to Subroutine */
13476 /*                        TRMESH. */
13477 
13478 /*       NIT = Maximum number of iterations to be performed. */
13479 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13480 
13481 /*       IWK = int array dimensioned 2 by NA containing */
13482 /*             the nodal indexes of the arc endpoints (pairs */
13483 /*             of endpoints are stored in columns). */
13484 
13485 /* On output: */
13486 
13487 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13488 /*                        ture reflecting the swaps. */
13489 
13490 /*       NIT = Number of iterations performed. */
13491 
13492 /*       IWK = Endpoint indexes of the new set of arcs */
13493 /*             reflecting the swaps. */
13494 
13495 /*       IER = Error indicator: */
13496 /*             IER = 0 if no errors were encountered. */
13497 /*             IER = 1 if a swap occurred on the last of */
13498 /*                     MAXIT iterations, where MAXIT is the */
13499 /*                     value of NIT on input.  The new set */
13500 /*                     of arcs is not necessarily optimal */
13501 /*                     in this case. */
13502 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13503 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13504 /*                     IWK(1,I) for some I in the range 1 */
13505 /*                     to NA.  A swap may have occurred in */
13506 /*                     this case. */
13507 /*             IER = 4 if a zero pointer was returned by */
13508 /*                     Subroutine SWAP. */
13509 
13510 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13511 
13512 /* Intrinsic function called by OPTIM:  ABS */
13513 
13514 /* *********************************************************** */
13515 
13516 
13517 /* Local parameters: */
13518 
13519 /* I =       Column index for IWK */
13520 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13521 /* ITER =    Iteration count */
13522 /* LP =      LIST pointer */
13523 /* LP21 =    Parameter returned by SWAP (not used) */
13524 /* LPL =     Pointer to the last neighbor of IO1 */
13525 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13526 /*             of IO1 */
13527 /* MAXIT =   Input value of NIT */
13528 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13529 /*             respectively */
13530 /* NNA =     Local copy of NA */
13531 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13532 /*             optimization loop */
13533 
13534     /* Parameter adjustments */
13535     --x;
13536     --y;
13537     --z__;
13538     iwk -= 3;
13539     --list;
13540     --lptr;
13541     --lend;
13542 
13543     /* Function Body */
13544     nna = *na;
13545     maxit = *nit;
13546     if (nna < 0 || maxit < 1) {
13547         goto L7;
13548     }
13549 
13550 /* Initialize iteration count ITER and test for NA = 0. */
13551 
13552     iter = 0;
13553     if (nna == 0) {
13554         goto L5;
13555     }
13556 
13557 /* Top of loop -- */
13558 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13559 
13560 L1:
13561     if (iter == maxit) {
13562         goto L6;
13563     }
13564     ++iter;
13565     swp = FALSE_;
13566 
13567 /*   Inner loop on arcs IO1-IO2 -- */
13568 
13569     i__1 = nna;
13570     for (i__ = 1; i__ <= i__1; ++i__) {
13571         io1 = iwk[(i__ << 1) + 1];
13572         io2 = iwk[(i__ << 1) + 2];
13573 
13574 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13575 /*     IO2->IO1, respectively.  Determine the following: */
13576 
13577 /*     LPL = pointer to the last neighbor of IO1, */
13578 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13579 /*     LPP = pointer to the node N2 preceding IO2. */
13580 
13581         lpl = lend[io1];
13582         lpp = lpl;
13583         lp = lptr[lpp];
13584 L2:
13585         if (list[lp] == io2) {
13586             goto L3;
13587         }
13588         lpp = lp;
13589         lp = lptr[lpp];
13590         if (lp != lpl) {
13591             goto L2;
13592         }
13593 
13594 /*   IO2 should be the last neighbor of IO1.  Test for no */
13595 /*     arc and bypass the swap test if IO1 is a boundary */
13596 /*     node. */
13597 
13598         if ((i__2 = list[lp], abs(i__2)) != io2) {
13599             goto L8;
13600         }
13601         if (list[lp] < 0) {
13602             goto L4;
13603         }
13604 
13605 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13606 /*     boundary node and IO2 is its first neighbor. */
13607 
13608 L3:
13609         n2 = list[lpp];
13610         if (n2 < 0) {
13611             goto L4;
13612         }
13613         lp = lptr[lp];
13614         n1 = (i__2 = list[lp], abs(i__2));
13615 
13616 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13617 
13618         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13619             goto L4;
13620         }
13621         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13622         if (lp21 == 0) {
13623             goto L9;
13624         }
13625         swp = TRUE_;
13626         iwk[(i__ << 1) + 1] = n1;
13627         iwk[(i__ << 1) + 2] = n2;
13628 L4:
13629         ;
13630     }
13631     if (swp) {
13632         goto L1;
13633     }
13634 
13635 /* Successful termination. */
13636 
13637 L5:
13638     *nit = iter;
13639     *ier = 0;
13640     return 0;
13641 
13642 /* MAXIT iterations performed without convergence. */
13643 
13644 L6:
13645     *nit = maxit;
13646     *ier = 1;
13647     return 0;
13648 
13649 /* Invalid input parameter. */
13650 
13651 L7:
13652     *nit = 0;
13653     *ier = 2;
13654     return 0;
13655 
13656 /* IO2 is not a neighbor of IO1. */
13657 
13658 L8:
13659     *nit = iter;
13660     *ier = 3;
13661     return 0;
13662 
13663 /* Zero pointer returned by SWAP. */
13664 
13665 L9:
13666     *nit = iter;
13667     *ier = 4;
13668     return 0;
13669 } /* optim_ */
13670 
13671 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13672         double *ox, double *oy, double *oz, double *ex,
13673         double *ey, double *ez, double *vx, double *vy,
13674         double *vz, long int *init, double *x, double *y,
13675         double *z__, int *ier)
13676 {
13677     /* Builtin functions */
13678     //double sqrt(double);
13679 
13680     /* Local variables */
13681     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13682             oes, xoe, yoe, zoe, xep, yep, zep;
13683 
13684 
13685 /* *********************************************************** */
13686 
13687 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13688 /*                                            Robert J. Renka */
13689 /*                                  Dept. of Computer Science */
13690 /*                                       Univ. of North Texas */
13691 /*                                           renka@cs.unt.edu */
13692 /*                                                   07/18/90 */
13693 
13694 /*   Given a projection plane and associated coordinate sys- */
13695 /* tem defined by an origin O, eye position E, and up-vector */
13696 /* V, this subroutine applies a perspective depth transform- */
13697 /* ation T to a point P = (PX,PY,PZ), returning the point */
13698 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13699 /* coordinates of the point that lies in the projection */
13700 /* plane and on the line defined by P and E, and Z is the */
13701 /* depth associated with P. */
13702 
13703 /*   The projection plane is defined to be the plane that */
13704 /* contains O and has normal defined by O and E. */
13705 
13706 /*   The depth Z is defined in such a way that Z < 1, T maps */
13707 /* lines to lines (and planes to planes), and if two distinct */
13708 /* points have the same projection plane coordinates, then */
13709 /* the one closer to E has a smaller depth.  (Z increases */
13710 /* monotonically with orthogonal distance from P to the plane */
13711 /* that is parallel to the projection plane and contains E.) */
13712 /* This depth value facilitates depth sorting and depth buf- */
13713 /* fer methods. */
13714 
13715 
13716 /* On input: */
13717 
13718 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13719 /*                  be mapped onto the projection plane.  The */
13720 /*                  half line that contains P and has end- */
13721 /*                  point at E must intersect the plane. */
13722 
13723 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13724 /*                  nate system in the projection plane).  A */
13725 /*                  reasonable value for O is a point near */
13726 /*                  the center of an object or scene to be */
13727 /*                  viewed. */
13728 
13729 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13730 /*                  ing the normal to the plane and the line */
13731 /*                  of sight for the projection.  E must not */
13732 /*                  coincide with O or P, and the angle be- */
13733 /*                  tween the vectors O-E and P-E must be */
13734 /*                  less than 90 degrees.  Note that E and P */
13735 /*                  may lie on opposite sides of the projec- */
13736 /*                  tion plane. */
13737 
13738 /*       VX,VY,VZ = Coordinates of a point V which defines */
13739 /*                  the positive Y axis of an X-Y coordinate */
13740 /*                  system in the projection plane as the */
13741 /*                  half-line containing O and the projection */
13742 /*                  of O+V onto the plane.  The positive X */
13743 /*                  axis has direction defined by the cross */
13744 /*                  product V X (E-O). */
13745 
13746 /* The above parameters are not altered by this routine. */
13747 
13748 /*       INIT = long int switch which must be set to TRUE on */
13749 /*              the first call and when the values of O, E, */
13750 /*              or V have been altered since a previous call. */
13751 /*              If INIT = FALSE, it is assumed that only the */
13752 /*              coordinates of P have changed since a previ- */
13753 /*              ous call.  Previously stored quantities are */
13754 /*              used for increased efficiency in this case. */
13755 
13756 /* On output: */
13757 
13758 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13759 
13760 /*       X,Y = Projection plane coordinates of the point */
13761 /*             that lies in the projection plane and on the */
13762 /*             line defined by E and P.  X and Y are not */
13763 /*             altered if IER .NE. 0. */
13764 
13765 /*       Z = Depth value defined above unless IER .NE. 0. */
13766 
13767 /*       IER = Error indicator. */
13768 /*             IER = 0 if no errors were encountered. */
13769 /*             IER = 1 if the inner product of O-E with P-E */
13770 /*                     is not positive, implying that E is */
13771 /*                     too close to the plane. */
13772 /*             IER = 2 if O, E, and O+V are collinear.  See */
13773 /*                     the description of VX,VY,VZ. */
13774 
13775 /* Modules required by PROJCT:  None */
13776 
13777 /* Intrinsic function called by PROJCT:  SQRT */
13778 
13779 /* *********************************************************** */
13780 
13781 
13782 /* Local parameters: */
13783 
13784 /* OES =         Norm squared of OE -- inner product (OE,OE) */
13785 /* S =           Scale factor for computing projections */
13786 /* SC =          Scale factor for normalizing VN and HN */
13787 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
13788 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
13789 /* XH,YH,ZH =    Components of a unit vector HN defining the */
13790 /*                 positive X-axis in the plane */
13791 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
13792 /* XV,YV,ZV =    Components of a unit vector VN defining the */
13793 /*                 positive Y-axis in the plane */
13794 /* XW,YW,ZW =    Components of the vector W from O to the */
13795 /*                 projection of P onto the plane */
13796 
13797     if (*init) {
13798 
13799 /* Compute parameters defining the transformation: */
13800 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
13801 /*   2 square roots. */
13802 
13803 /* Set the coordinates of E to local variables, compute */
13804 /*   OE = E-O and OES, and test for OE = 0. */
13805 
13806         xe = *ex;
13807         ye = *ey;
13808         ze = *ez;
13809         xoe = xe - *ox;
13810         yoe = ye - *oy;
13811         zoe = ze - *oz;
13812         oes = xoe * xoe + yoe * yoe + zoe * zoe;
13813         if (oes == 0.) {
13814             goto L1;
13815         }
13816 
13817 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
13818 
13819         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13820         xv = *vx - s * xoe;
13821         yv = *vy - s * yoe;
13822         zv = *vz - s * zoe;
13823 
13824 /* Normalize VN to a unit vector. */
13825 
13826         sc = xv * xv + yv * yv + zv * zv;
13827         if (sc == 0.) {
13828             goto L2;
13829         }
13830         sc = 1. / sqrt(sc);
13831         xv = sc * xv;
13832         yv = sc * yv;
13833         zv = sc * zv;
13834 
13835 /* Compute HN = VN X OE (normalized). */
13836 
13837         xh = yv * zoe - yoe * zv;
13838         yh = xoe * zv - xv * zoe;
13839         zh = xv * yoe - xoe * yv;
13840         sc = sqrt(xh * xh + yh * yh + zh * zh);
13841         if (sc == 0.) {
13842             goto L2;
13843         }
13844         sc = 1. / sc;
13845         xh = sc * xh;
13846         yh = sc * yh;
13847         zh = sc * zh;
13848     }
13849 
13850 /* Apply the transformation:  13 adds, 12 multiplies, */
13851 /*                            1 divide, and 1 compare. */
13852 
13853 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
13854 
13855     xep = *px - xe;
13856     yep = *py - ye;
13857     zep = *pz - ze;
13858     s = xoe * xep + yoe * yep + zoe * zep;
13859     if (s >= 0.) {
13860         goto L1;
13861     }
13862     s = oes / s;
13863     xw = xoe - s * xep;
13864     yw = yoe - s * yep;
13865     zw = zoe - s * zep;
13866 
13867 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
13868 /*   reset INIT. */
13869 
13870     *x = xw * xh + yw * yh + zw * zh;
13871     *y = xw * xv + yw * yv + zw * zv;
13872     *z__ = s + 1.;
13873     *init = FALSE_;
13874     *ier = 0;
13875     return 0;
13876 
13877 /* (OE,EP) .GE. 0. */
13878 
13879 L1:
13880     *ier = 1;
13881     return 0;
13882 
13883 /* O, E, and O+V are collinear. */
13884 
13885 L2:
13886     *ier = 2;
13887     return 0;
13888 } /* projct_ */
13889 
13890 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
13891         double *plat, double *plon, double *pnrm)
13892 {
13893     /* Builtin functions */
13894     //double sqrt(double), atan2(double, double), asin(double);
13895 
13896 
13897 /* *********************************************************** */
13898 
13899 /*                                              From STRIPACK */
13900 /*                                            Robert J. Renka */
13901 /*                                  Dept. of Computer Science */
13902 /*                                       Univ. of North Texas */
13903 /*                                           renka@cs.unt.edu */
13904 /*                                                   08/27/90 */
13905 
13906 /*   This subroutine converts a point P from Cartesian coor- */
13907 /* dinates to spherical coordinates. */
13908 
13909 
13910 /* On input: */
13911 
13912 /*       PX,PY,PZ = Cartesian coordinates of P. */
13913 
13914 /* Input parameters are not altered by this routine. */
13915 
13916 /* On output: */
13917 
13918 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
13919 /*              0 if PNRM = 0.  PLAT should be scaled by */
13920 /*              180/PI to obtain the value in degrees. */
13921 
13922 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
13923 /*              if P lies on the Z-axis.  PLON should be */
13924 /*              scaled by 180/PI to obtain the value in */
13925 /*              degrees. */
13926 
13927 /*       PNRM = Magnitude (Euclidean norm) of P. */
13928 
13929 /* Modules required by SCOORD:  None */
13930 
13931 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
13932 
13933 /* *********************************************************** */
13934 
13935     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
13936     if (*px != 0. || *py != 0.) {
13937         *plon = atan2(*py, *px);
13938     } else {
13939         *plon = 0.;
13940     }
13941     if (*pnrm != 0.) {
13942         *plat = asin(*pz / *pnrm);
13943     } else {
13944         *plat = 0.;
13945     }
13946     return 0;
13947 } /* scoord_ */
13948 
13949 double store_(double *x)
13950 {
13951     /* System generated locals */
13952     double ret_val;
13953 
13954 
13955 /* *********************************************************** */
13956 
13957 /*                                              From STRIPACK */
13958 /*                                            Robert J. Renka */
13959 /*                                  Dept. of Computer Science */
13960 /*                                       Univ. of North Texas */
13961 /*                                           renka@cs.unt.edu */
13962 /*                                                   05/09/92 */
13963 
13964 /*   This function forces its argument X to be stored in a */
13965 /* memory location, thus providing a means of determining */
13966 /* floating point number characteristics (such as the machine */
13967 /* precision) when it is necessary to avoid computation in */
13968 /* high precision registers. */
13969 
13970 
13971 /* On input: */
13972 
13973 /*       X = Value to be stored. */
13974 
13975 /* X is not altered by this function. */
13976 
13977 /* On output: */
13978 
13979 /*       STORE = Value of X after it has been stored and */
13980 /*               possibly truncated or rounded to the single */
13981 /*               precision word length. */
13982 
13983 /* Modules required by STORE:  None */
13984 
13985 /* *********************************************************** */
13986 
13987     stcom_1.y = *x;
13988     ret_val = stcom_1.y;
13989     return ret_val;
13990 } /* store_ */
13991 
13992 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
13993         io2, int *list, int *lptr, int *lend, int *lp21)
13994 {
13995     /* System generated locals */
13996     int i__1;
13997 
13998     /* Local variables */
13999     static int lp, lph, lpsav;
14000     extern int lstptr_(int *, int *, int *, int *);
14001 
14002 
14003 /* *********************************************************** */
14004 
14005 /*                                              From STRIPACK */
14006 /*                                            Robert J. Renka */
14007 /*                                  Dept. of Computer Science */
14008 /*                                       Univ. of North Texas */
14009 /*                                           renka@cs.unt.edu */
14010 /*                                                   06/22/98 */
14011 
14012 /*   Given a triangulation of a set of points on the unit */
14013 /* sphere, this subroutine replaces a diagonal arc in a */
14014 /* strictly convex quadrilateral (defined by a pair of adja- */
14015 /* cent triangles) with the other diagonal.  Equivalently, a */
14016 /* pair of adjacent triangles is replaced by another pair */
14017 /* having the same union. */
14018 
14019 
14020 /* On input: */
14021 
14022 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14023 /*                         the quadrilateral.  IO1-IO2 is re- */
14024 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14025 /*                         and (IO2,IO1,IN2) must be trian- */
14026 /*                         gles on input. */
14027 
14028 /* The above parameters are not altered by this routine. */
14029 
14030 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14031 /*                        gulation.  Refer to Subroutine */
14032 /*                        TRMESH. */
14033 
14034 /* On output: */
14035 
14036 /*       LIST,LPTR,LEND = Data structure updated with the */
14037 /*                        swap -- triangles (IO1,IO2,IN1) and */
14038 /*                        (IO2,IO1,IN2) are replaced by */
14039 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14040 /*                        unless LP21 = 0. */
14041 
14042 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14043 /*              swap is performed unless IN1 and IN2 are */
14044 /*              adjacent on input, in which case LP21 = 0. */
14045 
14046 /* Module required by SWAP:  LSTPTR */
14047 
14048 /* Intrinsic function called by SWAP:  ABS */
14049 
14050 /* *********************************************************** */
14051 
14052 
14053 /* Local parameters: */
14054 
14055 /* LP,LPH,LPSAV = LIST pointers */
14056 
14057 
14058 /* Test for IN1 and IN2 adjacent. */
14059 
14060     /* Parameter adjustments */
14061     --lend;
14062     --lptr;
14063     --list;
14064 
14065     /* Function Body */
14066     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14067     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14068         *lp21 = 0;
14069         return 0;
14070     }
14071 
14072 /* Delete IO2 as a neighbor of IO1. */
14073 
14074     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14075     lph = lptr[lp];
14076     lptr[lp] = lptr[lph];
14077 
14078 /* If IO2 is the last neighbor of IO1, make IN2 the */
14079 /*   last neighbor. */
14080 
14081     if (lend[*io1] == lph) {
14082         lend[*io1] = lp;
14083     }
14084 
14085 /* Insert IN2 as a neighbor of IN1 following IO1 */
14086 /*   using the hole created above. */
14087 
14088     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14089     lpsav = lptr[lp];
14090     lptr[lp] = lph;
14091     list[lph] = *in2;
14092     lptr[lph] = lpsav;
14093 
14094 /* Delete IO1 as a neighbor of IO2. */
14095 
14096     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14097     lph = lptr[lp];
14098     lptr[lp] = lptr[lph];
14099 
14100 /* If IO1 is the last neighbor of IO2, make IN1 the */
14101 /*   last neighbor. */
14102 
14103     if (lend[*io2] == lph) {
14104         lend[*io2] = lp;
14105     }
14106 
14107 /* Insert IN1 as a neighbor of IN2 following IO2. */
14108 
14109     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14110     lpsav = lptr[lp];
14111     lptr[lp] = lph;
14112     list[lph] = *in1;
14113     lptr[lph] = lpsav;
14114     *lp21 = lph;
14115     return 0;
14116 } /* swap_ */
14117 
14118 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14119         double *x, double *y, double *z__)
14120 {
14121     /* System generated locals */
14122     long int ret_val;
14123 
14124     /* Local variables */
14125     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14126 
14127 
14128 /* *********************************************************** */
14129 
14130 /*                                              From STRIPACK */
14131 /*                                            Robert J. Renka */
14132 /*                                  Dept. of Computer Science */
14133 /*                                       Univ. of North Texas */
14134 /*                                           renka@cs.unt.edu */
14135 /*                                                   03/29/91 */
14136 
14137 /*   This function decides whether or not to replace a */
14138 /* diagonal arc in a quadrilateral with the other diagonal. */
14139 /* The decision will be to swap (SWPTST = TRUE) if and only */
14140 /* if N4 lies above the plane (in the half-space not contain- */
14141 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14142 /* the projection of N4 onto this plane is interior to the */
14143 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14144 /* swap if the quadrilateral is not strictly convex. */
14145 
14146 
14147 /* On input: */
14148 
14149 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14150 /*                     quadrilateral with N1 adjacent to N2, */
14151 /*                     and (N1,N2,N3) in counterclockwise */
14152 /*                     order.  The arc connecting N1 to N2 */
14153 /*                     should be replaced by an arc connec- */
14154 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14155 /*                     to Subroutine SWAP. */
14156 
14157 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14158 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14159 /*               define node I for I = N1, N2, N3, and N4. */
14160 
14161 /* Input parameters are not altered by this routine. */
14162 
14163 /* On output: */
14164 
14165 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14166 /*                and N2 should be swapped for an arc con- */
14167 /*                necting N3 and N4. */
14168 
14169 /* Modules required by SWPTST:  None */
14170 
14171 /* *********************************************************** */
14172 
14173 
14174 /* Local parameters: */
14175 
14176 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14177 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14178 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14179 /* X4,Y4,Z4 =    Coordinates of N4 */
14180 
14181     /* Parameter adjustments */
14182     --z__;
14183     --y;
14184     --x;
14185 
14186     /* Function Body */
14187     x4 = x[*n4];
14188     y4 = y[*n4];
14189     z4 = z__[*n4];
14190     dx1 = x[*n1] - x4;
14191     dx2 = x[*n2] - x4;
14192     dx3 = x[*n3] - x4;
14193     dy1 = y[*n1] - y4;
14194     dy2 = y[*n2] - y4;
14195     dy3 = y[*n3] - y4;
14196     dz1 = z__[*n1] - z4;
14197     dz2 = z__[*n2] - z4;
14198     dz3 = z__[*n3] - z4;
14199 
14200 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14201 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14202 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14203 
14204     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14205             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14206     return ret_val;
14207 } /* swptst_ */
14208 
14209 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14210         double *x, double *y, double *z__)
14211 {
14212     /* System generated locals */
14213     int i__1;
14214 
14215     /* Builtin functions */
14216     //double cos(double), sin(double);
14217 
14218     /* Local variables */
14219     static int i__, nn;
14220     static double phi, theta, cosphi;
14221 
14222 
14223 /* *********************************************************** */
14224 
14225 /*                                              From STRIPACK */
14226 /*                                            Robert J. Renka */
14227 /*                                  Dept. of Computer Science */
14228 /*                                       Univ. of North Texas */
14229 /*                                           renka@cs.unt.edu */
14230 /*                                                   04/08/90 */
14231 
14232 /*   This subroutine transforms spherical coordinates into */
14233 /* Cartesian coordinates on the unit sphere for input to */
14234 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14235 /* storage for RLAT and RLON if the latter need not be saved. */
14236 
14237 
14238 /* On input: */
14239 
14240 /*       N = Number of nodes (points on the unit sphere) */
14241 /*           whose coordinates are to be transformed. */
14242 
14243 /*       RLAT = Array of length N containing latitudinal */
14244 /*              coordinates of the nodes in radians. */
14245 
14246 /*       RLON = Array of length N containing longitudinal */
14247 /*              coordinates of the nodes in radians. */
14248 
14249 /* The above parameters are not altered by this routine. */
14250 
14251 /*       X,Y,Z = Arrays of length at least N. */
14252 
14253 /* On output: */
14254 
14255 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14256 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14257 /*               to N. */
14258 
14259 /* Modules required by TRANS:  None */
14260 
14261 /* Intrinsic functions called by TRANS:  COS, SIN */
14262 
14263 /* *********************************************************** */
14264 
14265 
14266 /* Local parameters: */
14267 
14268 /* COSPHI = cos(PHI) */
14269 /* I =      DO-loop index */
14270 /* NN =     Local copy of N */
14271 /* PHI =    Latitude */
14272 /* THETA =  Longitude */
14273 
14274     /* Parameter adjustments */
14275     --z__;
14276     --y;
14277     --x;
14278     --rlon;
14279     --rlat;
14280 
14281     /* Function Body */
14282     nn = *n;
14283     i__1 = nn;
14284     for (i__ = 1; i__ <= i__1; ++i__) {
14285         phi = rlat[i__];
14286         theta = rlon[i__];
14287         cosphi = cos(phi);
14288         x[i__] = cosphi * cos(theta);
14289         y[i__] = cosphi * sin(theta);
14290         z__[i__] = sin(phi);
14291 /* L1: */
14292     }
14293     return 0;
14294 } /* trans_ */
14295 
14296 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14297         double *x, double *y, double *z__, int *list, int
14298         *lptr, int *lend, double *b1, double *b2, double *b3,
14299         int *i1, int *i2, int *i3)
14300 {
14301     /* Initialized data */
14302 
14303     static int ix = 1;
14304     static int iy = 2;
14305     static int iz = 3;
14306 
14307     /* System generated locals */
14308     int i__1;
14309     double d__1, d__2;
14310 
14311     /* Local variables */
14312     static double q[3];
14313     static int n0, n1, n2, n3, n4, nf;
14314     static double s12;
14315     static int nl, lp;
14316     static double xp, yp, zp;
14317     static int n1s, n2s;
14318     static double eps, tol, ptn1, ptn2;
14319     static int next;
14320     extern int jrand_(int *, int *, int *, int *);
14321     extern double store_(double *);
14322     extern int lstptr_(int *, int *, int *, int *);
14323 
14324 
14325 /* *********************************************************** */
14326 
14327 /*                                              From STRIPACK */
14328 /*                                            Robert J. Renka */
14329 /*                                  Dept. of Computer Science */
14330 /*                                       Univ. of North Texas */
14331 /*                                           renka@cs.unt.edu */
14332 /*                                                   11/30/99 */
14333 
14334 /*   This subroutine locates a point P relative to a triangu- */
14335 /* lation created by Subroutine TRMESH.  If P is contained in */
14336 /* a triangle, the three vertex indexes and barycentric coor- */
14337 /* dinates are returned.  Otherwise, the indexes of the */
14338 /* visible boundary nodes are returned. */
14339 
14340 
14341 /* On input: */
14342 
14343 /*       NST = Index of a node at which TRFIND begins its */
14344 /*             search.  Search time depends on the proximity */
14345 /*             of this node to P. */
14346 
14347 /*       P = Array of length 3 containing the x, y, and z */
14348 /*           coordinates (in that order) of the point P to be */
14349 /*           located. */
14350 
14351 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14352 
14353 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14354 /*               coordinates of the triangulation nodes (unit */
14355 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14356 /*               for I = 1 to N. */
14357 
14358 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14359 /*                        gulation.  Refer to Subroutine */
14360 /*                        TRMESH. */
14361 
14362 /* Input parameters are not altered by this routine. */
14363 
14364 /* On output: */
14365 
14366 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14367 /*                  the central projection of P onto the un- */
14368 /*                  derlying planar triangle if P is in the */
14369 /*                  convex hull of the nodes.  These parame- */
14370 /*                  ters are not altered if I1 = 0. */
14371 
14372 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14373 /*                  of a triangle containing P if P is con- */
14374 /*                  tained in a triangle.  If P is not in the */
14375 /*                  convex hull of the nodes, I1 and I2 are */
14376 /*                  the rightmost and leftmost (boundary) */
14377 /*                  nodes that are visible from P, and */
14378 /*                  I3 = 0.  (If all boundary nodes are vis- */
14379 /*                  ible from P, then I1 and I2 coincide.) */
14380 /*                  I1 = I2 = I3 = 0 if P and all of the */
14381 /*                  nodes are coplanar (lie on a common great */
14382 /*                  circle. */
14383 
14384 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14385 
14386 /* Intrinsic function called by TRFIND:  ABS */
14387 
14388 /* *********************************************************** */
14389 
14390 
14391     /* Parameter adjustments */
14392     --p;
14393     --lend;
14394     --z__;
14395     --y;
14396     --x;
14397     --list;
14398     --lptr;
14399 
14400     /* Function Body */
14401 
14402 /* Local parameters: */
14403 
14404 /* EPS =      Machine precision */
14405 /* IX,IY,IZ = int seeds for JRAND */
14406 /* LP =       LIST pointer */
14407 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14408 /*              cone (with vertex N0) containing P, or end- */
14409 /*              points of a boundary edge such that P Right */
14410 /*              N1->N2 */
14411 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14412 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14413 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14414 /* NF,NL =    First and last neighbors of N0, or first */
14415 /*              (rightmost) and last (leftmost) nodes */
14416 /*              visible from P when P is exterior to the */
14417 /*              triangulation */
14418 /* PTN1 =     Scalar product <P,N1> */
14419 /* PTN2 =     Scalar product <P,N2> */
14420 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14421 /*              the boundary traversal when P is exterior */
14422 /* S12 =      Scalar product <N1,N2> */
14423 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14424 /*              bound on the magnitude of a negative bary- */
14425 /*              centric coordinate (B1 or B2) for P in a */
14426 /*              triangle -- used to avoid an infinite number */
14427 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14428 /*              B2 < 0 but small in magnitude */
14429 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14430 /* X0,Y0,Z0 = Dummy arguments for DET */
14431 /* X1,Y1,Z1 = Dummy arguments for DET */
14432 /* X2,Y2,Z2 = Dummy arguments for DET */
14433 
14434 /* Statement function: */
14435 
14436 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14437 /*                       (closed) left hemisphere defined by */
14438 /*                       the plane containing (0,0,0), */
14439 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14440 /*                       left is defined relative to an ob- */
14441 /*                       server at (X1,Y1,Z1) facing */
14442 /*                       (X2,Y2,Z2). */
14443 
14444 
14445 /* Initialize variables. */
14446 
14447     xp = p[1];
14448     yp = p[2];
14449     zp = p[3];
14450     n0 = *nst;
14451     if (n0 < 1 || n0 > *n) {
14452         n0 = jrand_(n, &ix, &iy, &iz);
14453     }
14454 
14455 /* Compute the relative machine precision EPS and TOL. */
14456 
14457     eps = 1.;
14458 L1:
14459     eps /= 2.;
14460     d__1 = eps + 1.;
14461     if (store_(&d__1) > 1.) {
14462         goto L1;
14463     }
14464     eps *= 2.;
14465     tol = eps * 4.;
14466 
14467 /* Set NF and NL to the first and last neighbors of N0, and */
14468 /*   initialize N1 = NF. */
14469 
14470 L2:
14471     lp = lend[n0];
14472     nl = list[lp];
14473     lp = lptr[lp];
14474     nf = list[lp];
14475     n1 = nf;
14476 
14477 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14478 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14479 
14480     if (nl > 0) {
14481 
14482 /*   N0 is an interior node.  Find N1. */
14483 
14484 L3:
14485         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14486                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14487                 -1e-10) {
14488             lp = lptr[lp];
14489             n1 = list[lp];
14490             if (n1 == nl) {
14491                 goto L6;
14492             }
14493             goto L3;
14494         }
14495     } else {
14496 
14497 /*   N0 is a boundary node.  Test for P exterior. */
14498 
14499         nl = -nl;
14500         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14501                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14502                 -1e-10) {
14503 
14504 /*   P is to the right of the boundary edge N0->NF. */
14505 
14506             n1 = n0;
14507             n2 = nf;
14508             goto L9;
14509         }
14510         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14511                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14512                 -1e-10) {
14513 
14514 /*   P is to the right of the boundary edge NL->N0. */
14515 
14516             n1 = nl;
14517             n2 = n0;
14518             goto L9;
14519         }
14520     }
14521 
14522 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14523 /*   next neighbor of N0 (following N1). */
14524 
14525 L4:
14526     lp = lptr[lp];
14527     n2 = (i__1 = list[lp], abs(i__1));
14528     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14529             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14530         goto L7;
14531     }
14532     n1 = n2;
14533     if (n1 != nl) {
14534         goto L4;
14535     }
14536     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14537             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14538         goto L6;
14539     }
14540 
14541 /* P is left of or on arcs N0->NB for all neighbors NB */
14542 /*   of N0.  Test for P = +/-N0. */
14543 
14544     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14545     if (store_(&d__2) < 1. - eps * 4.) {
14546 
14547 /*   All points are collinear iff P Left NB->N0 for all */
14548 /*     neighbors NB of N0.  Search the neighbors of N0. */
14549 /*     Note:  N1 = NL and LP points to NL. */
14550 
14551 L5:
14552         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14553                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14554                 -1e-10) {
14555             lp = lptr[lp];
14556             n1 = (i__1 = list[lp], abs(i__1));
14557             if (n1 == nl) {
14558                 goto L14;
14559             }
14560             goto L5;
14561         }
14562     }
14563 
14564 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14565 /*   and start over. */
14566 
14567     n0 = n1;
14568     goto L2;
14569 
14570 /* P is between arcs N0->N1 and N0->NF. */
14571 
14572 L6:
14573     n2 = nf;
14574 
14575 /* P is contained in a wedge defined by geodesics N0-N1 and */
14576 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14577 /*   test for cycling. */
14578 
14579 L7:
14580     n3 = n0;
14581     n1s = n1;
14582     n2s = n2;
14583 
14584 /* Top of edge-hopping loop: */
14585 
14586 L8:
14587 
14588     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14589             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14590      if (*b3 < -1e-10) {
14591 
14592 /*   Set N4 to the first neighbor of N2 following N1 (the */
14593 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14594 
14595         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14596         if (list[lp] < 0) {
14597             goto L9;
14598         }
14599         lp = lptr[lp];
14600         n4 = (i__1 = list[lp], abs(i__1));
14601 
14602 /*   Define a new arc N1->N2 which intersects the geodesic */
14603 /*     N0-P. */
14604         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14605                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14606                 -1e-10) {
14607             n3 = n2;
14608             n2 = n4;
14609             n1s = n1;
14610             if (n2 != n2s && n2 != n0) {
14611                 goto L8;
14612             }
14613         } else {
14614             n3 = n1;
14615             n1 = n4;
14616             n2s = n2;
14617             if (n1 != n1s && n1 != n0) {
14618                 goto L8;
14619             }
14620         }
14621 
14622 /*   The starting node N0 or edge N1-N2 was encountered */
14623 /*     again, implying a cycle (infinite loop).  Restart */
14624 /*     with N0 randomly selected. */
14625 
14626         n0 = jrand_(n, &ix, &iy, &iz);
14627         goto L2;
14628     }
14629 
14630 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14631 /*   or P is close to -N0. */
14632 
14633     if (*b3 >= eps) {
14634 
14635 /*   B3 .NE. 0. */
14636 
14637         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14638                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14639         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14640                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14641         if (*b1 < -tol || *b2 < -tol) {
14642 
14643 /*   Restart with N0 randomly selected. */
14644 
14645             n0 = jrand_(n, &ix, &iy, &iz);
14646             goto L2;
14647         }
14648     } else {
14649 
14650 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14651 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14652 
14653         *b3 = 0.;
14654         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14655         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14656         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14657         *b1 = ptn1 - s12 * ptn2;
14658         *b2 = ptn2 - s12 * ptn1;
14659         if (*b1 < -tol || *b2 < -tol) {
14660 
14661 /*   Restart with N0 randomly selected. */
14662 
14663             n0 = jrand_(n, &ix, &iy, &iz);
14664             goto L2;
14665         }
14666     }
14667 
14668 /* P is in (N1,N2,N3). */
14669 
14670     *i1 = n1;
14671     *i2 = n2;
14672     *i3 = n3;
14673     if (*b1 < 0.f) {
14674         *b1 = 0.f;
14675     }
14676     if (*b2 < 0.f) {
14677         *b2 = 0.f;
14678     }
14679     return 0;
14680 
14681 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14682 /*   Save N1 and N2, and set NL = 0 to indicate that */
14683 /*   NL has not yet been found. */
14684 
14685 L9:
14686     n1s = n1;
14687     n2s = n2;
14688     nl = 0;
14689 
14690 /*           Counterclockwise Boundary Traversal: */
14691 
14692 L10:
14693 
14694     lp = lend[n2];
14695     lp = lptr[lp];
14696     next = list[lp];
14697      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14698              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14699             >= -1e-10) {
14700 
14701 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14702 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14703 
14704         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14705         q[0] = x[n1] - s12 * x[n2];
14706         q[1] = y[n1] - s12 * y[n2];
14707         q[2] = z__[n1] - s12 * z__[n2];
14708         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14709             goto L11;
14710         }
14711         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14712             goto L11;
14713         }
14714 
14715 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14716 /*     the leftmost visible node. */
14717 
14718         nl = n2;
14719     }
14720 
14721 /* Bottom of counterclockwise loop: */
14722 
14723     n1 = n2;
14724     n2 = next;
14725     if (n2 != n1s) {
14726         goto L10;
14727     }
14728 
14729 /* All boundary nodes are visible from P. */
14730 
14731     *i1 = n1s;
14732     *i2 = n1s;
14733     *i3 = 0;
14734     return 0;
14735 
14736 /* N2 is the rightmost visible node. */
14737 
14738 L11:
14739     nf = n2;
14740     if (nl == 0) {
14741 
14742 /* Restore initial values of N1 and N2, and begin the search */
14743 /*   for the leftmost visible node. */
14744 
14745         n2 = n2s;
14746         n1 = n1s;
14747 
14748 /*           Clockwise Boundary Traversal: */
14749 
14750 L12:
14751         lp = lend[n1];
14752         next = -list[lp];
14753         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14754                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14755                  y[next]) >= -1e-10) {
14756 
14757 /*   N1 is the leftmost visible node if P or NEXT is */
14758 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14759 
14760             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14761             q[0] = x[n2] - s12 * x[n1];
14762             q[1] = y[n2] - s12 * y[n1];
14763             q[2] = z__[n2] - s12 * z__[n1];
14764             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14765                 goto L13;
14766             }
14767             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14768                 goto L13;
14769             }
14770 
14771 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14772 /*     rightmost visible node. */
14773 
14774             nf = n1;
14775         }
14776 
14777 /* Bottom of clockwise loop: */
14778 
14779         n2 = n1;
14780         n1 = next;
14781         if (n1 != n1s) {
14782             goto L12;
14783         }
14784 
14785 /* All boundary nodes are visible from P. */
14786 
14787         *i1 = n1;
14788         *i2 = n1;
14789         *i3 = 0;
14790         return 0;
14791 
14792 /* N1 is the leftmost visible node. */
14793 
14794 L13:
14795         nl = n1;
14796     }
14797 
14798 /* NF and NL have been found. */
14799 
14800     *i1 = nf;
14801     *i2 = nl;
14802     *i3 = 0;
14803     return 0;
14804 
14805 /* All points are collinear (coplanar). */
14806 
14807 L14:
14808     *i1 = 0;
14809     *i2 = 0;
14810     *i3 = 0;
14811     return 0;
14812 } /* trfind_ */
14813 
14814 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
14815         int *lend, int *nrow, int *nt, int *ltri, int *
14816         ier)
14817 {
14818     /* System generated locals */
14819     int ltri_dim1, ltri_offset, i__1, i__2;
14820 
14821     /* Local variables */
14822     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14823             lpl, isv;
14824     static long int arcs;
14825     static int lpln1;
14826 
14827 
14828 /* *********************************************************** */
14829 
14830 /*                                              From STRIPACK */
14831 /*                                            Robert J. Renka */
14832 /*                                  Dept. of Computer Science */
14833 /*                                       Univ. of North Texas */
14834 /*                                           renka@cs.unt.edu */
14835 /*                                                   07/20/96 */
14836 
14837 /*   This subroutine converts a triangulation data structure */
14838 /* from the linked list created by Subroutine TRMESH to a */
14839 /* triangle list. */
14840 
14841 /* On input: */
14842 
14843 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14844 
14845 /*       LIST,LPTR,LEND = Linked list data structure defin- */
14846 /*                        ing the triangulation.  Refer to */
14847 /*                        Subroutine TRMESH. */
14848 
14849 /*       NROW = Number of rows (entries per triangle) re- */
14850 /*              served for the triangle list LTRI.  The value */
14851 /*              must be 6 if only the vertex indexes and */
14852 /*              neighboring triangle indexes are to be */
14853 /*              stored, or 9 if arc indexes are also to be */
14854 /*              assigned and stored.  Refer to LTRI. */
14855 
14856 /* The above parameters are not altered by this routine. */
14857 
14858 /*       LTRI = int array of length at least NROW*NT, */
14859 /*              where NT is at most 2N-4.  (A sufficient */
14860 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
14861 
14862 /* On output: */
14863 
14864 /*       NT = Number of triangles in the triangulation unless */
14865 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
14866 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
14867 /*            number of boundary nodes. */
14868 
14869 /*       LTRI = NROW by NT array whose J-th column contains */
14870 /*              the vertex nodal indexes (first three rows), */
14871 /*              neighboring triangle indexes (second three */
14872 /*              rows), and, if NROW = 9, arc indexes (last */
14873 /*              three rows) associated with triangle J for */
14874 /*              J = 1,...,NT.  The vertices are ordered */
14875 /*              counterclockwise with the first vertex taken */
14876 /*              to be the one with smallest index.  Thus, */
14877 /*              LTRI(2,J) and LTRI(3,J) are larger than */
14878 /*              LTRI(1,J) and index adjacent neighbors of */
14879 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
14880 /*              and LTRI(I+6,J) index the triangle and arc, */
14881 /*              respectively, which are opposite (not shared */
14882 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
14883 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
14884 /*              indexes range from 1 to N, triangle indexes */
14885 /*              from 0 to NT, and, if included, arc indexes */
14886 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
14887 /*              or 3N-6 if NB = 0.  The triangles are or- */
14888 /*              dered on first (smallest) vertex indexes. */
14889 
14890 /*       IER = Error indicator. */
14891 /*             IER = 0 if no errors were encountered. */
14892 /*             IER = 1 if N or NROW is outside its valid */
14893 /*                     range on input. */
14894 /*             IER = 2 if the triangulation data structure */
14895 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
14896 /*                     however, that these arrays are not */
14897 /*                     completely tested for validity. */
14898 
14899 /* Modules required by TRLIST:  None */
14900 
14901 /* Intrinsic function called by TRLIST:  ABS */
14902 
14903 /* *********************************************************** */
14904 
14905 
14906 /* Local parameters: */
14907 
14908 /* ARCS =     long int variable with value TRUE iff are */
14909 /*              indexes are to be stored */
14910 /* I,J =      LTRI row indexes (1 to 3) associated with */
14911 /*              triangles KT and KN, respectively */
14912 /* I1,I2,I3 = Nodal indexes of triangle KN */
14913 /* ISV =      Variable used to permute indexes I1,I2,I3 */
14914 /* KA =       Arc index and number of currently stored arcs */
14915 /* KN =       Index of the triangle that shares arc I1-I2 */
14916 /*              with KT */
14917 /* KT =       Triangle index and number of currently stored */
14918 /*              triangles */
14919 /* LP =       LIST pointer */
14920 /* LP2 =      Pointer to N2 as a neighbor of N1 */
14921 /* LPL =      Pointer to the last neighbor of I1 */
14922 /* LPLN1 =    Pointer to the last neighbor of N1 */
14923 /* N1,N2,N3 = Nodal indexes of triangle KT */
14924 /* NM2 =      N-2 */
14925 
14926 
14927 /* Test for invalid input parameters. */
14928 
14929     /* Parameter adjustments */
14930     --lend;
14931     --list;
14932     --lptr;
14933     ltri_dim1 = *nrow;
14934     ltri_offset = 1 + ltri_dim1;
14935     ltri -= ltri_offset;
14936 
14937     /* Function Body */
14938     if (*n < 3 || *nrow != 6 && *nrow != 9) {
14939         goto L11;
14940     }
14941 
14942 /* Initialize parameters for loop on triangles KT = (N1,N2, */
14943 /*   N3), where N1 < N2 and N1 < N3. */
14944 
14945 /*   ARCS = TRUE iff arc indexes are to be stored. */
14946 /*   KA,KT = Numbers of currently stored arcs and triangles. */
14947 /*   NM2 = Upper bound on candidates for N1. */
14948 
14949     arcs = *nrow == 9;
14950     ka = 0;
14951     kt = 0;
14952     nm2 = *n - 2;
14953 
14954 /* Loop on nodes N1. */
14955 
14956     i__1 = nm2;
14957     for (n1 = 1; n1 <= i__1; ++n1) {
14958 
14959 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
14960 /*   to the last neighbor of N1, and LP2 points to N2. */
14961 
14962         lpln1 = lend[n1];
14963         lp2 = lpln1;
14964 L1:
14965         lp2 = lptr[lp2];
14966         n2 = list[lp2];
14967         lp = lptr[lp2];
14968         n3 = (i__2 = list[lp], abs(i__2));
14969         if (n2 < n1 || n3 < n1) {
14970             goto L8;
14971         }
14972 
14973 /* Add a new triangle KT = (N1,N2,N3). */
14974 
14975         ++kt;
14976         ltri[kt * ltri_dim1 + 1] = n1;
14977         ltri[kt * ltri_dim1 + 2] = n2;
14978         ltri[kt * ltri_dim1 + 3] = n3;
14979 
14980 /* Loop on triangle sides (I2,I1) with neighboring triangles */
14981 /*   KN = (I1,I2,I3). */
14982 
14983         for (i__ = 1; i__ <= 3; ++i__) {
14984             if (i__ == 1) {
14985                 i1 = n3;
14986                 i2 = n2;
14987             } else if (i__ == 2) {
14988                 i1 = n1;
14989                 i2 = n3;
14990             } else {
14991                 i1 = n2;
14992                 i2 = n1;
14993             }
14994 
14995 /* Set I3 to the neighbor of I1 that follows I2 unless */
14996 /*   I2->I1 is a boundary arc. */
14997 
14998             lpl = lend[i1];
14999             lp = lptr[lpl];
15000 L2:
15001             if (list[lp] == i2) {
15002                 goto L3;
15003             }
15004             lp = lptr[lp];
15005             if (lp != lpl) {
15006                 goto L2;
15007             }
15008 
15009 /*   I2 is the last neighbor of I1 unless the data structure */
15010 /*     is invalid.  Bypass the search for a neighboring */
15011 /*     triangle if I2->I1 is a boundary arc. */
15012 
15013             if ((i__2 = list[lp], abs(i__2)) != i2) {
15014                 goto L12;
15015             }
15016             kn = 0;
15017             if (list[lp] < 0) {
15018                 goto L6;
15019             }
15020 
15021 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15022 /*     a neighbor of I1. */
15023 
15024 L3:
15025             lp = lptr[lp];
15026             i3 = (i__2 = list[lp], abs(i__2));
15027 
15028 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15029 /*   and permute the vertex indexes of KN so that I1 is */
15030 /*   smallest. */
15031 
15032             if (i1 < i2 && i1 < i3) {
15033                 j = 3;
15034             } else if (i2 < i3) {
15035                 j = 2;
15036                 isv = i1;
15037                 i1 = i2;
15038                 i2 = i3;
15039                 i3 = isv;
15040             } else {
15041                 j = 1;
15042                 isv = i1;
15043                 i1 = i3;
15044                 i3 = i2;
15045                 i2 = isv;
15046             }
15047 
15048 /* Test for KN > KT (triangle index not yet assigned). */
15049 
15050             if (i1 > n1) {
15051                 goto L7;
15052             }
15053 
15054 /* Find KN, if it exists, by searching the triangle list in */
15055 /*   reverse order. */
15056 
15057             for (kn = kt - 1; kn >= 1; --kn) {
15058                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15059                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15060                     goto L5;
15061                 }
15062 /* L4: */
15063             }
15064             goto L7;
15065 
15066 /* Store KT as a neighbor of KN. */
15067 
15068 L5:
15069             ltri[j + 3 + kn * ltri_dim1] = kt;
15070 
15071 /* Store KN as a neighbor of KT, and add a new arc KA. */
15072 
15073 L6:
15074             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15075             if (arcs) {
15076                 ++ka;
15077                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15078                 if (kn != 0) {
15079                     ltri[j + 6 + kn * ltri_dim1] = ka;
15080                 }
15081             }
15082 L7:
15083             ;
15084         }
15085 
15086 /* Bottom of loop on triangles. */
15087 
15088 L8:
15089         if (lp2 != lpln1) {
15090             goto L1;
15091         }
15092 /* L9: */
15093     }
15094 
15095 /* No errors encountered. */
15096 
15097     *nt = kt;
15098     *ier = 0;
15099     return 0;
15100 
15101 /* Invalid input parameter. */
15102 
15103 L11:
15104     *nt = 0;
15105     *ier = 1;
15106     return 0;
15107 
15108 /* Invalid triangulation data structure:  I1 is a neighbor of */
15109 /*   I2, but I2 is not a neighbor of I1. */
15110 
15111 L12:
15112     *nt = 0;
15113     *ier = 2;
15114     return 0;
15115 } /* trlist_ */
15116 
15117 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15118         double *z__, int *iflag, int *nrow, int *nt, int *
15119         ltri, int *lout)
15120 {
15121     /* Initialized data */
15122 
15123     static int nmax = 9999;
15124     static int nlmax = 58;
15125 
15126     /* System generated locals */
15127     int ltri_dim1, ltri_offset, i__1;
15128 
15129     /* Local variables */
15130     static int i__, k, na, nb, nl, lun;
15131 
15132 
15133 /* *********************************************************** */
15134 
15135 /*                                              From STRIPACK */
15136 /*                                            Robert J. Renka */
15137 /*                                  Dept. of Computer Science */
15138 /*                                       Univ. of North Texas */
15139 /*                                           renka@cs.unt.edu */
15140 /*                                                   07/02/98 */
15141 
15142 /*   This subroutine prints the triangle list created by Sub- */
15143 /* routine TRLIST and, optionally, the nodal coordinates */
15144 /* (either latitude and longitude or Cartesian coordinates) */
15145 /* on long int unit LOUT.  The numbers of boundary nodes, */
15146 /* triangles, and arcs are also printed. */
15147 
15148 
15149 /* On input: */
15150 
15151 /*       N = Number of nodes in the triangulation. */
15152 /*           3 .LE. N .LE. 9999. */
15153 
15154 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15155 /*               coordinates of the nodes if IFLAG = 0, or */
15156 /*               (X and Y only) arrays of length N containing */
15157 /*               longitude and latitude, respectively, if */
15158 /*               IFLAG > 0, or unused dummy parameters if */
15159 /*               IFLAG < 0. */
15160 
15161 /*       IFLAG = Nodal coordinate option indicator: */
15162 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15163 /*                         Cartesian coordinates) are to be */
15164 /*                         printed (to 6 decimal places). */
15165 /*               IFLAG > 0 if only X and Y (assumed to con- */
15166 /*                         tain longitude and latitude) are */
15167 /*                         to be printed (to 6 decimal */
15168 /*                         places). */
15169 /*               IFLAG < 0 if only the adjacency lists are to */
15170 /*                         be printed. */
15171 
15172 /*       NROW = Number of rows (entries per triangle) re- */
15173 /*              served for the triangle list LTRI.  The value */
15174 /*              must be 6 if only the vertex indexes and */
15175 /*              neighboring triangle indexes are stored, or 9 */
15176 /*              if arc indexes are also stored. */
15177 
15178 /*       NT = Number of triangles in the triangulation. */
15179 /*            1 .LE. NT .LE. 9999. */
15180 
15181 /*       LTRI = NROW by NT array whose J-th column contains */
15182 /*              the vertex nodal indexes (first three rows), */
15183 /*              neighboring triangle indexes (second three */
15184 /*              rows), and, if NROW = 9, arc indexes (last */
15185 /*              three rows) associated with triangle J for */
15186 /*              J = 1,...,NT. */
15187 
15188 /*       LOUT = long int unit number for output.  If LOUT is */
15189 /*              not in the range 0 to 99, output is written */
15190 /*              to unit 6. */
15191 
15192 /* Input parameters are not altered by this routine. */
15193 
15194 /* On output: */
15195 
15196 /*   The triangle list and nodal coordinates (as specified by */
15197 /* IFLAG) are written to unit LOUT. */
15198 
15199 /* Modules required by TRLPRT:  None */
15200 
15201 /* *********************************************************** */
15202 
15203     /* Parameter adjustments */
15204     --z__;
15205     --y;
15206     --x;
15207     ltri_dim1 = *nrow;
15208     ltri_offset = 1 + ltri_dim1;
15209     ltri -= ltri_offset;
15210 
15211     /* Function Body */
15212 
15213 /* Local parameters: */
15214 
15215 /* I =     DO-loop, nodal index, and row index for LTRI */
15216 /* K =     DO-loop and triangle index */
15217 /* LUN =   long int unit number for output */
15218 /* NA =    Number of triangulation arcs */
15219 /* NB =    Number of boundary nodes */
15220 /* NL =    Number of lines printed on the current page */
15221 /* NLMAX = Maximum number of print lines per page (except */
15222 /*           for the last page which may have two addi- */
15223 /*           tional lines) */
15224 /* NMAX =  Maximum value of N and NT (4-digit format) */
15225 
15226     lun = *lout;
15227     if (lun < 0 || lun > 99) {
15228         lun = 6;
15229     }
15230 
15231 /* Print a heading and test for invalid input. */
15232 
15233 /*      WRITE (LUN,100) N */
15234     nl = 3;
15235     if (*n < 3 || *n > nmax || *nrow != 6 && *nrow != 9 || *nt < 1 || *nt >
15236             nmax) {
15237 
15238 /* Print an error message and exit. */
15239 
15240 /*        WRITE (LUN,110) N, NROW, NT */
15241         return 0;
15242     }
15243     if (*iflag == 0) {
15244 
15245 /* Print X, Y, and Z. */
15246 
15247 /*        WRITE (LUN,101) */
15248         nl = 6;
15249         i__1 = *n;
15250         for (i__ = 1; i__ <= i__1; ++i__) {
15251             if (nl >= nlmax) {
15252 /*            WRITE (LUN,108) */
15253                 nl = 0;
15254             }
15255 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15256             ++nl;
15257 /* L1: */
15258         }
15259     } else if (*iflag > 0) {
15260 
15261 /* Print X (longitude) and Y (latitude). */
15262 
15263 /*        WRITE (LUN,102) */
15264         nl = 6;
15265         i__1 = *n;
15266         for (i__ = 1; i__ <= i__1; ++i__) {
15267             if (nl >= nlmax) {
15268 /*            WRITE (LUN,108) */
15269                 nl = 0;
15270             }
15271 /*          WRITE (LUN,104) I, X(I), Y(I) */
15272             ++nl;
15273 /* L2: */
15274         }
15275     }
15276 
15277 /* Print the triangulation LTRI. */
15278 
15279     if (nl > nlmax / 2) {
15280 /*        WRITE (LUN,108) */
15281         nl = 0;
15282     }
15283     if (*nrow == 6) {
15284 /*        WRITE (LUN,105) */
15285     } else {
15286 /*        WRITE (LUN,106) */
15287     }
15288     nl += 5;
15289     i__1 = *nt;
15290     for (k = 1; k <= i__1; ++k) {
15291         if (nl >= nlmax) {
15292 /*          WRITE (LUN,108) */
15293             nl = 0;
15294         }
15295 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15296         ++nl;
15297 /* L3: */
15298     }
15299 
15300 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15301 /*   triangles). */
15302 
15303     nb = (*n << 1) - *nt - 2;
15304     if (nb < 3) {
15305         nb = 0;
15306         na = *n * 3 - 6;
15307     } else {
15308         na = *nt + *n - 1;
15309     }
15310 /*      WRITE (LUN,109) NB, NA, NT */
15311     return 0;
15312 
15313 /* Print formats: */
15314 
15315 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15316 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15317 /*     .        'Z(Node)'//) */
15318 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15319 /*  103 FORMAT (8X,I4,3D17.6) */
15320 /*  104 FORMAT (16X,I4,2D17.6) */
15321 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15322 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15323 /*     .        'KT2',4X,'KT3'/) */
15324 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15325 /*     .        14X,'Arcs'/ */
15326 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15327 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15328 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15329 /*  108 FORMAT (///) */
15330 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15331 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15332 /*     .        ' Triangles') */
15333 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15334 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15335 } /* trlprt_ */
15336 
15337 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15338         double *z__, int *list, int *lptr, int *lend, int
15339         *lnew, int *near__, int *next, double *dist, int *ier)
15340 {
15341     /* System generated locals */
15342     int i__1, i__2;
15343 
15344     /* Local variables */
15345     static double d__;
15346     static int i__, j, k;
15347     static double d1, d2, d3;
15348     static int i0, lp, nn, lpl;
15349     extern long int left_(double *, double *, double *, double
15350             *, double *, double *, double *, double *,
15351             double *);
15352     static int nexti;
15353     extern /* Subroutine */ int addnod_(int *, int *, double *,
15354             double *, double *, int *, int *, int *,
15355             int *, int *);
15356 
15357 
15358 /* *********************************************************** */
15359 
15360 /*                                              From STRIPACK */
15361 /*                                            Robert J. Renka */
15362 /*                                  Dept. of Computer Science */
15363 /*                                       Univ. of North Texas */
15364 /*                                           renka@cs.unt.edu */
15365 /*                                                   03/04/03 */
15366 
15367 /*   This subroutine creates a Delaunay triangulation of a */
15368 /* set of N arbitrarily distributed points, referred to as */
15369 /* nodes, on the surface of the unit sphere.  The Delaunay */
15370 /* triangulation is defined as a set of (spherical) triangles */
15371 /* with the following five properties: */
15372 
15373 /*  1)  The triangle vertices are nodes. */
15374 /*  2)  No triangle contains a node other than its vertices. */
15375 /*  3)  The interiors of the triangles are pairwise disjoint. */
15376 /*  4)  The union of triangles is the convex hull of the set */
15377 /*        of nodes (the smallest convex set that contains */
15378 /*        the nodes).  If the nodes are not contained in a */
15379 /*        single hemisphere, their convex hull is the en- */
15380 /*        tire sphere and there are no boundary nodes. */
15381 /*        Otherwise, there are at least three boundary nodes. */
15382 /*  5)  The interior of the circumcircle of each triangle */
15383 /*        contains no node. */
15384 
15385 /* The first four properties define a triangulation, and the */
15386 /* last property results in a triangulation which is as close */
15387 /* as possible to equiangular in a certain sense and which is */
15388 /* uniquely defined unless four or more nodes lie in a common */
15389 /* plane.  This property makes the triangulation well-suited */
15390 /* for solving closest-point problems and for triangle-based */
15391 /* interpolation. */
15392 
15393 /*   The algorithm has expected time complexity O(N*log(N)) */
15394 /* for most nodal distributions. */
15395 
15396 /*   Spherical coordinates (latitude and longitude) may be */
15397 /* converted to Cartesian coordinates by Subroutine TRANS. */
15398 
15399 /*   The following is a list of the software package modules */
15400 /* which a user may wish to call directly: */
15401 
15402 /*  ADDNOD - Updates the triangulation by appending a new */
15403 /*             node. */
15404 
15405 /*  AREAS  - Returns the area of a spherical triangle. */
15406 
15407 /*  AREAV  - Returns the area of a Voronoi region associated */
15408 /*           with an interior node without requiring that the */
15409 /*           entire Voronoi diagram be computed and stored. */
15410 
15411 /*  BNODES - Returns an array containing the indexes of the */
15412 /*             boundary nodes (if any) in counterclockwise */
15413 /*             order.  Counts of boundary nodes, triangles, */
15414 /*             and arcs are also returned. */
15415 
15416 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15417 /*           formly spaced points on the unit circle centered */
15418 /*           at (0,0). */
15419 
15420 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15421 /*             gle. */
15422 
15423 /*  CRLIST - Returns the set of triangle circumcenters */
15424 /*             (Voronoi vertices) and circumradii associated */
15425 /*             with a triangulation. */
15426 
15427 /*  DELARC - Deletes a boundary arc from a triangulation. */
15428 
15429 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15430 
15431 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15432 /*             ted by an arc in the triangulation. */
15433 
15434 /*  GETNP  - Determines the ordered sequence of L closest */
15435 /*             nodes to a given node, along with the associ- */
15436 /*             ated distances. */
15437 
15438 /*  INSIDE - Locates a point relative to a polygon on the */
15439 /*             surface of the sphere. */
15440 
15441 /*  INTRSC - Returns the point of intersection between a */
15442 /*             pair of great circle arcs. */
15443 
15444 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15445 /*             int. */
15446 
15447 /*  LEFT   - Locates a point relative to a great circle. */
15448 
15449 /*  NEARND - Returns the index of the nearest node to an */
15450 /*             arbitrary point, along with its squared */
15451 /*             distance. */
15452 
15453 /*  PROJCT - Applies a perspective-depth projection to a */
15454 /*             point in 3-space. */
15455 
15456 /*  SCOORD - Converts a point from Cartesian coordinates to */
15457 /*             spherical coordinates. */
15458 
15459 /*  STORE  - Forces a value to be stored in main memory so */
15460 /*             that the precision of floating point numbers */
15461 /*             in memory locations rather than registers is */
15462 /*             computed. */
15463 
15464 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15465 /*             coordinates on the unit sphere for input to */
15466 /*             Subroutine TRMESH. */
15467 
15468 /*  TRLIST - Converts the triangulation data structure to a */
15469 /*             triangle list more suitable for use in a fin- */
15470 /*             ite element code. */
15471 
15472 /*  TRLPRT - Prints the triangle list created by Subroutine */
15473 /*             TRLIST. */
15474 
15475 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15476 /*             nodes. */
15477 
15478 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15479 /*             file containing a triangulation plot. */
15480 
15481 /*  TRPRNT - Prints the triangulation data structure and, */
15482 /*             optionally, the nodal coordinates. */
15483 
15484 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15485 /*             file containing a Voronoi diagram plot. */
15486 
15487 
15488 /* On input: */
15489 
15490 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15491 
15492 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15493 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15494 /*               Z(K)) is referred to as node K, and K is re- */
15495 /*               ferred to as a nodal index.  It is required */
15496 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15497 /*               K.  The first three nodes must not be col- */
15498 /*               linear (lie on a common great circle). */
15499 
15500 /* The above parameters are not altered by this routine. */
15501 
15502 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15503 
15504 /*       LEND = Array of length at least N. */
15505 
15506 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15507 /*                        least N.  The space is used to */
15508 /*                        efficiently determine the nearest */
15509 /*                        triangulation node to each un- */
15510 /*                        processed node for use by ADDNOD. */
15511 
15512 /* On output: */
15513 
15514 /*       LIST = Set of nodal indexes which, along with LPTR, */
15515 /*              LEND, and LNEW, define the triangulation as a */
15516 /*              set of N adjacency lists -- counterclockwise- */
15517 /*              ordered sequences of neighboring nodes such */
15518 /*              that the first and last neighbors of a bound- */
15519 /*              ary node are boundary nodes (the first neigh- */
15520 /*              bor of an interior node is arbitrary).  In */
15521 /*              order to distinguish between interior and */
15522 /*              boundary nodes, the last neighbor of each */
15523 /*              boundary node is represented by the negative */
15524 /*              of its index. */
15525 
15526 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15527 /*              correspondence with the elements of LIST. */
15528 /*              LIST(LPTR(I)) indexes the node which follows */
15529 /*              LIST(I) in cyclical counterclockwise order */
15530 /*              (the first neighbor follows the last neigh- */
15531 /*              bor). */
15532 
15533 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15534 /*              points to the last neighbor of node K for */
15535 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15536 /*              only if K is a boundary node. */
15537 
15538 /*       LNEW = Pointer to the first empty location in LIST */
15539 /*              and LPTR (list length plus one).  LIST, LPTR, */
15540 /*              LEND, and LNEW are not altered if IER < 0, */
15541 /*              and are incomplete if IER > 0. */
15542 
15543 /*       NEAR,NEXT,DIST = Garbage. */
15544 
15545 /*       IER = Error indicator: */
15546 /*             IER =  0 if no errors were encountered. */
15547 /*             IER = -1 if N < 3 on input. */
15548 /*             IER = -2 if the first three nodes are */
15549 /*                      collinear. */
15550 /*             IER =  L if nodes L and M coincide for some */
15551 /*                      M > L.  The data structure represents */
15552 /*                      a triangulation of nodes 1 to M-1 in */
15553 /*                      this case. */
15554 
15555 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15556 /*                                INSERT, INTADD, JRAND, */
15557 /*                                LEFT, LSTPTR, STORE, SWAP, */
15558 /*                                SWPTST, TRFIND */
15559 
15560 /* Intrinsic function called by TRMESH:  ABS */
15561 
15562 /* *********************************************************** */
15563 
15564 
15565 /* Local parameters: */
15566 
15567 /* D =        (Negative cosine of) distance from node K to */
15568 /*              node I */
15569 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15570 /*              respectively */
15571 /* I,J =      Nodal indexes */
15572 /* I0 =       Index of the node preceding I in a sequence of */
15573 /*              unprocessed nodes:  I = NEXT(I0) */
15574 /* K =        Index of node to be added and DO-loop index: */
15575 /*              K > 3 */
15576 /* LP =       LIST index (pointer) of a neighbor of K */
15577 /* LPL =      Pointer to the last neighbor of K */
15578 /* NEXTI =    NEXT(I) */
15579 /* NN =       Local copy of N */
15580 
15581     /* Parameter adjustments */
15582     --dist;
15583     --next;
15584     --near__;
15585     --lend;
15586     --z__;
15587     --y;
15588     --x;
15589     --list;
15590     --lptr;
15591 
15592     /* Function Body */
15593     nn = *n;
15594     if (nn < 3) {
15595         *ier = -1;
15596         return 0;
15597     }
15598 
15599 /* Store the first triangle in the linked list. */
15600 
15601     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15602             z__[3])) {
15603 
15604 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15605 
15606         list[1] = 3;
15607         lptr[1] = 2;
15608         list[2] = -2;
15609         lptr[2] = 1;
15610         lend[1] = 2;
15611 
15612         list[3] = 1;
15613         lptr[3] = 4;
15614         list[4] = -3;
15615         lptr[4] = 3;
15616         lend[2] = 4;
15617 
15618         list[5] = 2;
15619         lptr[5] = 6;
15620         list[6] = -1;
15621         lptr[6] = 5;
15622         lend[3] = 6;
15623 
15624     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15625             y[3], &z__[3])) {
15626 
15627 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15628 /*     i.e., node 3 lies in the left hemisphere defined by */
15629 /*     arc 1->2. */
15630 
15631         list[1] = 2;
15632         lptr[1] = 2;
15633         list[2] = -3;
15634         lptr[2] = 1;
15635         lend[1] = 2;
15636 
15637         list[3] = 3;
15638         lptr[3] = 4;
15639         list[4] = -1;
15640         lptr[4] = 3;
15641         lend[2] = 4;
15642 
15643         list[5] = 1;
15644         lptr[5] = 6;
15645         list[6] = -2;
15646         lptr[6] = 5;
15647         lend[3] = 6;
15648 
15649     } else {
15650 
15651 /*   The first three nodes are collinear. */
15652 
15653         *ier = -2;
15654         return 0;
15655     }
15656 
15657 /* Initialize LNEW and test for N = 3. */
15658 
15659     *lnew = 7;
15660     if (nn == 3) {
15661         *ier = 0;
15662         return 0;
15663     }
15664 
15665 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15666 /*   used to obtain an expected-time (N*log(N)) incremental */
15667 /*   algorithm by enabling constant search time for locating */
15668 /*   each new node in the triangulation. */
15669 
15670 /* For each unprocessed node K, NEAR(K) is the index of the */
15671 /*   triangulation node closest to K (used as the starting */
15672 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15673 /*   is an increasing function of the arc length (angular */
15674 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15675 /*   length a. */
15676 
15677 /* Since it is necessary to efficiently find the subset of */
15678 /*   unprocessed nodes associated with each triangulation */
15679 /*   node J (those that have J as their NEAR entries), the */
15680 /*   subsets are stored in NEAR and NEXT as follows:  for */
15681 /*   each node J in the triangulation, I = NEAR(J) is the */
15682 /*   first unprocessed node in J's set (with I = 0 if the */
15683 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15684 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15685 /*   set are initially ordered by increasing indexes (which */
15686 /*   maximizes efficiency) but that ordering is not main- */
15687 /*   tained as the data structure is updated. */
15688 
15689 /* Initialize the data structure for the single triangle. */
15690 
15691     near__[1] = 0;
15692     near__[2] = 0;
15693     near__[3] = 0;
15694     for (k = nn; k >= 4; --k) {
15695         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15696         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15697         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15698         if (d1 <= d2 && d1 <= d3) {
15699             near__[k] = 1;
15700             dist[k] = d1;
15701             next[k] = near__[1];
15702             near__[1] = k;
15703         } else if (d2 <= d1 && d2 <= d3) {
15704             near__[k] = 2;
15705             dist[k] = d2;
15706             next[k] = near__[2];
15707             near__[2] = k;
15708         } else {
15709             near__[k] = 3;
15710             dist[k] = d3;
15711             next[k] = near__[3];
15712             near__[3] = k;
15713         }
15714 /* L1: */
15715     }
15716 
15717 /* Add the remaining nodes */
15718 
15719     i__1 = nn;
15720     for (k = 4; k <= i__1; ++k) {
15721         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15722                 lend[1], lnew, ier);
15723         if (*ier != 0) {
15724             return 0;
15725         }
15726 
15727 /* Remove K from the set of unprocessed nodes associated */
15728 /*   with NEAR(K). */
15729 
15730         i__ = near__[k];
15731         if (near__[i__] == k) {
15732             near__[i__] = next[k];
15733         } else {
15734             i__ = near__[i__];
15735 L2:
15736             i0 = i__;
15737             i__ = next[i0];
15738             if (i__ != k) {
15739                 goto L2;
15740             }
15741             next[i0] = next[k];
15742         }
15743         near__[k] = 0;
15744 
15745 /* Loop on neighbors J of node K. */
15746 
15747         lpl = lend[k];
15748         lp = lpl;
15749 L3:
15750         lp = lptr[lp];
15751         j = (i__2 = list[lp], abs(i__2));
15752 
15753 /* Loop on elements I in the sequence of unprocessed nodes */
15754 /*   associated with J:  K is a candidate for replacing J */
15755 /*   as the nearest triangulation node to I.  The next value */
15756 /*   of I in the sequence, NEXT(I), must be saved before I */
15757 /*   is moved because it is altered by adding I to K's set. */
15758 
15759         i__ = near__[j];
15760 L4:
15761         if (i__ == 0) {
15762             goto L5;
15763         }
15764         nexti = next[i__];
15765 
15766 /* Test for the distance from I to K less than the distance */
15767 /*   from I to J. */
15768 
15769         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15770         if (d__ < dist[i__]) {
15771 
15772 /* Replace J by K as the nearest triangulation node to I: */
15773 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15774 /*   of unprocessed nodes and add it to K's set. */
15775 
15776             near__[i__] = k;
15777             dist[i__] = d__;
15778             if (i__ == near__[j]) {
15779                 near__[j] = nexti;
15780             } else {
15781                 next[i0] = nexti;
15782             }
15783             next[i__] = near__[k];
15784             near__[k] = i__;
15785         } else {
15786             i0 = i__;
15787         }
15788 
15789 /* Bottom of loop on I. */
15790 
15791         i__ = nexti;
15792         goto L4;
15793 
15794 /* Bottom of loop on neighbors J. */
15795 
15796 L5:
15797         if (lp != lpl) {
15798             goto L3;
15799         }
15800 /* L6: */
15801     }
15802     return 0;
15803 } /* trmesh_ */
15804 
15805 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
15806         elat, double *elon, double *a, int *n, double *x,
15807         double *y, double *z__, int *list, int *lptr, int
15808         *lend, char *, long int *numbr, int *ier, short )
15809 {
15810     /* Initialized data */
15811 
15812     static long int annot = TRUE_;
15813     static double fsizn = 10.;
15814     static double fsizt = 16.;
15815     static double tol = .5;
15816 
15817     /* System generated locals */
15818     int i__1, i__2;
15819     double d__1;
15820 
15821     /* Builtin functions */
15822     //double atan(double), sin(double);
15823     //int i_dnnt(double *);
15824     //double cos(double), sqrt(double);
15825 
15826     /* Local variables */
15827     static double t;
15828     static int n0, n1;
15829     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15830     static int ir, lp;
15831     static double ex, ey, ez, wr, tx, ty;
15832     static int lpl;
15833     static double wrs;
15834     static int ipx1, ipx2, ipy1, ipy2, nseg;
15835     extern /* Subroutine */ int drwarc_(int *, double *, double *,
15836              double *, int *);
15837 
15838 
15839 /* *********************************************************** */
15840 
15841 /*                                              From STRIPACK */
15842 /*                                            Robert J. Renka */
15843 /*                                  Dept. of Computer Science */
15844 /*                                       Univ. of North Texas */
15845 /*                                           renka@cs.unt.edu */
15846 /*                                                   03/04/03 */
15847 
15848 /*   This subroutine creates a level-2 Encapsulated Post- */
15849 /* script (EPS) file containing a graphical display of a */
15850 /* triangulation of a set of nodes on the surface of the unit */
15851 /* sphere.  The visible portion of the triangulation is */
15852 /* projected onto the plane that contains the origin and has */
15853 /* normal defined by a user-specified eye-position. */
15854 
15855 
15856 /* On input: */
15857 
15858 /*       LUN = long int unit number in the range 0 to 99. */
15859 /*             The unit should be opened with an appropriate */
15860 /*             file name before the call to this routine. */
15861 
15862 /*       PLTSIZ = Plot size in inches.  A circular window in */
15863 /*                the projection plane is mapped to a circu- */
15864 /*                lar viewport with diameter equal to .88* */
15865 /*                PLTSIZ (leaving room for labels outside the */
15866 /*                viewport).  The viewport is centered on the */
15867 /*                8.5 by 11 inch page, and its boundary is */
15868 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
15869 
15870 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
15871 /*                   the center of projection E (the center */
15872 /*                   of the plot).  The projection plane is */
15873 /*                   the plane that contains the origin and */
15874 /*                   has E as unit normal.  In a rotated */
15875 /*                   coordinate system for which E is the */
15876 /*                   north pole, the projection plane con- */
15877 /*                   tains the equator, and only northern */
15878 /*                   hemisphere nodes are visible (from the */
15879 /*                   point at infinity in the direction E). */
15880 /*                   These are projected orthogonally onto */
15881 /*                   the projection plane (by zeroing the z- */
15882 /*                   component in the rotated coordinate */
15883 /*                   system).  ELAT and ELON must be in the */
15884 /*                   range -90 to 90 and -180 to 180, respec- */
15885 /*                   tively. */
15886 
15887 /*       A = Angular distance in degrees from E to the boun- */
15888 /*           dary of a circular window against which the */
15889 /*           triangulation is clipped.  The projected window */
15890 /*           is a disk of radius r = Sin(A) centered at the */
15891 /*           origin, and only visible nodes whose projections */
15892 /*           are within distance r of the origin are included */
15893 /*           in the plot.  Thus, if A = 90, the plot includes */
15894 /*           the entire hemisphere centered at E.  0 .LT. A */
15895 /*           .LE. 90. */
15896 
15897 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15898 
15899 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15900 /*               coordinates of the nodes (unit vectors). */
15901 
15902 /*       LIST,LPTR,LEND = Data structure defining the trian- */
15903 /*                        gulation.  Refer to Subroutine */
15904 /*                        TRMESH. */
15905 
15906 /*       TITLE = Type CHARACTER variable or constant contain- */
15907 /*               ing a string to be centered above the plot. */
15908 /*               The string must be enclosed in parentheses; */
15909 /*               i.e., the first and last characters must be */
15910 /*               '(' and ')', respectively, but these are not */
15911 /*               displayed.  TITLE may have at most 80 char- */
15912 /*               acters including the parentheses. */
15913 
15914 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
15915 /*               nodal indexes are plotted next to the nodes. */
15916 
15917 /* Input parameters are not altered by this routine. */
15918 
15919 /* On output: */
15920 
15921 /*       IER = Error indicator: */
15922 /*             IER = 0 if no errors were encountered. */
15923 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
15924 /*                     valid range. */
15925 /*             IER = 2 if ELAT, ELON, or A is outside its */
15926 /*                     valid range. */
15927 /*             IER = 3 if an error was encountered in writing */
15928 /*                     to unit LUN. */
15929 
15930 /*   The values in the data statement below may be altered */
15931 /* in order to modify various plotting options. */
15932 
15933 /* Module required by TRPLOT:  DRWARC */
15934 
15935 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
15936 /*                                          DBLE, NINT, SIN, */
15937 /*                                          SQRT */
15938 
15939 /* *********************************************************** */
15940 
15941 
15942     /* Parameter adjustments */
15943     --lend;
15944     --z__;
15945     --y;
15946     --x;
15947     --list;
15948     --lptr;
15949 
15950     /* Function Body */
15951 
15952 /* Local parameters: */
15953 
15954 /* ANNOT =     long int variable with value TRUE iff the plot */
15955 /*               is to be annotated with the values of ELAT, */
15956 /*               ELON, and A */
15957 /* CF =        Conversion factor for degrees to radians */
15958 /* CT =        Cos(ELAT) */
15959 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
15960 /* FSIZN =     Font size in points for labeling nodes with */
15961 /*               their indexes if NUMBR = TRUE */
15962 /* FSIZT =     Font size in points for the title (and */
15963 /*               annotation if ANNOT = TRUE) */
15964 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
15965 /*               left corner of the bounding box or viewport */
15966 /*               box */
15967 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
15968 /*               right corner of the bounding box or viewport */
15969 /*               box */
15970 /* IR =        Half the width (height) of the bounding box or */
15971 /*               viewport box in points -- viewport radius */
15972 /* LP =        LIST index (pointer) */
15973 /* LPL =       Pointer to the last neighbor of N0 */
15974 /* N0 =        Index of a node whose incident arcs are to be */
15975 /*               drawn */
15976 /* N1 =        Neighbor of N0 */
15977 /* NSEG =      Number of line segments used by DRWARC in a */
15978 /*               polygonal approximation to a projected edge */
15979 /* P0 =        Coordinates of N0 in the rotated coordinate */
15980 /*               system or label location (first two */
15981 /*               components) */
15982 /* P1 =        Coordinates of N1 in the rotated coordinate */
15983 /*               system or intersection of edge N0-N1 with */
15984 /*               the equator (in the rotated coordinate */
15985 /*               system) */
15986 /* R11...R23 = Components of the first two rows of a rotation */
15987 /*               that maps E to the north pole (0,0,1) */
15988 /* SF =        Scale factor for mapping world coordinates */
15989 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
15990 /*               to viewport coordinates in [IPX1,IPX2] X */
15991 /*               [IPY1,IPY2] */
15992 /* T =         Temporary variable */
15993 /* TOL =       Maximum distance in points between a projected */
15994 /*               triangulation edge and its approximation by */
15995 /*               a polygonal curve */
15996 /* TX,TY =     Translation vector for mapping world coordi- */
15997 /*               nates to viewport coordinates */
15998 /* WR =        Window radius r = Sin(A) */
15999 /* WRS =       WR**2 */
16000 
16001 
16002 /* Test for invalid parameters. */
16003 
16004     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16005         goto L11;
16006     }
16007     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16008         goto L12;
16009     }
16010 
16011 /* Compute a conversion factor CF for degrees to radians */
16012 /*   and compute the window radius WR. */
16013 
16014     cf = atan(1.) / 45.;
16015     wr = sin(cf * *a);
16016     wrs = wr * wr;
16017 
16018 /* Compute the lower left (IPX1,IPY1) and upper right */
16019 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16020 /*   The coordinates, specified in default user space units */
16021 /*   (points, at 72 points/inch with origin at the lower */
16022 /*   left corner of the page), are chosen to preserve the */
16023 /*   square aspect ratio, and to center the plot on the 8.5 */
16024 /*   by 11 inch page.  The center of the page is (306,396), */
16025 /*   and IR = PLTSIZ/2 in points. */
16026 
16027     d__1 = *pltsiz * 36.;
16028     ir = i_dnnt(&d__1);
16029     ipx1 = 306 - ir;
16030     ipx2 = ir + 306;
16031     ipy1 = 396 - ir;
16032     ipy2 = ir + 396;
16033 
16034 /* Output header comments. */
16035 
16036 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16037 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16038 /*     .        '%%BoundingBox:',4I4/ */
16039 /*     .        '%%Title:  Triangulation'/ */
16040 /*     .        '%%Creator:  STRIPACK'/ */
16041 /*     .        '%%EndComments') */
16042 
16043 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16044 /*   of a viewport box obtained by shrinking the bounding box */
16045 /*   by 12% in each dimension. */
16046 
16047     d__1 = (double) ir * .88;
16048     ir = i_dnnt(&d__1);
16049     ipx1 = 306 - ir;
16050     ipx2 = ir + 306;
16051     ipy1 = 396 - ir;
16052     ipy2 = ir + 396;
16053 
16054 /* Set the line thickness to 2 points, and draw the */
16055 /*   viewport boundary. */
16056 
16057     t = 2.;
16058 /*      WRITE (LUN,110,ERR=13) T */
16059 /*      WRITE (LUN,120,ERR=13) IR */
16060 /*      WRITE (LUN,130,ERR=13) */
16061 /*  110 FORMAT (F12.6,' setlinewidth') */
16062 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16063 /*  130 FORMAT ('stroke') */
16064 
16065 /* Set up an affine mapping from the window box [-WR,WR] X */
16066 /*   [-WR,WR] to the viewport box. */
16067 
16068     sf = (double) ir / wr;
16069     tx = ipx1 + sf * wr;
16070     ty = ipy1 + sf * wr;
16071 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16072 /*  140 FORMAT (2F12.6,' translate'/ */
16073 /*    .        2F12.6,' scale') */
16074 
16075 /* The line thickness must be changed to reflect the new */
16076 /*   scaling which is applied to all subsequent output. */
16077 /*   Set it to 1.0 point. */
16078 
16079     t = 1. / sf;
16080 /*      WRITE (LUN,110,ERR=13) T */
16081 
16082 /* Save the current graphics state, and set the clip path to */
16083 /*   the boundary of the window. */
16084 
16085 /*      WRITE (LUN,150,ERR=13) */
16086 /*      WRITE (LUN,160,ERR=13) WR */
16087 /*      WRITE (LUN,170,ERR=13) */
16088 /*  150 FORMAT ('gsave') */
16089 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16090 /*  170 FORMAT ('clip newpath') */
16091 
16092 /* Compute the Cartesian coordinates of E and the components */
16093 /*   of a rotation R which maps E to the north pole (0,0,1). */
16094 /*   R is taken to be a rotation about the z-axis (into the */
16095 /*   yz-plane) followed by a rotation about the x-axis chosen */
16096 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16097 /*   E is the north or south pole. */
16098 
16099 /*           ( R11  R12  0   ) */
16100 /*       R = ( R21  R22  R23 ) */
16101 /*           ( EX   EY   EZ  ) */
16102 
16103     t = cf * *elon;
16104     ct = cos(cf * *elat);
16105     ex = ct * cos(t);
16106     ey = ct * sin(t);
16107     ez = sin(cf * *elat);
16108     if (ct != 0.) {
16109         r11 = -ey / ct;
16110         r12 = ex / ct;
16111     } else {
16112         r11 = 0.;
16113         r12 = 1.;
16114     }
16115     r21 = -ez * r12;
16116     r22 = ez * r11;
16117     r23 = ct;
16118 
16119 /* Loop on visible nodes N0 that project to points */
16120 /*   (P0(1),P0(2)) in the window. */
16121 
16122     i__1 = *n;
16123     for (n0 = 1; n0 <= i__1; ++n0) {
16124         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16125         if (p0[2] < 0.) {
16126             goto L3;
16127         }
16128         p0[0] = r11 * x[n0] + r12 * y[n0];
16129         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16130         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16131             goto L3;
16132         }
16133         lpl = lend[n0];
16134         lp = lpl;
16135 
16136 /* Loop on neighbors N1 of N0.  LPL points to the last */
16137 /*   neighbor of N0.  Copy the components of N1 into P. */
16138 
16139 L1:
16140         lp = lptr[lp];
16141         n1 = (i__2 = list[lp], abs(i__2));
16142         p1[0] = r11 * x[n1] + r12 * y[n1];
16143         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16144         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16145         if (p1[2] < 0.) {
16146 
16147 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16148 /*     intersection of edge N0-N1 with the equator so that */
16149 /*     the edge is clipped properly.  P1(3) is set to 0. */
16150 
16151             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16152             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16153             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16154             p1[0] /= t;
16155             p1[1] /= t;
16156         }
16157 
16158 /*   If node N1 is in the window and N1 < N0, bypass edge */
16159 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16160 
16161         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16162             goto L2;
16163         }
16164 
16165 /*   Add the edge to the path.  (TOL is converted to world */
16166 /*     coordinates.) */
16167 
16168         if (p1[2] < 0.) {
16169             p1[2] = 0.;
16170         }
16171         d__1 = tol / sf;
16172         drwarc_(lun, p0, p1, &d__1, &nseg);
16173 
16174 /* Bottom of loops. */
16175 
16176 L2:
16177         if (lp != lpl) {
16178             goto L1;
16179         }
16180 L3:
16181         ;
16182     }
16183 
16184 /* Paint the path and restore the saved graphics state (with */
16185 /*   no clip path). */
16186 
16187 /*      WRITE (LUN,130,ERR=13) */
16188 /*      WRITE (LUN,190,ERR=13) */
16189 /*  190 FORMAT ('grestore') */
16190     if (*numbr) {
16191 
16192 /* Nodes in the window are to be labeled with their indexes. */
16193 /*   Convert FSIZN from points to world coordinates, and */
16194 /*   output the commands to select a font and scale it. */
16195 
16196         t = fsizn / sf;
16197 /*        WRITE (LUN,200,ERR=13) T */
16198 /*  200   FORMAT ('/Helvetica findfont'/ */
16199 /*     .          F12.6,' scalefont setfont') */
16200 
16201 /* Loop on visible nodes N0 that project to points */
16202 /*   P0 = (P0(1),P0(2)) in the window. */
16203 
16204         i__1 = *n;
16205         for (n0 = 1; n0 <= i__1; ++n0) {
16206             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16207                 goto L4;
16208             }
16209             p0[0] = r11 * x[n0] + r12 * y[n0];
16210             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16211             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16212                 goto L4;
16213             }
16214 
16215 /*   Move to P0 and draw the label N0.  The first character */
16216 /*     will will have its lower left corner about one */
16217 /*     character width to the right of the nodal position. */
16218 
16219 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16220 /*          WRITE (LUN,220,ERR=13) N0 */
16221 /*  210     FORMAT (2F12.6,' moveto') */
16222 /*  220     FORMAT ('(',I3,') show') */
16223 L4:
16224             ;
16225         }
16226     }
16227 
16228 /* Convert FSIZT from points to world coordinates, and output */
16229 /*   the commands to select a font and scale it. */
16230 
16231     t = fsizt / sf;
16232 /*      WRITE (LUN,200,ERR=13) T */
16233 
16234 /* Display TITLE centered above the plot: */
16235 
16236     p0[1] = wr + t * 3.;
16237 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16238 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16239 /*     .        ' moveto') */
16240 /*      WRITE (LUN,240,ERR=13) TITLE */
16241 /*  240 FORMAT (A80/'  show') */
16242     if (annot) {
16243 
16244 /* Display the window center and radius below the plot. */
16245 
16246         p0[0] = -wr;
16247         p0[1] = -wr - 50. / sf;
16248 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16249 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16250         p0[1] -= t * 2.;
16251 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16252 /*        WRITE (LUN,260,ERR=13) A */
16253 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16254 /*     .          ',  ELON = ',F8.2,') show') */
16255 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16256     }
16257 
16258 /* Paint the path and output the showpage command and */
16259 /*   end-of-file indicator. */
16260 
16261 /*      WRITE (LUN,270,ERR=13) */
16262 /*  270 FORMAT ('stroke'/ */
16263 /*     .        'showpage'/ */
16264 /*     .        '%%EOF') */
16265 
16266 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16267 /*   indicator (to eliminate a timeout error message): */
16268 /*   ASCII 4. */
16269 
16270 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16271 /*  280 FORMAT (A1) */
16272 
16273 /* No error encountered. */
16274 
16275     *ier = 0;
16276     return 0;
16277 
16278 /* Invalid input parameter LUN, PLTSIZ, or N. */
16279 
16280 L11:
16281     *ier = 1;
16282     return 0;
16283 
16284 /* Invalid input parameter ELAT, ELON, or A. */
16285 
16286 L12:
16287     *ier = 2;
16288     return 0;
16289 
16290 /* Error writing to unit LUN. */
16291 
16292 /* L13: */
16293     *ier = 3;
16294     return 0;
16295 } /* trplot_ */
16296 
16297 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16298         double *z__, int *iflag, int *list, int *lptr,
16299         int *lend, int *lout)
16300 {
16301     /* Initialized data */
16302 
16303     static int nmax = 9999;
16304     static int nlmax = 58;
16305 
16306     /* System generated locals */
16307     int i__1;
16308 
16309     /* Local variables */
16310     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16311             400];
16312 
16313 
16314 /* *********************************************************** */
16315 
16316 /*                                              From STRIPACK */
16317 /*                                            Robert J. Renka */
16318 /*                                  Dept. of Computer Science */
16319 /*                                       Univ. of North Texas */
16320 /*                                           renka@cs.unt.edu */
16321 /*                                                   07/25/98 */
16322 
16323 /*   This subroutine prints the triangulation adjacency lists */
16324 /* created by Subroutine TRMESH and, optionally, the nodal */
16325 /* coordinates (either latitude and longitude or Cartesian */
16326 /* coordinates) on long int unit LOUT.  The list of neighbors */
16327 /* of a boundary node is followed by index 0.  The numbers of */
16328 /* boundary nodes, triangles, and arcs are also printed. */
16329 
16330 
16331 /* On input: */
16332 
16333 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16334 /*           and N .LE. 9999. */
16335 
16336 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16337 /*               coordinates of the nodes if IFLAG = 0, or */
16338 /*               (X and Y only) arrays of length N containing */
16339 /*               longitude and latitude, respectively, if */
16340 /*               IFLAG > 0, or unused dummy parameters if */
16341 /*               IFLAG < 0. */
16342 
16343 /*       IFLAG = Nodal coordinate option indicator: */
16344 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16345 /*                         Cartesian coordinates) are to be */
16346 /*                         printed (to 6 decimal places). */
16347 /*               IFLAG > 0 if only X and Y (assumed to con- */
16348 /*                         tain longitude and latitude) are */
16349 /*                         to be printed (to 6 decimal */
16350 /*                         places). */
16351 /*               IFLAG < 0 if only the adjacency lists are to */
16352 /*                         be printed. */
16353 
16354 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16355 /*                        gulation.  Refer to Subroutine */
16356 /*                        TRMESH. */
16357 
16358 /*       LOUT = long int unit for output.  If LOUT is not in */
16359 /*              the range 0 to 99, output is written to */
16360 /*              long int unit 6. */
16361 
16362 /* Input parameters are not altered by this routine. */
16363 
16364 /* On output: */
16365 
16366 /*   The adjacency lists and nodal coordinates (as specified */
16367 /* by IFLAG) are written to unit LOUT. */
16368 
16369 /* Modules required by TRPRNT:  None */
16370 
16371 /* *********************************************************** */
16372 
16373     /* Parameter adjustments */
16374     --lend;
16375     --z__;
16376     --y;
16377     --x;
16378     --list;
16379     --lptr;
16380 
16381     /* Function Body */
16382 
16383 /* Local parameters: */
16384 
16385 /* I =     NABOR index (1 to K) */
16386 /* INC =   Increment for NL associated with an adjacency list */
16387 /* K =     Counter and number of neighbors of NODE */
16388 /* LP =    LIST pointer of a neighbor of NODE */
16389 /* LPL =   Pointer to the last neighbor of NODE */
16390 /* LUN =   long int unit for output (copy of LOUT) */
16391 /* NA =    Number of arcs in the triangulation */
16392 /* NABOR = Array containing the adjacency list associated */
16393 /*           with NODE, with zero appended if NODE is a */
16394 /*           boundary node */
16395 /* NB =    Number of boundary nodes encountered */
16396 /* ND =    Index of a neighbor of NODE (or negative index) */
16397 /* NL =    Number of lines that have been printed on the */
16398 /*           current page */
16399 /* NLMAX = Maximum number of print lines per page (except */
16400 /*           for the last page which may have two addi- */
16401 /*           tional lines) */
16402 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16403 /* NODE =  Index of a node and DO-loop index (1 to N) */
16404 /* NN =    Local copy of N */
16405 /* NT =    Number of triangles in the triangulation */
16406 
16407     nn = *n;
16408     lun = *lout;
16409     if (lun < 0 || lun > 99) {
16410         lun = 6;
16411     }
16412 
16413 /* Print a heading and test the range of N. */
16414 
16415 /*      WRITE (LUN,100) NN */
16416     if (nn < 3 || nn > nmax) {
16417 
16418 /* N is outside its valid range. */
16419 
16420 /*        WRITE (LUN,110) */
16421         return 0;
16422     }
16423 
16424 /* Initialize NL (the number of lines printed on the current */
16425 /*   page) and NB (the number of boundary nodes encountered). */
16426 
16427     nl = 6;
16428     nb = 0;
16429     if (*iflag < 0) {
16430 
16431 /* Print LIST only.  K is the number of neighbors of NODE */
16432 /*   that have been stored in NABOR. */
16433 
16434 /*        WRITE (LUN,101) */
16435         i__1 = nn;
16436         for (node = 1; node <= i__1; ++node) {
16437             lpl = lend[node];
16438             lp = lpl;
16439             k = 0;
16440 
16441 L1:
16442             ++k;
16443             lp = lptr[lp];
16444             nd = list[lp];
16445             nabor[k - 1] = nd;
16446             if (lp != lpl) {
16447                 goto L1;
16448             }
16449             if (nd <= 0) {
16450 
16451 /*   NODE is a boundary node.  Correct the sign of the last */
16452 /*     neighbor, add 0 to the end of the list, and increment */
16453 /*     NB. */
16454 
16455                 nabor[k - 1] = -nd;
16456                 ++k;
16457                 nabor[k - 1] = 0;
16458                 ++nb;
16459             }
16460 
16461 /*   Increment NL and print the list of neighbors. */
16462 
16463             inc = (k - 1) / 14 + 2;
16464             nl += inc;
16465             if (nl > nlmax) {
16466 /*            WRITE (LUN,108) */
16467                 nl = inc;
16468             }
16469 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16470 /*          IF (K .NE. 14) */
16471 /*           WRITE (LUN,107) */
16472 /* L2: */
16473         }
16474     } else if (*iflag > 0) {
16475 
16476 /* Print X (longitude), Y (latitude), and LIST. */
16477 
16478 /*        WRITE (LUN,102) */
16479         i__1 = nn;
16480         for (node = 1; node <= i__1; ++node) {
16481             lpl = lend[node];
16482             lp = lpl;
16483             k = 0;
16484 
16485 L3:
16486             ++k;
16487             lp = lptr[lp];
16488             nd = list[lp];
16489             nabor[k - 1] = nd;
16490             if (lp != lpl) {
16491                 goto L3;
16492             }
16493             if (nd <= 0) {
16494 
16495 /*   NODE is a boundary node. */
16496 
16497                 nabor[k - 1] = -nd;
16498                 ++k;
16499                 nabor[k - 1] = 0;
16500                 ++nb;
16501             }
16502 
16503 /*   Increment NL and print X, Y, and NABOR. */
16504 
16505             inc = (k - 1) / 8 + 2;
16506             nl += inc;
16507             if (nl > nlmax) {
16508 /*            WRITE (LUN,108) */
16509                 nl = inc;
16510             }
16511 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16512 /*          IF (K .NE. 8) */
16513 /*           PRINT *,K */
16514 /*           WRITE (LUN,107) */
16515 /* L4: */
16516         }
16517     } else {
16518 
16519 /* Print X, Y, Z, and LIST. */
16520 
16521 /*        WRITE (LUN,103) */
16522         i__1 = nn;
16523         for (node = 1; node <= i__1; ++node) {
16524             lpl = lend[node];
16525             lp = lpl;
16526             k = 0;
16527 
16528 L5:
16529             ++k;
16530             lp = lptr[lp];
16531             nd = list[lp];
16532             nabor[k - 1] = nd;
16533             if (lp != lpl) {
16534                 goto L5;
16535             }
16536             if (nd <= 0) {
16537 
16538 /*   NODE is a boundary node. */
16539 
16540                 nabor[k - 1] = -nd;
16541                 ++k;
16542                 nabor[k - 1] = 0;
16543                 ++nb;
16544             }
16545 
16546 /*   Increment NL and print X, Y, Z, and NABOR. */
16547 
16548             inc = (k - 1) / 5 + 2;
16549             nl += inc;
16550             if (nl > nlmax) {
16551 /*            WRITE (LUN,108) */
16552                 nl = inc;
16553             }
16554 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16555 /*          IF (K .NE. 5) */
16556 /*           print *,K */
16557 /*           WRITE (LUN,107) */
16558 /* L6: */
16559         }
16560     }
16561 
16562 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16563 /*   triangles). */
16564 
16565     if (nb != 0) {
16566         na = nn * 3 - nb - 3;
16567         nt = (nn << 1) - nb - 2;
16568     } else {
16569         na = nn * 3 - 6;
16570         nt = (nn << 1) - 4;
16571     }
16572 /*      WRITE (LUN,109) NB, NA, NT */
16573     return 0;
16574 
16575 /* Print formats: */
16576 
16577 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16578 /*     .        'Structure,  N = ',I5//) */
16579 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16580 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16581 /*     .        18X,'Neighbors of Node'//) */
16582 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16583 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16584 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16585 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16586 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16587 /*  107 FORMAT (1X) */
16588 /*  108 FORMAT (///) */
16589 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16590 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16591 /*     .        ' Triangles') */
16592 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16593 /*     .        ' range ***') */
16594 } /* trprnt_ */
16595 
16596 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16597         elat, double *elon, double *a, int *n, double *x,
16598         double *y, double *z__, int *nt, int *listc, int *
16599         lptr, int *lend, double *xc, double *yc, double *zc,
16600         char *, long int *numbr, int *ier, short)
16601 {
16602     /* Initialized data */
16603 
16604     static long int annot = TRUE_;
16605     static double fsizn = 10.;
16606     static double fsizt = 16.;
16607     static double tol = .5;
16608 
16609     /* System generated locals */
16610     int i__1;
16611     double d__1;
16612 
16613     /* Builtin functions */
16614     //double atan(double), sin(double);
16615     //int i_dnnt(double *);
16616     //double cos(double), sqrt(double);
16617 
16618     /* Local variables */
16619     static double t;
16620     static int n0;
16621     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16622             sf;
16623     static int ir, lp;
16624     static double ex, ey, ez, wr, tx, ty;
16625     static long int in1, in2;
16626     static int kv1, kv2, lpl;
16627     static double wrs;
16628     static int ipx1, ipx2, ipy1, ipy2, nseg;
16629     extern /* Subroutine */ int drwarc_(int *, double *, double *,
16630              double *, int *);
16631 
16632 
16633 /* *********************************************************** */
16634 
16635 /*                                              From STRIPACK */
16636 /*                                            Robert J. Renka */
16637 /*                                  Dept. of Computer Science */
16638 /*                                       Univ. of North Texas */
16639 /*                                           renka@cs.unt.edu */
16640 /*                                                   03/04/03 */
16641 
16642 /*   This subroutine creates a level-2 Encapsulated Post- */
16643 /* script (EPS) file containing a graphical depiction of a */
16644 /* Voronoi diagram of a set of nodes on the unit sphere. */
16645 /* The visible portion of the diagram is projected orthog- */
16646 /* onally onto the plane that contains the origin and has */
16647 /* normal defined by a user-specified eye-position. */
16648 
16649 /*   The parameters defining the Voronoi diagram may be com- */
16650 /* puted by Subroutine CRLIST. */
16651 
16652 
16653 /* On input: */
16654 
16655 /*       LUN = long int unit number in the range 0 to 99. */
16656 /*             The unit should be opened with an appropriate */
16657 /*             file name before the call to this routine. */
16658 
16659 /*       PLTSIZ = Plot size in inches.  A circular window in */
16660 /*                the projection plane is mapped to a circu- */
16661 /*                lar viewport with diameter equal to .88* */
16662 /*                PLTSIZ (leaving room for labels outside the */
16663 /*                viewport).  The viewport is centered on the */
16664 /*                8.5 by 11 inch page, and its boundary is */
16665 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16666 
16667 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16668 /*                   the center of projection E (the center */
16669 /*                   of the plot).  The projection plane is */
16670 /*                   the plane that contains the origin and */
16671 /*                   has E as unit normal.  In a rotated */
16672 /*                   coordinate system for which E is the */
16673 /*                   north pole, the projection plane con- */
16674 /*                   tains the equator, and only northern */
16675 /*                   hemisphere points are visible (from the */
16676 /*                   point at infinity in the direction E). */
16677 /*                   These are projected orthogonally onto */
16678 /*                   the projection plane (by zeroing the z- */
16679 /*                   component in the rotated coordinate */
16680 /*                   system).  ELAT and ELON must be in the */
16681 /*                   range -90 to 90 and -180 to 180, respec- */
16682 /*                   tively. */
16683 
16684 /*       A = Angular distance in degrees from E to the boun- */
16685 /*           dary of a circular window against which the */
16686 /*           Voronoi diagram is clipped.  The projected win- */
16687 /*           dow is a disk of radius r = Sin(A) centered at */
16688 /*           the origin, and only visible vertices whose */
16689 /*           projections are within distance r of the origin */
16690 /*           are included in the plot.  Thus, if A = 90, the */
16691 /*           plot includes the entire hemisphere centered at */
16692 /*           E.  0 .LT. A .LE. 90. */
16693 
16694 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16695 /*           regions.  N .GE. 3. */
16696 
16697 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16698 /*               coordinates of the nodes (unit vectors). */
16699 
16700 /*       NT = Number of Voronoi region vertices (triangles, */
16701 /*            including those in the extended triangulation */
16702 /*            if the number of boundary nodes NB is nonzero): */
16703 /*            NT = 2*N-4. */
16704 
16705 /*       LISTC = Array of length 3*NT containing triangle */
16706 /*               indexes (indexes to XC, YC, and ZC) stored */
16707 /*               in 1-1 correspondence with LIST/LPTR entries */
16708 /*               (or entries that would be stored in LIST for */
16709 /*               the extended triangulation):  the index of */
16710 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16711 /*               LISTC(L), and LISTC(M), where LIST(K), */
16712 /*               LIST(L), and LIST(M) are the indexes of N2 */
16713 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16714 /*               and N1 as a neighbor of N3.  The Voronoi */
16715 /*               region associated with a node is defined by */
16716 /*               the CCW-ordered sequence of circumcenters in */
16717 /*               one-to-one correspondence with its adjacency */
16718 /*               list (in the extended triangulation). */
16719 
16720 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16721 /*              set of pointers (LISTC indexes) in one-to-one */
16722 /*              correspondence with the elements of LISTC. */
16723 /*              LISTC(LPTR(I)) indexes the triangle which */
16724 /*              follows LISTC(I) in cyclical counterclockwise */
16725 /*              order (the first neighbor follows the last */
16726 /*              neighbor). */
16727 
16728 /*       LEND = Array of length N containing a set of */
16729 /*              pointers to triangle lists.  LP = LEND(K) */
16730 /*              points to a triangle (indexed by LISTC(LP)) */
16731 /*              containing node K for K = 1 to N. */
16732 
16733 /*       XC,YC,ZC = Arrays of length NT containing the */
16734 /*                  Cartesian coordinates of the triangle */
16735 /*                  circumcenters (Voronoi vertices). */
16736 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16737 
16738 /*       TITLE = Type CHARACTER variable or constant contain- */
16739 /*               ing a string to be centered above the plot. */
16740 /*               The string must be enclosed in parentheses; */
16741 /*               i.e., the first and last characters must be */
16742 /*               '(' and ')', respectively, but these are not */
16743 /*               displayed.  TITLE may have at most 80 char- */
16744 /*               acters including the parentheses. */
16745 
16746 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16747 /*               nodal indexes are plotted at the Voronoi */
16748 /*               region centers. */
16749 
16750 /* Input parameters are not altered by this routine. */
16751 
16752 /* On output: */
16753 
16754 /*       IER = Error indicator: */
16755 /*             IER = 0 if no errors were encountered. */
16756 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16757 /*                     its valid range. */
16758 /*             IER = 2 if ELAT, ELON, or A is outside its */
16759 /*                     valid range. */
16760 /*             IER = 3 if an error was encountered in writing */
16761 /*                     to unit LUN. */
16762 
16763 /* Module required by VRPLOT:  DRWARC */
16764 
16765 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16766 /*                                          DBLE, NINT, SIN, */
16767 /*                                          SQRT */
16768 
16769 /* *********************************************************** */
16770 
16771 
16772     /* Parameter adjustments */
16773     --lend;
16774     --z__;
16775     --y;
16776     --x;
16777     --zc;
16778     --yc;
16779     --xc;
16780     --listc;
16781     --lptr;
16782 
16783     /* Function Body */
16784 
16785 /* Local parameters: */
16786 
16787 /* ANNOT =     long int variable with value TRUE iff the plot */
16788 /*               is to be annotated with the values of ELAT, */
16789 /*               ELON, and A */
16790 /* CF =        Conversion factor for degrees to radians */
16791 /* CT =        Cos(ELAT) */
16792 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16793 /* FSIZN =     Font size in points for labeling nodes with */
16794 /*               their indexes if NUMBR = TRUE */
16795 /* FSIZT =     Font size in points for the title (and */
16796 /*               annotation if ANNOT = TRUE) */
16797 /* IN1,IN2 =   long int variables with value TRUE iff the */
16798 /*               projections of vertices KV1 and KV2, respec- */
16799 /*               tively, are inside the window */
16800 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16801 /*               left corner of the bounding box or viewport */
16802 /*               box */
16803 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16804 /*               right corner of the bounding box or viewport */
16805 /*               box */
16806 /* IR =        Half the width (height) of the bounding box or */
16807 /*               viewport box in points -- viewport radius */
16808 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
16809 /* LP =        LIST index (pointer) */
16810 /* LPL =       Pointer to the last neighbor of N0 */
16811 /* N0 =        Index of a node */
16812 /* NSEG =      Number of line segments used by DRWARC in a */
16813 /*               polygonal approximation to a projected edge */
16814 /* P1 =        Coordinates of vertex KV1 in the rotated */
16815 /*               coordinate system */
16816 /* P2 =        Coordinates of vertex KV2 in the rotated */
16817 /*               coordinate system or intersection of edge */
16818 /*               KV1-KV2 with the equator (in the rotated */
16819 /*               coordinate system) */
16820 /* R11...R23 = Components of the first two rows of a rotation */
16821 /*               that maps E to the north pole (0,0,1) */
16822 /* SF =        Scale factor for mapping world coordinates */
16823 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16824 /*               to viewport coordinates in [IPX1,IPX2] X */
16825 /*               [IPY1,IPY2] */
16826 /* T =         Temporary variable */
16827 /* TOL =       Maximum distance in points between a projected */
16828 /*               Voronoi edge and its approximation by a */
16829 /*               polygonal curve */
16830 /* TX,TY =     Translation vector for mapping world coordi- */
16831 /*               nates to viewport coordinates */
16832 /* WR =        Window radius r = Sin(A) */
16833 /* WRS =       WR**2 */
16834 /* X0,Y0 =     Projection plane coordinates of node N0 or */
16835 /*               label location */
16836 
16837 
16838 /* Test for invalid parameters. */
16839 
16840     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16841             nt != 2 * *n - 4) {
16842         goto L11;
16843     }
16844     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16845         goto L12;
16846     }
16847 
16848 /* Compute a conversion factor CF for degrees to radians */
16849 /*   and compute the window radius WR. */
16850 
16851     cf = atan(1.) / 45.;
16852     wr = sin(cf * *a);
16853     wrs = wr * wr;
16854 
16855 /* Compute the lower left (IPX1,IPY1) and upper right */
16856 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16857 /*   The coordinates, specified in default user space units */
16858 /*   (points, at 72 points/inch with origin at the lower */
16859 /*   left corner of the page), are chosen to preserve the */
16860 /*   square aspect ratio, and to center the plot on the 8.5 */
16861 /*   by 11 inch page.  The center of the page is (306,396), */
16862 /*   and IR = PLTSIZ/2 in points. */
16863 
16864     d__1 = *pltsiz * 36.;
16865     ir = i_dnnt(&d__1);
16866     ipx1 = 306 - ir;
16867     ipx2 = ir + 306;
16868     ipy1 = 396 - ir;
16869     ipy2 = ir + 396;
16870 
16871 /* Output header comments. */
16872 
16873 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16874 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16875 /*     .        '%%BoundingBox:',4I4/ */
16876 /*     .        '%%Title:  Voronoi diagram'/ */
16877 /*     .        '%%Creator:  STRIPACK'/ */
16878 /*     .        '%%EndComments') */
16879 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16880 /*   of a viewport box obtained by shrinking the bounding box */
16881 /*   by 12% in each dimension. */
16882 
16883     d__1 = (double) ir * .88;
16884     ir = i_dnnt(&d__1);
16885     ipx1 = 306 - ir;
16886     ipx2 = ir + 306;
16887     ipy1 = 396 - ir;
16888     ipy2 = ir + 396;
16889 
16890 /* Set the line thickness to 2 points, and draw the */
16891 /*   viewport boundary. */
16892 
16893     t = 2.;
16894 /*      WRITE (LUN,110,ERR=13) T */
16895 /*      WRITE (LUN,120,ERR=13) IR */
16896 /*      WRITE (LUN,130,ERR=13) */
16897 /*  110 FORMAT (F12.6,' setlinewidth') */
16898 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16899 /*  130 FORMAT ('stroke') */
16900 
16901 /* Set up an affine mapping from the window box [-WR,WR] X */
16902 /*   [-WR,WR] to the viewport box. */
16903 
16904     sf = (double) ir / wr;
16905     tx = ipx1 + sf * wr;
16906     ty = ipy1 + sf * wr;
16907 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16908 /*  140 FORMAT (2F12.6,' translate'/ */
16909 /*     .        2F12.6,' scale') */
16910 
16911 /* The line thickness must be changed to reflect the new */
16912 /*   scaling which is applied to all subsequent output. */
16913 /*   Set it to 1.0 point. */
16914 
16915     t = 1. / sf;
16916 /*      WRITE (LUN,110,ERR=13) T */
16917 
16918 /* Save the current graphics state, and set the clip path to */
16919 /*   the boundary of the window. */
16920 
16921 /*      WRITE (LUN,150,ERR=13) */
16922 /*      WRITE (LUN,160,ERR=13) WR */
16923 /*      WRITE (LUN,170,ERR=13) */
16924 /*  150 FORMAT ('gsave') */
16925 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16926 /*  170 FORMAT ('clip newpath') */
16927 
16928 /* Compute the Cartesian coordinates of E and the components */
16929 /*   of a rotation R which maps E to the north pole (0,0,1). */
16930 /*   R is taken to be a rotation about the z-axis (into the */
16931 /*   yz-plane) followed by a rotation about the x-axis chosen */
16932 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16933 /*   E is the north or south pole. */
16934 
16935 /*           ( R11  R12  0   ) */
16936 /*       R = ( R21  R22  R23 ) */
16937 /*           ( EX   EY   EZ  ) */
16938 
16939     t = cf * *elon;
16940     ct = cos(cf * *elat);
16941     ex = ct * cos(t);
16942     ey = ct * sin(t);
16943     ez = sin(cf * *elat);
16944     if (ct != 0.) {
16945         r11 = -ey / ct;
16946         r12 = ex / ct;
16947     } else {
16948         r11 = 0.;
16949         r12 = 1.;
16950     }
16951     r21 = -ez * r12;
16952     r22 = ez * r11;
16953     r23 = ct;
16954 
16955 /* Loop on nodes (Voronoi centers) N0. */
16956 /*   LPL indexes the last neighbor of N0. */
16957 
16958     i__1 = *n;
16959     for (n0 = 1; n0 <= i__1; ++n0) {
16960         lpl = lend[n0];
16961 
16962 /* Set KV2 to the first (and last) vertex index and compute */
16963 /*   its coordinates P2 in the rotated coordinate system. */
16964 
16965         kv2 = listc[lpl];
16966         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
16967         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
16968         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
16969 
16970 /*   IN2 = TRUE iff KV2 is in the window. */
16971 
16972         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
16973 
16974 /* Loop on neighbors N1 of N0.  For each triangulation edge */
16975 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
16976 
16977         lp = lpl;
16978 L1:
16979         lp = lptr[lp];
16980         kv1 = kv2;
16981         p1[0] = p2[0];
16982         p1[1] = p2[1];
16983         p1[2] = p2[2];
16984         in1 = in2;
16985         kv2 = listc[lp];
16986 
16987 /*   Compute the new values of P2 and IN2. */
16988 
16989         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
16990         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
16991         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
16992         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
16993 
16994 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
16995 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
16996 /*   outside (so that the edge is drawn only once). */
16997 
16998         if (! in1 || in2 && kv2 <= kv1) {
16999             goto L2;
17000         }
17001         if (p2[2] < 0.) {
17002 
17003 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17004 /*     intersection of edge KV1-KV2 with the equator so that */
17005 /*     the edge is clipped properly.  P2(3) is set to 0. */
17006 
17007             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17008             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17009             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17010             p2[0] /= t;
17011             p2[1] /= t;
17012         }
17013 
17014 /*   Add the edge to the path.  (TOL is converted to world */
17015 /*     coordinates.) */
17016 
17017         if (p2[2] < 0.) {
17018             p2[2] = 0.f;
17019         }
17020         d__1 = tol / sf;
17021         drwarc_(lun, p1, p2, &d__1, &nseg);
17022 
17023 /* Bottom of loops. */
17024 
17025 L2:
17026         if (lp != lpl) {
17027             goto L1;
17028         }
17029 /* L3: */
17030     }
17031 
17032 /* Paint the path and restore the saved graphics state (with */
17033 /*   no clip path). */
17034 
17035 /*      WRITE (LUN,130,ERR=13) */
17036 /*      WRITE (LUN,190,ERR=13) */
17037 /*  190 FORMAT ('grestore') */
17038     if (*numbr) {
17039 
17040 /* Nodes in the window are to be labeled with their indexes. */
17041 /*   Convert FSIZN from points to world coordinates, and */
17042 /*   output the commands to select a font and scale it. */
17043 
17044         t = fsizn / sf;
17045 /*        WRITE (LUN,200,ERR=13) T */
17046 /*  200   FORMAT ('/Helvetica findfont'/ */
17047 /*     .          F12.6,' scalefont setfont') */
17048 
17049 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17050 /*   the window. */
17051 
17052         i__1 = *n;
17053         for (n0 = 1; n0 <= i__1; ++n0) {
17054             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17055                 goto L4;
17056             }
17057             x0 = r11 * x[n0] + r12 * y[n0];
17058             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17059             if (x0 * x0 + y0 * y0 > wrs) {
17060                 goto L4;
17061             }
17062 
17063 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17064 /*     of the first character at (X0,Y0). */
17065 
17066 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17067 /*          WRITE (LUN,220,ERR=13) N0 */
17068 /*  210     FORMAT (2F12.6,' moveto') */
17069 /*  220     FORMAT ('(',I3,') show') */
17070 L4:
17071             ;
17072         }
17073     }
17074 
17075 /* Convert FSIZT from points to world coordinates, and output */
17076 /*   the commands to select a font and scale it. */
17077 
17078     t = fsizt / sf;
17079 /*      WRITE (LUN,200,ERR=13) T */
17080 
17081 /* Display TITLE centered above the plot: */
17082 
17083     y0 = wr + t * 3.;
17084 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17085 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17086 /*     .        ' moveto') */
17087 /*      WRITE (LUN,240,ERR=13) TITLE */
17088 /*  240 FORMAT (A80/'  show') */
17089     if (annot) {
17090 
17091 /* Display the window center and radius below the plot. */
17092 
17093         x0 = -wr;
17094         y0 = -wr - 50. / sf;
17095 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17096 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17097         y0 -= t * 2.;
17098 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17099 /*        WRITE (LUN,260,ERR=13) A */
17100 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17101 /*     .          ',  ELON = ',F8.2,') show') */
17102 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17103     }
17104 
17105 /* Paint the path and output the showpage command and */
17106 /*   end-of-file indicator. */
17107 
17108 /*      WRITE (LUN,270,ERR=13) */
17109 /*  270 FORMAT ('stroke'/ */
17110 /*     .        'showpage'/ */
17111 /*     .        '%%EOF') */
17112 
17113 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17114 /*   indicator (to eliminate a timeout error message): */
17115 /*   ASCII 4. */
17116 
17117 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17118 /*  280 FORMAT (A1) */
17119 
17120 /* No error encountered. */
17121 
17122     *ier = 0;
17123     return 0;
17124 
17125 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17126 
17127 L11:
17128     *ier = 1;
17129     return 0;
17130 
17131 /* Invalid input parameter ELAT, ELON, or A. */
17132 
17133 L12:
17134     *ier = 2;
17135     return 0;
17136 
17137 /* Error writing to unit LUN. */
17138 
17139 /* L13: */
17140     *ier = 3;
17141     return 0;
17142 } /* vrplot_ */
17143 
17144 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17145         double *rannum)
17146 {
17147     static double x;
17148 
17149 
17150 /*   This routine returns pseudo-random numbers uniformly */
17151 /* distributed in the interval (0,1).  int seeds IX, IY, */
17152 /* and IZ should be initialized to values in the range 1 to */
17153 /* 30,000 before the first call to RANDOM, and should not */
17154 /* be altered between subsequent calls (unless a sequence */
17155 /* of random numbers is to be repeated by reinitializing the */
17156 /* seeds). */
17157 
17158 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17159 /*             and Portable Pseudo-random Number Generator, */
17160 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17161 /*             pp. 188-190. */
17162 
17163     *ix = *ix * 171 % 30269;
17164     *iy = *iy * 172 % 30307;
17165     *iz = *iz * 170 % 30323;
17166     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17167             double) (*iz) / 30323.;
17168     *rannum = x - (int) x;
17169     return 0;
17170 } /* random_ */
17171 
17172 #undef TRUE_
17173 #undef FALSE_
17174 #undef abs
17175 
17176 /*################################################################################################
17177 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17178 ######   You must link the resulting object file with the libraries: #############################
17179 ####################    -lf2c -lm   (in that order)   ############################################
17180 ################################################################################################*/
17181 
17182 
17183 
17184 EMData* Util::mult_scalar(EMData* img, float scalar)
17185 {
17186         ENTERFUNC;
17187         /* Exception Handle */
17188         if (!img) {
17189                 throw NullPointerException("NULL input image");
17190         }
17191         /* ============  output = scalar*input  ================== */
17192 
17193         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17194         int size = nx*ny*nz;
17195         EMData * img2 = img->copy_head();
17196         float *img_ptr  =img->get_data();
17197         float *img2_ptr = img2->get_data();
17198         for (int i=0;i<size;i++)img2_ptr[i] = img_ptr[i]*scalar;
17199         img2->update();
17200 
17201         if(img->is_complex()) {
17202                 img2->set_complex(true);
17203                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17204         }
17205         EXITFUNC;
17206         return img2;
17207 }
17208 
17209 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17210 {
17211         ENTERFUNC;
17212         /* Exception Handle */
17213         if (!img) {
17214                 throw NullPointerException("NULL input image");
17215         }
17216         /* ==============   output = img + scalar*img1   ================ */
17217 
17218         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17219         int size = nx*ny*nz;
17220         EMData * img2 = img->copy_head();
17221         float *img_ptr  =img->get_data();
17222         float *img2_ptr = img2->get_data();
17223         float *img1_ptr = img1->get_data();
17224         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17225         img2->update();
17226         if(img->is_complex()) {
17227                 img2->set_complex(true);
17228                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17229         }
17230 
17231         EXITFUNC;
17232         return img2;
17233 }
17234 
17235 EMData* Util::addn_img(EMData* img, EMData* img1)
17236 {
17237         ENTERFUNC;
17238         /* Exception Handle */
17239         if (!img) {
17240                 throw NullPointerException("NULL input image");
17241         }
17242         /* ==============   output = img + img1   ================ */
17243 
17244         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17245         int size = nx*ny*nz;
17246         EMData * img2 = img->copy_head();
17247         float *img_ptr  =img->get_data();
17248         float *img2_ptr = img2->get_data();
17249         float *img1_ptr = img1->get_data();
17250         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17251         img2->update();
17252         if(img->is_complex()) {
17253                 img2->set_complex(true);
17254                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17255         }
17256 
17257         EXITFUNC;
17258         return img2;
17259 }
17260 
17261 EMData* Util::subn_img(EMData* img, EMData* img1)
17262 {
17263         ENTERFUNC;
17264         /* Exception Handle */
17265         if (!img) {
17266                 throw NullPointerException("NULL input image");
17267         }
17268         /* ==============   output = img - img1   ================ */
17269 
17270         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17271         int size = nx*ny*nz;
17272         EMData * img2 = img->copy_head();
17273         float *img_ptr  =img->get_data();
17274         float *img2_ptr = img2->get_data();
17275         float *img1_ptr = img1->get_data();
17276         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17277         img2->update();
17278         if(img->is_complex()) {
17279                 img2->set_complex(true);
17280                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17281         }
17282 
17283         EXITFUNC;
17284         return img2;
17285 }
17286 
17287 EMData* Util::muln_img(EMData* img, EMData* img1)
17288 {
17289         ENTERFUNC;
17290         /* Exception Handle */
17291         if (!img) {
17292                 throw NullPointerException("NULL input image");
17293         }
17294         /* ==============   output = img * img1   ================ */
17295 
17296         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17297         int size = nx*ny*nz;
17298         EMData * img2 = img->copy_head();
17299         float *img_ptr  =img->get_data();
17300         float *img2_ptr = img2->get_data();
17301         float *img1_ptr = img1->get_data();
17302         if(img->is_complex()) {
17303                 for (int i=0; i<size; i+=2) {
17304                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17305                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17306                 }
17307                 img2->set_complex(true);
17308                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17309         } else {
17310                 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17311                 img2->update();
17312         }
17313 
17314         EXITFUNC;
17315         return img2;
17316 }
17317 
17318 EMData* Util::divn_img(EMData* img, EMData* img1)
17319 {
17320         ENTERFUNC;
17321         /* Exception Handle */
17322         if (!img) {
17323                 throw NullPointerException("NULL input image");
17324         }
17325         /* ==============   output = img / img1   ================ */
17326 
17327         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17328         int size = nx*ny*nz;
17329         EMData * img2 = img->copy_head();
17330         float *img_ptr  =img->get_data();
17331         float *img2_ptr = img2->get_data();
17332         float *img1_ptr = img1->get_data();
17333         if(img->is_complex()) {
17334                 float  sq2;
17335                 for (int i=0; i<size; i+=2) {
17336                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17337                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17338                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17339                 }
17340                 img2->set_complex(true);
17341                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17342         } else {
17343                 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17344                 img2->update();
17345         }
17346 
17347         EXITFUNC;
17348         return img2;
17349 }
17350 
17351 EMData* Util::divn_filter(EMData* img, EMData* img1)
17352 {
17353         ENTERFUNC;
17354         /* Exception Handle */
17355         if (!img) {
17356                 throw NullPointerException("NULL input image");
17357         }
17358         /* ========= img /= img1 ===================== */
17359 
17360         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17361         int size = nx*ny*nz;
17362         EMData * img2 = img->copy_head();
17363         float *img_ptr  =img->get_data();
17364         float *img1_ptr = img1->get_data();
17365         float *img2_ptr = img2->get_data();
17366         if(img->is_complex()) {
17367                 for (int i=0; i<size; i+=2) {
17368                         if(img1_ptr[i] > 1.e-10f) {
17369                         img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17370                         img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17371                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17372                 }
17373         } else  throw ImageFormatException("Only Fourier image allowed");
17374 
17375         img->update();
17376 
17377         EXITFUNC;
17378         return img2;
17379 }
17380 
17381 void Util::mul_scalar(EMData* img, float scalar)
17382 {
17383         ENTERFUNC;
17384         /* Exception Handle */
17385         if (!img) {
17386                 throw NullPointerException("NULL input image");
17387         }
17388         /* ============  output = scalar*input  ================== */
17389 
17390         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17391         int size = nx*ny*nz;
17392         float *img_ptr  =img->get_data();
17393         for (int i=0;i<size;i++) img_ptr[i] *= scalar;
17394         img->update();
17395 
17396         EXITFUNC;
17397 }
17398 
17399 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17400 {
17401         ENTERFUNC;
17402         /* Exception Handle */
17403         if (!img) {
17404                 throw NullPointerException("NULL input image");
17405         }
17406         /* ==============   img += scalar*img1   ================ */
17407 
17408         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17409         int size = nx*ny*nz;
17410         float *img_ptr  =img->get_data();
17411         float *img1_ptr = img1->get_data();
17412         for (int i=0;i<size;i++)img_ptr[i] += img1_ptr[i]*scalar;
17413         img1->update();
17414 
17415         EXITFUNC;
17416 }
17417 
17418 void Util::add_img(EMData* img, EMData* img1)
17419 {
17420         ENTERFUNC;
17421         /* Exception Handle */
17422         if (!img) {
17423                 throw NullPointerException("NULL input image");
17424         }
17425         /* ========= img += img1 ===================== */
17426 
17427         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17428         int size = nx*ny*nz;
17429         float *img_ptr  = img->get_data();
17430         float *img1_ptr = img1->get_data();
17431         for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i];
17432         img->update();
17433 
17434         EXITFUNC;
17435 }
17436 
17437 void Util::add_img_abs(EMData* img, EMData* img1)
17438 {
17439         ENTERFUNC;
17440         /* Exception Handle */
17441         if (!img) {
17442                 throw NullPointerException("NULL input image");
17443         }
17444         /* ========= img += img1 ===================== */
17445 
17446         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17447         int size = nx*ny*nz;
17448         float *img_ptr  = img->get_data();
17449         float *img1_ptr = img1->get_data();
17450         for (int i=0;i<size;i++) img_ptr[i] += abs(img1_ptr[i]);
17451         img->update();
17452 
17453         EXITFUNC;
17454 }
17455 
17456 void Util::add_img2(EMData* img, EMData* img1)
17457 {
17458         ENTERFUNC;
17459         /* Exception Handle */
17460         if (!img) {
17461                 throw NullPointerException("NULL input image");
17462         }
17463         /* ========= img += img1**2 ===================== */
17464 
17465         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17466         int size = nx*ny*nz;
17467         float *img_ptr  = img->get_data();
17468         float *img1_ptr = img1->get_data();
17469         if(img->is_complex()) {
17470                 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] ;
17471         } else {
17472                 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17473         }
17474         img->update();
17475 
17476         EXITFUNC;
17477 }
17478 
17479 void Util::sub_img(EMData* img, EMData* img1)
17480 {
17481         ENTERFUNC;
17482         /* Exception Handle */
17483         if (!img) {
17484                 throw NullPointerException("NULL input image");
17485         }
17486         /* ========= img -= img1 ===================== */
17487 
17488         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17489         int size = nx*ny*nz;
17490         float *img_ptr  = img->get_data();
17491         float *img1_ptr = img1->get_data();
17492         for (int i=0;i<size;i++) img_ptr[i] -= img1_ptr[i];
17493         img->update();
17494 
17495         EXITFUNC;
17496 }
17497 
17498 void Util::mul_img(EMData* img, EMData* img1)
17499 {
17500         ENTERFUNC;
17501         /* Exception Handle */
17502         if (!img) {
17503                 throw NullPointerException("NULL input image");
17504         }
17505         /* ========= img *= img1 ===================== */
17506 
17507         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17508         int size = nx*ny*nz;
17509         float *img_ptr  = img->get_data();
17510         float *img1_ptr = img1->get_data();
17511         if(img->is_complex()) {
17512                 for (int i=0; i<size; i+=2) {
17513                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17514                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17515                         img_ptr[i]   = tmp;
17516 
17517                 }
17518         } else {
17519                 for (int i=0;i<size;i++) img_ptr[i] *= img1_ptr[i];
17520         }
17521         img->update();
17522 
17523         EXITFUNC;
17524 }
17525 
17526 void Util::div_img(EMData* img, EMData* img1)
17527 {
17528         ENTERFUNC;
17529         /* Exception Handle */
17530         if (!img) {
17531                 throw NullPointerException("NULL input image");
17532         }
17533         /* ========= img /= img1 ===================== */
17534 
17535         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17536         int size = nx*ny*nz;
17537         float *img_ptr  = img->get_data();
17538         float *img1_ptr = img1->get_data();
17539         if(img->is_complex()) {
17540                 float  sq2;
17541                 for (int i=0; i<size; i+=2) {
17542                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17543                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17544                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17545                         img_ptr[i]   = tmp;
17546                 }
17547         } else {
17548                 for (int i=0; i<size; i++) img_ptr[i] /= img1_ptr[i];
17549         }
17550         img->update();
17551 
17552         EXITFUNC;
17553 }
17554 
17555 void Util::div_filter(EMData* img, EMData* img1)
17556 {
17557         ENTERFUNC;
17558         /* Exception Handle */
17559         if (!img) {
17560                 throw NullPointerException("NULL input image");
17561         }
17562         /* ========= img /= img1 ===================== */
17563 
17564         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17565         int size = nx*ny*nz;
17566         float *img_ptr  = img->get_data();
17567         float *img1_ptr = img1->get_data();
17568         if(img->is_complex()) {
17569                 for (int i=0; i<size; i+=2) {
17570                         if(img1_ptr[i] > 1.e-10f) {
17571                         img_ptr[i]   /= img1_ptr[i];
17572                         img_ptr[i+1] /= img1_ptr[i];
17573                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17574                 }
17575         } else throw ImageFormatException("Only Fourier image allowed");
17576 
17577         img->update();
17578 
17579         EXITFUNC;
17580 }
17581 
17582 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*nxo]
17583 
17584 EMData* Util::pack_complex_to_real(EMData* img)
17585 {
17586         ENTERFUNC;
17587         /* Exception Handle */
17588         if (!img) {
17589                 throw NullPointerException("NULL input image");
17590         }
17591         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17592                               output is img packed into real image with Friedel part added,   ================ */
17593 
17594         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17595         int nx = nxo - 2 + img->is_fftodd();
17596         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17597         int nyt, nzt;
17598         int nx2 = nx/2;
17599         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17600         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17601         int nx2p = nx2+nx%2;
17602         int ny2p = ny2+ny%2;
17603         int nz2p = nz2+nz%2;
17604         EMData& power = *(new EMData()); // output image
17605         power.set_size(nx, ny, nz);
17606         power.set_array_offsets(-nx2,-ny2,-nz2);
17607         //img->set_array_offsets(1,1,1);
17608         float *img_ptr  = img->get_data();
17609         for (int iz = 1; iz <= nz; iz++) {
17610                 int jz=iz-1;
17611                 if(jz>=nz2p) jz=jz-nzt;
17612                 for (int iy = 1; iy <= ny; iy++) {
17613                         int jy=iy-1;
17614                         if(jy>=ny2p) jy=jy-nyt;
17615                         for (int ix = 1; ix <= lsd2; ix++) {
17616                                 int jx=ix-1;
17617                                 if(jx>=nx2p) jx=jx-nx;
17618                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17619                         }
17620                 }
17621         }
17622 //  Create the Friedel related half
17623         int  nzb, nze, nyb, nye, nxb, nxe;
17624         nxb =-nx2+(nx+1)%2;
17625         nxe = nx2-(nx+1)%2;
17626         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17627         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17628         for (int iz = nzb; iz <= nze; iz++) {
17629                 for (int iy = nyb; iy <= nye; iy++) {
17630                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17631                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17632                         }
17633                 }
17634         }
17635         if(ny2 != 0)  {
17636                 if(nz2 != 0)  {
17637                         if(nz%2 == 0) {  //if nz even, fix the first slice
17638                                 for (int iy = nyb; iy <= nye; iy++) {
17639                                         for (int ix = nxb; ix <= -1; ix++) {
17640                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17641                                         }
17642                                 }
17643                                 if(ny%2 == 0) {  //if ny even, fix the first line
17644                                         for (int ix = nxb; ix <= -1; ix++) {
17645                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17646                                         }
17647                                 }
17648                         }
17649                 }
17650                 if(ny%2 == 0) {  //if ny even, fix the first column
17651                         for (int iz = nzb; iz <= nze; iz++) {
17652                                 for (int ix = nxb; ix <= -1; ix++) {
17653                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17654                                 }
17655                         }
17656                 }
17657 
17658         }
17659         power.update();
17660         power.set_array_offsets(0,0,0);
17661         return &power;
17662 }
17663 #undef  img_ptr
17664 
17665 float Util::ang_n(float peakp, string mode, int maxrin)
17666 {
17667     if (mode == "f" || mode == "F")
17668         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17669     else
17670         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17671 }
17672 
17673 
17674 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17675 {
17676     float* data = ring->get_data();
17677     float av=0.0;
17678     float sq=0.0;
17679     float nn=0.0;
17680     int nring = numr.size()/3;
17681     for( int i=0; i < nring; ++i )
17682     {
17683         int numr3i = numr[3*i+2];
17684         int numr2i = numr[3*i+1]-1;
17685         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17686         for( int j=0; j < numr3i; ++j )
17687         {
17688             int jc = numr2i+j;
17689             av += data[jc] * w;
17690             sq += data[jc] * data[jc] * w;
17691             nn += w;
17692         }
17693     }
17694 
17695     float avg = av/nn;
17696     float sgm = sqrt( (sq-av*av/nn)/nn );
17697     int n = ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17698     for( int i=0; i < n; ++i )
17699     {
17700         data[i] -= avg;
17701         data[i] /= sgm;
17702     }
17703 
17704     ring->update();
17705 }
17706 
17707 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17708                 float xrng, float yrng, float step, string mode,
17709                 vector<int>numr, float cnx, float cny) {
17710 
17711     // Manually extract.
17712 /*    vector< EMAN::EMData* > crefim;
17713     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17714     crefim.reserve(crefim_len);
17715 
17716     for(std::size_t i=0;i<crefim_len;i++) {
17717         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17718         crefim.push_back(proxy());
17719     }
17720 */
17721 
17722         size_t crefim_len = crefim.size();
17723 
17724         int   ky = int(2*yrng/step+0.5)/2;
17725         int   kx = int(2*xrng/step+0.5)/2;
17726         int   iref, nref=0, mirror=0;
17727         float iy, ix, sx=0, sy=0;
17728         float peak = -1.0E23f;
17729         float ang=0.0f;
17730         for (int i = -ky; i <= ky; i++) {
17731                 iy = i * step ;
17732                 for (int j = -kx; j <= kx; j++) {
17733                         ix = j*step ;
17734                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17735 
17736                         Normalize_ring( cimage, numr );
17737 
17738                         Frngs(cimage, numr);
17739                         //  compare with all reference images
17740                         // for iref in xrange(len(crefim)):
17741                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17742                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17743                                 double qn = retvals["qn"];
17744                                 double qm = retvals["qm"];
17745                                 if(qn >= peak || qm >= peak) {
17746                                         sx = -ix;
17747                                         sy = -iy;
17748                                         nref = iref;
17749                                         if (qn >= qm) {
17750                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17751                                                 peak = static_cast<float>(qn);
17752                                                 mirror = 0;
17753                                         } else {
17754                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17755                                                 peak = static_cast<float>(qm);
17756                                                 mirror = 1;
17757                                         }
17758                                 }
17759                         }  delete cimage; cimage = 0;
17760                 }
17761         }
17762         float co, so, sxs, sys;
17763         co = static_cast<float>( cos(ang*pi/180.0) );
17764         so = static_cast<float>( -sin(ang*pi/180.0) );
17765         sxs = sx*co - sy*so;
17766         sys = sx*so + sy*co;
17767         vector<float> res;
17768         res.push_back(ang);
17769         res.push_back(sxs);
17770         res.push_back(sys);
17771         res.push_back(static_cast<float>(mirror));
17772         res.push_back(static_cast<float>(nref));
17773         res.push_back(peak);
17774         return res;
17775 }
17776 
17777 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17778                 float xrng, float yrng, float step, string mode,
17779                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17780 
17781     // Manually extract.
17782 /*    vector< EMAN::EMData* > crefim;
17783     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17784     crefim.reserve(crefim_len);
17785 
17786     for(std::size_t i=0;i<crefim_len;i++) {
17787         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17788         crefim.push_back(proxy());
17789     }
17790 */
17791 
17792         size_t crefim_len = crefim.size();
17793 
17794         int   ky = int(2*yrng/step+0.5)/2;
17795         int   kx = int(2*xrng/step+0.5)/2;
17796         int   iref, nref=0, mirror=0;
17797         float iy, ix, sx=0, sy=0;
17798         float peak = -1.0E23f;
17799         float ang=0.0f;
17800         for (int i = -ky; i <= ky; i++) {
17801                 iy = i * step ;
17802                 for (int j = -kx; j <= kx; j++) {
17803                         ix = j*step ;
17804                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17805 
17806                         Normalize_ring( cimage, numr );
17807 
17808                         Frngs(cimage, numr);
17809                         //  compare with all reference images
17810                         // for iref in xrange(len(crefim)):
17811                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17812                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17813                                 double qn = retvals["qn"];
17814                                 double qm = retvals["qm"];
17815                                 if(qn >= peak || qm >= peak) {
17816                                         sx = -ix;
17817                                         sy = -iy;
17818                                         nref = iref;
17819                                         if (qn >= qm) {
17820                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17821                                                 peak = static_cast<float>(qn);
17822                                                 mirror = 0;
17823                                         } else {
17824                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17825                                                 peak = static_cast<float>(qm);
17826                                                 mirror = 1;
17827                                         }
17828                                 }
17829                         }  delete cimage; cimage = 0;
17830                 }
17831         }
17832         float co, so, sxs, sys;
17833         co = static_cast<float>( cos(ang*pi/180.0) );
17834         so = static_cast<float>( -sin(ang*pi/180.0) );
17835         sxs = sx*co - sy*so;
17836         sys = sx*so + sy*co;
17837         vector<float> res;
17838         res.push_back(ang);
17839         res.push_back(sxs);
17840         res.push_back(sys);
17841         res.push_back(static_cast<float>(mirror));
17842         res.push_back(static_cast<float>(nref));
17843         res.push_back(peak);
17844         return res;
17845 }
17846 
17847 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
17848                 float xrng, float yrng, float step, string mode,
17849                 vector< int >numr, float cnx, float cny) {
17850 
17851     // Manually extract.
17852 /*    vector< EMAN::EMData* > crefim;
17853     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17854     crefim.reserve(crefim_len);
17855 
17856     for(std::size_t i=0;i<crefim_len;i++) {
17857         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17858         crefim.push_back(proxy());
17859     }
17860 */
17861         size_t crefim_len = crefim.size();
17862 
17863         int   ky = int(2*yrng/step+0.5)/2;
17864         int   kx = int(2*xrng/step+0.5)/2;
17865         int   iref, nref=0;
17866         float iy, ix, sx=0, sy=0;
17867         float peak = -1.0E23f;
17868         float ang=0.0f;
17869         for (int i = -ky; i <= ky; i++) {
17870                 iy = i * step ;
17871                 for (int j = -kx; j <= kx; j++) {
17872                         ix = j*step ;
17873                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17874                         Frngs(cimage, numr);
17875                         //  compare with all reference images
17876                         // for iref in xrange(len(crefim)):
17877                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17878                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
17879                                 double qn = retvals["qn"];
17880                                 if(qn >= peak) {
17881                                         sx = -ix;
17882                                         sy = -iy;
17883                                         nref = iref;
17884                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17885                                         peak = static_cast<float>(qn);
17886                                 }
17887                         }  delete cimage; cimage = 0;
17888                 }
17889         }
17890         float co, so, sxs, sys;
17891         co = static_cast<float>( cos(ang*pi/180.0) );
17892         so = static_cast<float>( -sin(ang*pi/180.0) );
17893         sxs = sx*co - sy*so;
17894         sys = sx*so + sy*co;
17895         vector<float> res;
17896         res.push_back(ang);
17897         res.push_back(sxs);
17898         res.push_back(sys);
17899         res.push_back(static_cast<float>(nref));
17900         res.push_back(peak);
17901         return res;
17902 }
17903 
17904 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
17905                 float xrng, float yrng, float step, float ant, string mode,
17906                 vector<int>numr, float cnx, float cny) {
17907 
17908     // Manually extract.
17909 /*    vector< EMAN::EMData* > crefim;
17910     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17911     crefim.reserve(crefim_len);
17912 
17913     for(std::size_t i=0;i<crefim_len;i++) {
17914         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17915         crefim.push_back(proxy());
17916     }
17917 */
17918         size_t crefim_len = crefim.size();
17919         const float qv = static_cast<float>( pi/180.0 );
17920 
17921         Transform * t = image->get_attr("xform.projection");
17922         Dict d = t->get_params("spider");
17923         if(t) {delete t; t=0;}
17924         float phi = d["phi"];
17925         float theta = d["theta"];
17926         int   ky = int(2*yrng/step+0.5)/2;
17927         int   kx = int(2*xrng/step+0.5)/2;
17928         int   iref, nref=0, mirror=0;
17929         float iy, ix, sx=0, sy=0;
17930         float peak = -1.0E23f;
17931         float ang=0.0f;
17932         float imn1 = sin(theta*qv)*cos(phi*qv);
17933         float imn2 = sin(theta*qv)*sin(phi*qv);
17934         float imn3 = cos(theta*qv);
17935         vector<float> n1(crefim_len);
17936         vector<float> n2(crefim_len);
17937         vector<float> n3(crefim_len);
17938         for ( iref = 0; iref < (int)crefim_len; iref++) {
17939                         n1[iref] = crefim[iref]->get_attr("n1");
17940                         n2[iref] = crefim[iref]->get_attr("n2");
17941                         n3[iref] = crefim[iref]->get_attr("n3");
17942         }
17943         for (int i = -ky; i <= ky; i++) {
17944             iy = i * step ;
17945             for (int j = -kx; j <= kx; j++) {
17946                 ix = j*step;
17947                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17948 
17949                 Normalize_ring( cimage, numr );
17950 
17951                 Frngs(cimage, numr);
17952                 //  compare with all reference images
17953                 // for iref in xrange(len(crefim)):
17954                 for ( iref = 0; iref < (int)crefim_len; iref++) {
17955                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
17956                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17957                                 double qn = retvals["qn"];
17958                                 double qm = retvals["qm"];
17959                                 if(qn >= peak || qm >= peak) {
17960                                         sx = -ix;
17961                                         sy = -iy;
17962                                         nref = iref;
17963                                         if (qn >= qm) {
17964                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17965                                                 peak = static_cast<float>( qn );
17966                                                 mirror = 0;
17967                                         } else {
17968                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17969                                                 peak = static_cast<float>( qm );
17970                                                 mirror = 1;
17971                                         }
17972                                 }
17973                         }
17974                 }  delete cimage; cimage = 0;
17975             }
17976         }
17977         float co, so, sxs, sys;
17978         if(peak == -1.0E23) {
17979                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
17980                 nref = -1;
17981         } else {
17982                 co =  cos(ang*qv);
17983                 so = -sin(ang*qv);
17984                 sxs = sx*co - sy*so;
17985                 sys = sx*so + sy*co;
17986         }
17987         vector<float> res;
17988         res.push_back(ang);
17989         res.push_back(sxs);
17990         res.push_back(sys);
17991         res.push_back(static_cast<float>(mirror));
17992         res.push_back(static_cast<float>(nref));
17993         res.push_back(peak);
17994         return res;
17995 }
17996 
17997 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
17998                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
17999                 vector<int>numr, float cnx, float cny) {
18000 
18001     // Manually extract.
18002 /*    vector< EMAN::EMData* > crefim;
18003     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18004     crefim.reserve(crefim_len);
18005 
18006     for(std::size_t i=0;i<crefim_len;i++) {
18007         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18008         crefim.push_back(proxy());
18009     }
18010 */
18011         size_t crefim_len = crefim.size();
18012         const float qv = static_cast<float>(pi/180.0);
18013 
18014         Transform* t = image->get_attr("xform.projection");
18015         Dict d = t->get_params("spider");
18016         if(t) {delete t; t=0;}
18017         float phi = d["phi"];
18018         float theta = d["theta"];
18019         float psi = d["psi"];
18020         int ky = int(2*yrng/step+0.5)/2;
18021         int kx = int(2*xrng/step+0.5)/2;
18022         int iref, nref = 0, mirror = 0;
18023         float iy, ix, sx = 0, sy = 0;
18024         float peak = -1.0E23f;
18025         float ang = 0.0f;
18026         float imn1 = sin(theta*qv)*cos(phi*qv);
18027         float imn2 = sin(theta*qv)*sin(phi*qv);
18028         float imn3 = cos(theta*qv);
18029         vector<float> n1(crefim_len);
18030         vector<float> n2(crefim_len);
18031         vector<float> n3(crefim_len);
18032         for (iref = 0; iref < (int)crefim_len; iref++) {
18033                         n1[iref] = crefim[iref]->get_attr("n1");
18034                         n2[iref] = crefim[iref]->get_attr("n2");
18035                         n3[iref] = crefim[iref]->get_attr("n3");
18036         }
18037         bool nomirror = (theta<90.0) || (theta==90.0) && (psi<psi_max);
18038         if (!nomirror) {
18039                 phi = fmod(phi+540.0f, 360.0f);
18040                 theta = 180-theta;
18041                 psi = fmod(540.0f-psi, 360.0f);
18042         }
18043         for (int i = -ky; i <= ky; i++) {
18044             iy = i * step ;
18045             for (int j = -kx; j <= kx; j++) {
18046                 ix = j*step;
18047                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18048 
18049                 Normalize_ring(cimage, numr);
18050 
18051                 Frngs(cimage, numr);
18052                 //  compare with all reference images
18053                 // for iref in xrange(len(crefim)):
18054                 for (iref = 0; iref < (int)crefim_len; iref++) {
18055                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18056                                 if (nomirror) {
18057                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18058                                         double qn = retvals["qn"];
18059                                         if (qn >= peak) {
18060                                                 sx = -ix;
18061                                                 sy = -iy;
18062                                                 nref = iref;
18063                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18064                                                 peak = static_cast<float>(qn);
18065                                                 mirror = 0;
18066                                         }
18067                                 } else {
18068                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18069                                         double qn = retvals["qn"];
18070                                         if (qn >= peak) {
18071                                                 sx = -ix;
18072                                                 sy = -iy;
18073                                                 nref = iref;
18074                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18075                                                 peak = static_cast<float>(qn);
18076                                                 mirror = 1;
18077                                         }
18078                                 }
18079                         }
18080                 }  delete cimage; cimage = 0;
18081             }
18082         }
18083         float co, so, sxs, sys;
18084         if(peak == -1.0E23) {
18085                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18086                 nref = -1;
18087         } else {
18088                 co =  cos(ang*qv);
18089                 so = -sin(ang*qv);
18090                 sxs = sx*co - sy*so;
18091                 sys = sx*so + sy*co;
18092         }
18093         vector<float> res;
18094         res.push_back(ang);
18095         res.push_back(sxs);
18096         res.push_back(sys);
18097         res.push_back(static_cast<float>(mirror));
18098         res.push_back(static_cast<float>(nref));
18099         res.push_back(peak);
18100         return res;
18101 }
18102 
18103 
18104 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18105                 float xrng, float yrng, float step, float psi_max, string mode,
18106                 vector<int>numr, float cnx, float cny) {
18107 
18108         size_t crefim_len = crefim.size();
18109 
18110         int   ky = int(2*yrng/step+0.5)/2;
18111         int   kx = int(2*xrng/step+0.5)/2;
18112         int   iref, nref=0, mirror=0;
18113         float iy, ix, sx=0, sy=0;
18114         float peak = -1.0E23f;
18115         float ang=0.0f;
18116         for (int i = -ky; i <= ky; i++) {
18117                 iy = i * step ;
18118                 for (int j = -kx; j <= kx; j++) {
18119                         ix = j*step ;
18120                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18121 
18122                         Normalize_ring( cimage, numr );
18123 
18124                         Frngs(cimage, numr);
18125                         //  compare with all reference images
18126                         // for iref in xrange(len(crefim)):
18127                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18128                                 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18129                                 double qn = retvals["qn"];
18130                                 double qm = retvals["qm"];
18131                                 if(qn >= peak || qm >= peak) {
18132                                         sx = -ix;
18133                                         sy = -iy;
18134                                         nref = iref;
18135                                         if (qn >= qm) {
18136                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18137                                                 peak = static_cast<float>(qn);
18138                                                 mirror = 0;
18139                                         } else {
18140                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18141                                                 peak = static_cast<float>(qm);
18142                                                 mirror = 1;
18143                                         }
18144                                 }
18145                         }  delete cimage; cimage = 0;
18146                 }
18147         }
18148         float co, so, sxs, sys;
18149         co = static_cast<float>( cos(ang*pi/180.0) );
18150         so = static_cast<float>( -sin(ang*pi/180.0) );
18151         sxs = sx*co - sy*so;
18152         sys = sx*so + sy*co;
18153         vector<float> res;
18154         res.push_back(ang);
18155         res.push_back(sxs);
18156         res.push_back(sys);
18157         res.push_back(static_cast<float>(mirror));
18158         res.push_back(static_cast<float>(nref));
18159         res.push_back(peak);
18160         return res;
18161 }
18162 
18163 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18164                         float xrng, float yrng, float step, string mode,
18165                         vector< int >numr, float cnx, float cny,
18166                         EMData *peaks, EMData *peakm) {
18167 
18168         int   maxrin = numr[numr.size()-1];
18169 
18170         int   ky = int(2*yrng/step+0.5)/2;
18171         int   kx = int(2*xrng/step+0.5)/2;
18172 
18173         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18174         float *p_ccf1ds = peaks->get_data();
18175 
18176         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18177         float *p_ccf1dm = peakm->get_data();
18178 
18179         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18180                 p_ccf1ds[i] = -1.e20f;
18181                 p_ccf1dm[i] = -1.e20f;
18182         }
18183 
18184         for (int i = -ky; i <= ky; i++) {
18185                 float iy = i * step;
18186                 for (int j = -kx; j <= kx; j++) {
18187                         float ix = j*step;
18188                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18189                         Frngs(cimage, numr);
18190                         Crosrng_msg_vec(crefim, cimage, numr,
18191                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18192                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18193                         delete cimage; cimage = 0;
18194                 }
18195         }
18196         return;
18197 }
18198 
18199 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18200      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18201      EMData *peaks_compress, EMData *peakm_compress) {
18202 
18203         int   maxrin = numr[numr.size()-1];
18204 
18205         int   ky = int(2*yrng/step+0.5)/2;
18206         int   kx = int(2*xrng/step+0.5)/2;
18207 
18208         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18209         float *p_ccf1ds = peaks->get_data();
18210 
18211         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18212         float *p_ccf1dm = peakm->get_data();
18213 
18214         peaks_compress->set_size(maxrin, 1, 1);
18215         float *p_ccf1ds_compress = peaks_compress->get_data();
18216 
18217         peakm_compress->set_size(maxrin, 1, 1);
18218         float *p_ccf1dm_compress = peakm_compress->get_data();
18219 
18220         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18221                 p_ccf1ds[i] = -1.e20f;
18222                 p_ccf1dm[i] = -1.e20f;
18223         }
18224 
18225         for (int i = -ky; i <= ky; i++) {
18226                 float iy = i * step;
18227                 for (int j = -kx; j <= kx; j++) {
18228                         float ix = j*step;
18229                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18230                         Frngs(cimage, numr);
18231                         Crosrng_msg_vec(crefim, cimage, numr,
18232                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18233                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18234                         delete cimage; cimage = 0;
18235                 }
18236         }
18237         for (int x=0; x<maxrin; x++) {
18238                 float maxs = -1.0e22f;
18239                 float maxm = -1.0e22f;
18240                 for (int i=1; i<=2*ky+1; i++) {
18241                         for (int j=1; j<=2*kx+1; j++) {
18242                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18243                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18244                         }
18245                 }
18246                 p_ccf1ds_compress[x] = maxs;
18247                 p_ccf1dm_compress[x] = maxm;
18248         }
18249         return;
18250 }
18251 
18252 struct ccf_point
18253 {
18254     float value;
18255     int i;
18256     int j;
18257     int k;
18258     int mirror;
18259 };
18260 
18261 
18262 struct ccf_value
18263 {
18264     bool operator()( const ccf_point& a, const ccf_point& b )
18265     {
18266         return a.value > b.value;
18267     }
18268 };
18269 
18270 
18271 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18272                         float xrng, float yrng, float step, string mode,
18273                         vector< int >numr, float cnx, float cny, double T) {
18274 
18275         int   maxrin = numr[numr.size()-1];
18276 
18277         int   ky = int(2*yrng/step+0.5)/2;
18278         int   kx = int(2*xrng/step+0.5)/2;
18279 
18280         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18281         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18282         int vol = maxrin*(2*kx+1)*(2*ky+1);
18283         vector<ccf_point> ccf(2*vol);
18284         ccf_point temp;
18285 
18286         int index = 0;
18287         for (int i = -ky; i <= ky; i++) {
18288                 float iy = i * step;
18289                 for (int j = -kx; j <= kx; j++) {
18290                         float ix = j*step;
18291                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18292                         Frngs(cimage, numr);
18293                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18294                         for (int k=0; k<maxrin; k++) {
18295                                 temp.value = p_ccf1ds[k];
18296                                 temp.i = k;
18297                                 temp.j = j;
18298                                 temp.k = i;
18299                                 temp.mirror = 0;
18300                                 ccf[index] = temp;
18301                                 index++;
18302                                 temp.value = p_ccf1dm[k];
18303                                 temp.mirror = 1;
18304                                 ccf[index] = temp;
18305                                 index++;
18306                         }
18307                         delete cimage; cimage = 0;
18308                 }
18309         }
18310 
18311         delete p_ccf1ds;
18312         delete p_ccf1dm;
18313         std::sort(ccf.begin(), ccf.end(), ccf_value());
18314 
18315         double qt = (double)ccf[0].value;
18316         vector <double> p(2*vol), cp(2*vol);
18317 
18318         double sump = 0.0;
18319         for (int i=0; i<2*vol; i++) {
18320                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18321                 sump += p[i];
18322         }
18323         for (int i=0; i<2*vol; i++) {
18324                 p[i] /= sump;
18325         }
18326         for (int i=1; i<2*vol; i++) {
18327                 p[i] += p[i-1];
18328         }
18329         p[2*vol-1] = 2.0;
18330 
18331         float t = get_frand(0.0f, 1.0f);
18332         int select = 0;
18333         while (p[select] < t)   select += 1;
18334 
18335         vector<float> a(6);
18336         a[0] = ccf[select].value;
18337         a[1] = (float)ccf[select].i;
18338         a[2] = (float)ccf[select].j;
18339         a[3] = (float)ccf[select].k;
18340         a[4] = (float)ccf[select].mirror;
18341         a[5] = (float)select;
18342         return a;
18343 }
18344 
18345 
18346 /*
18347 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
18348                         float xrng, float yrng, float step, string mode,
18349                         vector< int >numr, float cnx, float cny,
18350                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
18351 
18352 // formerly known as apmq
18353     // Determine shift and rotation between image and many reference
18354     // images (crefim, weights have to be applied) quadratic
18355     // interpolation
18356 
18357 
18358     // Manually extract.
18359 *//*    vector< EMAN::EMData* > crefim;
18360     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18361     crefim.reserve(crefim_len);
18362 
18363     for(std::size_t i=0;i<crefim_len;i++) {
18364         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18365         crefim.push_back(proxy());
18366     }
18367 */
18368 /*
18369         int   maxrin = numr[numr.size()-1];
18370 
18371         size_t crefim_len = crefim.size();
18372 
18373         int   iref;
18374         int   ky = int(2*yrng/step+0.5)/2;
18375         int   kx = int(2*xrng/step+0.5)/2;
18376         int   tkx = 2*kx+3;
18377         int   tky = 2*ky+3;
18378 
18379         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
18380         float *p_ccf1ds = peaks->get_data();
18381 
18382 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
18383 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
18384         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
18385         float *p_ccf1dm = peakm->get_data();
18386 
18387         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
18388                 p_ccf1ds[i] = -1.e20f;
18389                 p_ccf1dm[i] = -1.e20f;
18390         }
18391 
18392         float  iy, ix;
18393         for (int i = -ky; i <= ky; i++) {
18394                 iy = i * step ;
18395                 for (int j = -kx; j <= kx; j++) {
18396                         ix = j*step ;
18397                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18398                         Frngs(cimage, numr);
18399                         //  compare with all reference images
18400                         // for iref in xrange(len(crefim)):
18401                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18402                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
18403                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
18404                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
18405                         }
18406                         delete cimage; cimage = 0;
18407                 }
18408         }
18409         return;
18410 }
18411 */
18412 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18413 
18414         EMData *rot;
18415 
18416         const int nmax=3, mmax=3;
18417         char task[60], csave[60];
18418         long int lsave[4];
18419         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18420         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];
18421         long int SIXTY=60;
18422 
18423         //     We wish to have no output.
18424         iprint = -1;
18425 
18426         //c     We specify the tolerances in the stopping criteria.
18427         factr=1.0e1;
18428         pgtol=1.0e-5;
18429 
18430         //     We specify the dimension n of the sample problem and the number
18431         //        m of limited memory corrections stored.  (n and m should not
18432         //        exceed the limits nmax and mmax respectively.)
18433         n=3;
18434         m=3;
18435 
18436         //     We now provide nbd which defines the bounds on the variables:
18437         //                    l   specifies the lower bounds,
18438         //                    u   specifies the upper bounds.
18439         //                    x   specifies the initial guess
18440         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18441         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18442         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18443 
18444 
18445         //     We start the iteration by initializing task.
18446         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18447         strcpy(task,"START");
18448         for (int i=5;i<60;i++)  task[i]=' ';
18449 
18450         //     This is the call to the L-BFGS-B code.
18451         // (* call the L-BFGS-B routine with task='START' once before loop *)
18452         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18453         //int step = 1;
18454 
18455         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18456         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18457 
18458                 if (strncmp(task,"FG",2)==0) {
18459                 //   the minimization routine has returned to request the
18460                 //   function f and gradient g values at the current x
18461 
18462                 //        Compute function value f for the sample problem.
18463                 rot = new EMData();
18464                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18465                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18466                 //f = -f;
18467                 delete rot;
18468 
18469                 //        Compute gradient g for the sample problem.
18470                 float dt = 1.0e-3f;
18471                 rot = new EMData();
18472                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18473                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18474                 //f1 = -f1;
18475                 g[0] = (f1-f)/dt;
18476                 delete rot;
18477 
18478                 dt = 1.0e-2f;
18479                 rot = new EMData();
18480                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18481                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18482                 //f2 = -f2;
18483                 g[1] = (f2-f)/dt;
18484                 delete rot;
18485 
18486                 rot = new EMData();
18487                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18488                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18489                 //f3 = -f3;
18490                 g[2] = (f3-f)/dt;
18491                 delete rot;
18492                 }
18493 
18494                 //c          go back to the minimization routine.
18495                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18496                 //step++;
18497         }
18498 
18499         //printf("Total step is %d\n", step);
18500         vector<float> res;
18501         res.push_back(static_cast<float>(x[0]));
18502         res.push_back(static_cast<float>(x[1]));
18503         res.push_back(static_cast<float>(x[2]));
18504         //res.push_back(step);
18505         return res;
18506 }
18507 
18508 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18509 
18510         EMData *rot;
18511 
18512         const int nmax=3, mmax=3;
18513         char task[60], csave[60];
18514         long int lsave[4];
18515         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18516         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];
18517         long int SIXTY=60;
18518 
18519         //     We wish to have no output.
18520         iprint = -1;
18521 
18522         //c     We specify the tolerances in the stopping criteria.
18523         factr=1.0e1;
18524         pgtol=1.0e-5;
18525 
18526         //     We specify the dimension n of the sample problem and the number
18527         //        m of limited memory corrections stored.  (n and m should not
18528         //        exceed the limits nmax and mmax respectively.)
18529         n=3;
18530         m=3;
18531 
18532         //     We now provide nbd which defines the bounds on the variables:
18533         //                    l   specifies the lower bounds,
18534         //                    u   specifies the upper bounds.
18535         //                    x   specifies the initial guess
18536         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18537         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18538         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18539 
18540 
18541         //     We start the iteration by initializing task.
18542         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18543         strcpy(task,"START");
18544         for (int i=5;i<60;i++)  task[i]=' ';
18545 
18546         //     This is the call to the L-BFGS-B code.
18547         // (* call the L-BFGS-B routine with task='START' once before loop *)
18548         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18549         //int step = 1;
18550 
18551         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18552         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18553 
18554                 if (strncmp(task,"FG",2)==0) {
18555                 //   the minimization routine has returned to request the
18556                 //   function f and gradient g values at the current x
18557 
18558                 //        Compute function value f for the sample problem.
18559                 rot = new EMData();
18560                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18561                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18562                 //f = -f;
18563                 delete rot;
18564 
18565                 //        Compute gradient g for the sample problem.
18566                 float dt = 1.0e-3f;
18567                 rot = new EMData();
18568                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18569                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18570                 //f1 = -f1;
18571                 g[0] = (f1-f)/dt;
18572                 delete rot;
18573 
18574                 rot = new EMData();
18575                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18576                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18577                 //f2 = -f2;
18578                 g[1] = (f2-f)/dt;
18579                 delete rot;
18580 
18581                 rot = new EMData();
18582                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18583                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18584                 //f3 = -f3;
18585                 g[2] = (f3-f)/dt;
18586                 delete rot;
18587                 }
18588 
18589                 //c          go back to the minimization routine.
18590                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18591                 //step++;
18592         }
18593 
18594         //printf("Total step is %d\n", step);
18595         vector<float> res;
18596         res.push_back(static_cast<float>(x[0]));
18597         res.push_back(static_cast<float>(x[1]));
18598         res.push_back(static_cast<float>(x[2]));
18599         //res.push_back(step);
18600         return res;
18601 }
18602 
18603 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) {
18604 
18605         EMData *proj, *proj2;
18606 
18607         const int nmax=5, mmax=5;
18608         char task[60], csave[60];
18609         long int lsave[4];
18610         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18611         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];
18612         long int SIXTY=60;
18613 
18614         //     We wish to have no output.
18615         iprint = -1;
18616 
18617         //c     We specify the tolerances in the stopping criteria.
18618         factr=1.0e1;
18619         pgtol=1.0e-5;
18620 
18621         //     We specify the dimension n of the sample problem and the number
18622         //        m of limited memory corrections stored.  (n and m should not
18623         //        exceed the limits nmax and mmax respectively.)
18624         n=5;
18625         m=5;
18626 
18627         //     We now provide nbd which defines the bounds on the variables:
18628         //                    l   specifies the lower bounds,
18629         //                    u   specifies the upper bounds.
18630         //                    x   specifies the initial guess
18631         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
18632         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
18633         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
18634         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
18635         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
18636 
18637 
18638         //     We start the iteration by initializing task.
18639         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18640         strcpy(task,"START");
18641         for (int i=5;i<60;i++)  task[i]=' ';
18642 
18643         //     This is the call to the L-BFGS-B code.
18644         // (* call the L-BFGS-B routine with task='START' once before loop *)
18645         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18646         int step = 1;
18647 
18648         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18649         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18650 
18651                 if (strncmp(task,"FG",2)==0) {
18652                 //   the minimization routine has returned to request the
18653                 //   function f and gradient g values at the current x
18654 
18655                 //        Compute function value f for the sample problem.
18656                 proj = new EMData();
18657                 proj2 = new EMData();
18658                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18659                 proj->fft_shuffle();
18660                 proj->center_origin_fft();
18661                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18662                 proj->do_ift_inplace();
18663                 int M = proj->get_ysize()/2;
18664                 proj2 = proj->window_center(M);
18665                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18666                 //f = -f;
18667                 delete proj;
18668                 delete proj2;
18669 
18670                 //        Compute gradient g for the sample problem.
18671                 float dt = 1.0e-3f;
18672                 proj = new EMData();
18673                 proj2 = new EMData();
18674                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18675                 proj->fft_shuffle();
18676                 proj->center_origin_fft();
18677                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18678                 proj->do_ift_inplace();
18679                 proj2 = proj->window_center(M);
18680                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18681                 //ft = -ft;
18682                 delete proj;
18683                 delete proj2;
18684                 g[0] = (ft-f)/dt;
18685 
18686                 proj = new EMData();
18687                 proj2 = new EMData();
18688                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18689                 proj->fft_shuffle();
18690                 proj->center_origin_fft();
18691                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18692                 proj->do_ift_inplace();
18693                 proj2 = proj->window_center(M);
18694                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18695                 //ft = -ft;
18696                 delete proj;
18697                 delete proj2;
18698                 g[1] = (ft-f)/dt;
18699 
18700                 proj = new EMData();
18701                 proj2 = new EMData();
18702                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18703                 proj->fft_shuffle();
18704                 proj->center_origin_fft();
18705                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18706                 proj->do_ift_inplace();
18707                 proj2 = proj->window_center(M);
18708                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18709                 //ft = -ft;
18710                 delete proj;
18711                 delete proj2;
18712                 g[2] = (ft-f)/dt;
18713 
18714                 proj = new EMData();
18715                 proj2 = new EMData();
18716                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18717                 proj->fft_shuffle();
18718                 proj->center_origin_fft();
18719                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18720                 proj->do_ift_inplace();
18721                 proj2 = proj->window_center(M);
18722                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18723                 //ft = -ft;
18724                 delete proj;
18725                 delete proj2;
18726                 g[3] = (ft-f)/dt;
18727 
18728                 proj = new EMData();
18729                 proj2 = new EMData();
18730                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18731                 proj->fft_shuffle();
18732                 proj->center_origin_fft();
18733                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18734                 proj->do_ift_inplace();
18735                 proj2 = proj->window_center(M);
18736                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18737                 //ft = -ft;
18738                 delete proj;
18739                 delete proj2;
18740                 g[4] = (ft-f)/dt;
18741                 }
18742 
18743                 //c          go back to the minimization routine.
18744                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18745                 step++;
18746         }
18747 
18748         //printf("Total step is %d\n", step);
18749         vector<float> res;
18750         res.push_back(static_cast<float>(x[0]));
18751         res.push_back(static_cast<float>(x[1]));
18752         res.push_back(static_cast<float>(x[2]));
18753         res.push_back(static_cast<float>(x[3]));
18754         res.push_back(static_cast<float>(x[4]));
18755         //res.push_back(step);
18756         return res;
18757 }
18758 
18759 
18760 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18761 
18762         double  x[4];
18763         int n;
18764         int l = 3;
18765         int m = 200;
18766         double e = 1e-9;
18767         double step = 0.01;
18768         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
18769 
18770         x[1] = ang;
18771         x[2] = sxs;
18772         x[3] = sys;
18773 
18774         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
18775         //printf("Took %d steps\n", n);
18776 
18777         vector<float> res;
18778         res.push_back(static_cast<float>(x[1]));
18779         res.push_back(static_cast<float>(x[2]));
18780         res.push_back(static_cast<float>(x[3]));
18781         res.push_back(static_cast<float>(n));
18782         return res;
18783 }
18784 
18785 
18786 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
18787 
18788         EMData *rot= new EMData();
18789         float ccc;
18790 
18791         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
18792         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18793         delete rot;
18794         return ccc;
18795 }
18796 
18797 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18798 
18799         double  x[4];
18800         int n;
18801         int l = 3;
18802         int m = 200;
18803         double e = 1e-9;
18804         double step = 0.001;
18805         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
18806 
18807         x[1] = ang;
18808         x[2] = sxs;
18809         x[3] = sys;
18810 
18811         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
18812         //printf("Took %d steps\n", n);
18813 
18814         vector<float> res;
18815         res.push_back(static_cast<float>(x[1]));
18816         res.push_back(static_cast<float>(x[2]));
18817         res.push_back(static_cast<float>(x[3]));
18818         res.push_back(static_cast<float>(n));
18819         return res;
18820 }
18821 
18822 
18823 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
18824 
18825         EMData *rot= new EMData();
18826         float ccc;
18827 
18828         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
18829         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18830         delete rot;
18831         return ccc;
18832 }
18833 
18834 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*nx]
18835 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*nx]
18836 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
18837 {
18838         ENTERFUNC;
18839         /* Exception Handle */
18840         if (!img) {
18841                 throw NullPointerException("NULL input image");
18842         }
18843 
18844         int newx, newy, newz;
18845         bool  keep_going;
18846         cout << " entered   " <<endl;
18847         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
18848         //int size = nx*ny*nz;
18849         EMData * img2 = new EMData();
18850         img2->set_size(nx,ny,nz);
18851         img2->to_zero();
18852         float *img_ptr  =img->get_data();
18853         float *img2_ptr = img2->get_data();
18854         int r2 = ro*ro;
18855         int r3 = r2*ro;
18856         int ri2 = ri*ri;
18857         int ri3 = ri2*ri;
18858 
18859         int n2 = nx/2;
18860 
18861         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
18862                 float z2 = static_cast<float>(k*k);
18863                 for (int j=-n2; j<=n2; j++) {
18864                         float y2 = z2 + j*j;
18865                         if(y2 <= r2) {
18866                                                                                         //cout << "  j  "<<j <<endl;
18867 
18868                                 for (int i=-n2; i<=n2; i++) {
18869                                         float x2 = y2 + i*i;
18870                                         if(x2 <= r3) {
18871                                                                                         //cout << "  i  "<<i <<endl;
18872                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
18873                                                 if(x2 >= ri3) {
18874                                                         //  this is the outer shell, here points can only vanish
18875                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
18876                                                                 //cout << "  1  "<<ib <<endl;
18877                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
18878                                                                         img2_ptr(ib,jb,kb) = 0.0f;
18879                                                                         keep_going = true;
18880                                                                 //cout << "  try  "<<ib <<endl;
18881                                                                         while(keep_going) {
18882                                                                                 newx = Util::get_irand(-ro,ro);
18883                                                                                 newy = Util::get_irand(-ro,ro);
18884                                                                                 newz = Util::get_irand(-ro,ro);
18885                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
18886                                                                                         newx += n2; newy += n2; newz += n2;
18887                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
18888                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
18889                                                                                                 keep_going = false; }
18890                                                                                 }
18891                                                                         }
18892                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
18893                                                         }
18894                                                 }  else  {
18895                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
18896                                                         if(img_ptr(ib,jb,kb) == 1.0) {
18897                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
18898                                                                         //  find out the number of neighbors
18899                                                                         float  numn = -1.0f;  // we already know the central one is 1
18900                                                                         for (newz = -1; newz <= 1; newz++)
18901                                                                                 for (newy = -1; newy <= 1; newy++)
18902                                                                                         for (newx = -1; newx <= 1; newx++)
18903                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
18904                                                                         img2_ptr(ib,jb,kb) = 0.0;
18905                                                                         if(numn == 26.0f) {
18906                                                                                 //  all neighbors exist, it has to vanish
18907                                                                                 keep_going = true;
18908                                                                                 while(keep_going) {
18909                                                                                         newx = Util::get_irand(-ro,ro);
18910                                                                                         newy = Util::get_irand(-ro,ro);
18911                                                                                         newz = Util::get_irand(-ro,ro);
18912                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
18913                                                                                                 newx += n2; newy += n2; newz += n2;
18914                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
18915                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
18916                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
18917                                                                                                                         newx += n2; newy += n2; newz += n2;
18918                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
18919                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
18920                                                                                                                                 keep_going = false; }
18921                                                                                                                 }
18922                                                                                                         }
18923                                                                                                 }
18924                                                                                         }
18925                                                                                 }
18926                                                                         }  else if(numn == 25.0f) {
18927                                                                                 // there is only one empty neighbor, move there
18928                                                                                 for (newz = -1; newz <= 1; newz++) {
18929                                                                                         for (newy = -1; newy <= 1; newy++) {
18930                                                                                                 for (newx = -1; newx <= 1; newx++) {
18931                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
18932                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
18933                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
18934                                                                                                                         }
18935                                                                                                         }
18936                                                                                                 }
18937                                                                                         }
18938                                                                                 }
18939                                                                         }  else {
18940                                                                                 //  more than one neighbor is zero, select randomly one and move there
18941                                                                                 keep_going = true;
18942                                                                                 while(keep_going) {
18943                                                                                         newx = Util::get_irand(-1,1);
18944                                                                                         newy = Util::get_irand(-1,1);
18945                                                                                         newz = Util::get_irand(-1,1);
18946                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
18947                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
18948                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
18949                                                                                                         keep_going = false;
18950                                                                                                 }
18951                                                                                         }
18952                                                                                 }
18953                                                                         }
18954                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
18955                                                         }
18956                                                 }
18957                                         }
18958                                 }
18959                         }
18960                 }
18961         }
18962         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
18963         img2->update();
18964 
18965         EXITFUNC;
18966         return img2;
18967 }
18968 #undef img_ptr
18969 #undef img2_ptr
18970 
18971 struct point3d_t
18972 {
18973         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
18974 
18975         int x;
18976         int y;
18977         int z;
18978 };
18979 
18980 
18981 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
18982 {
18983         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
18984         int noff = 6;
18985 
18986         int nx = visited->get_xsize();
18987         int ny = visited->get_ysize();
18988         int nz = visited->get_zsize();
18989 
18990         vector< point3d_t > pts;
18991         pts.push_back( point3d_t(ix, iy, iz) );
18992         visited->set_value_at( ix, iy, iz, (float)grpid );
18993 
18994         int start = 0;
18995         int end = pts.size();
18996 
18997         while( end > start ) {
18998                 for(int i=start; i < end; ++i ) {
18999                         int ix = pts[i].x;
19000                         int iy = pts[i].y;
19001                         int iz = pts[i].z;
19002 
19003                         for( int j=0; j < noff; ++j ) {
19004                                 int jx = ix + offs[j][0];
19005                                 int jy = iy + offs[j][1];
19006                                 int jz = iz + offs[j][2];
19007 
19008                                 if( jx < 0 || jx >= nx ) continue;
19009                                 if( jy < 0 || jy >= ny ) continue;
19010                                 if( jz < 0 || jz >= nz ) continue;
19011 
19012 
19013                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19014                                     pts.push_back( point3d_t(jx, jy, jz) );
19015                                     visited->set_value_at( jx, jy, jz, (float)grpid );
19016                                 }
19017 
19018                         }
19019                 }
19020 
19021                 start = end;
19022                 end = pts.size();
19023         }
19024         return pts.size();
19025 }
19026 
19027 
19028 EMData* Util::get_biggest_cluster( EMData* mg )
19029 {
19030         int nx = mg->get_xsize();
19031         int ny = mg->get_ysize();
19032         int nz = mg->get_zsize();
19033 
19034         EMData* visited = new EMData();
19035         visited->set_size( nx, ny, nz );
19036         visited->to_zero();
19037         int grpid = 0;
19038         int maxgrp = 0;
19039         int maxsize = 0;
19040         for( int iz=0; iz < nz; ++iz ) {
19041                 for( int iy=0; iy < ny; ++iy ) {
19042                         for( int ix=0; ix < nx; ++ix ) {
19043                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19044 
19045                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
19046                                         // visited before, must be in other group.
19047                                         continue;
19048                                 }
19049 
19050                                 grpid++;
19051                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19052                                 if( grpsize > maxsize ) {
19053                                         maxsize = grpsize;
19054                                         maxgrp = grpid;
19055                                 }
19056                         }
19057                 }
19058         }
19059 
19060         Assert( maxgrp > 0 );
19061 
19062         int npoint = 0;
19063         EMData* result = new EMData();
19064         result->set_size( nx, ny, nz );
19065         result->to_zero();
19066 
19067         for( int iz=0; iz < nz; ++iz ) {
19068                 for( int iy=0; iy < ny; ++iy ) {
19069                         for( int ix=0; ix < nx; ++ix ) {
19070                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
19071                                         (*result)(ix,iy,iz) = 1.0;
19072                                         npoint++;
19073                                 }
19074                         }
19075                 }
19076         }
19077 
19078         Assert( npoint==maxsize );
19079         delete visited;
19080         return result;
19081 
19082 }
19083 
19084 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)
19085 {
19086         int   ix, iy, iz;
19087         int   i,  j, k;
19088         int   nr2, nl2;
19089         float  dzz, az, ak;
19090         float  scx, scy, scz;
19091         int offset = 2 - nx%2;
19092         int lsm = nx + offset;
19093         EMData* ctf_img1 = new EMData();
19094         ctf_img1->set_size(lsm, ny, nz);
19095         float freq = 1.0f/(2.0f*ps);
19096         scx = 2.0f/float(nx);
19097         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19098         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19099         nr2 = ny/2 ;
19100         nl2 = nz/2 ;
19101         for ( k=0; k<nz;k++) {
19102                 iz = k;  if(k>nl2) iz=k-nz;
19103                 for ( j=0; j<ny;j++) {
19104                         iy = j;  if(j>nr2) iy=j - ny;
19105                         for ( i=0; i<lsm/2; i++) {
19106                                 ix=i;
19107                                 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19108                                 if(ak!=0) az=0.0; else az=M_PI;
19109                                 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19110                                 (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19111                                 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19112                         }
19113                 }
19114         }
19115         ctf_img1->update();
19116         ctf_img1->set_complex(true);
19117         ctf_img1->set_ri(true);
19118         //ctf_img1->attr_dict["is_complex"] = 1;
19119         //ctf_img1->attr_dict["is_ri"] = 1;
19120         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19121         return ctf_img1;
19122 }
19123 /*
19124 #define  cent(i)     out[i+N]
19125 #define  assign(i)   out[i]
19126 vector<float> Util::cluster_pairwise(EMData* d, int K) {
19127 
19128         int nx = d->get_xsize();
19129         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19130         vector<float> out(N+K+2);
19131         if(N*(N-1)/2 != nx) {
19132                 //print  "  incorrect dimension"
19133                 return out;}
19134         //  assign random objects as centers
19135         for(int i=0; i<N; i++) assign(i) = float(i);
19136         // shuffle
19137         for(int i=0; i<N; i++) {
19138                 int j = Util::get_irand(0,N-1);
19139                 float temp = assign(i);
19140                 assign(i) = assign(j);
19141                 assign(j) = temp;
19142         }
19143         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19144         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19145         //
19146         for(int i=0; i<N; i++) assign(i) = 0.0f;
19147         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19148         bool change = true;
19149         int it = -1;
19150         while(change && disp < dispold) {
19151                 change = false;
19152                 dispold = disp;
19153                 it++;
19154                 //cout<<"Iteration:  "<<it<<endl;
19155                 // dispersion is a sum of distance from objects to object center
19156                 disp = 0.0f;
19157                 for(int i=0; i<N; i++) {
19158                         qm = 1.0e23f;
19159                         for(int k=0; k<K; k++) {
19160                                 if(float(i) == cent(k)) {
19161                                         qm = 0.0f;
19162                                         na = (float)k;
19163                                 } else {
19164                                         float dt = (*d)(mono(i,int(cent(k))));
19165                                         if(dt < qm) {
19166                                                 qm = dt;
19167                                                 na = (float)k;
19168                                         }
19169                                 }
19170                         }
19171                         disp += qm;
19172                         if(na != assign(i)) {
19173                                 assign(i) = na;
19174                                 change = true;
19175                         }
19176                 }
19177         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19178                 //print disp
19179                 //print  assign
19180                 // find centers
19181                 for(int k=0; k<K; k++) {
19182                         qm = 1.0e23f;
19183                         for(int i=0; i<N; i++) {
19184                                 if(assign(i) == float(k)) {
19185                                         float q = 0.0;
19186                                         for(int j=0; j<N; j++) {
19187                                                 if(assign(j) == float(k)) {
19188                                                                 //it cannot be the same object
19189                                                         if(i != j)  q += (*d)(mono(i,j));
19190                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19191                                                 }
19192                                         }
19193                                         if(q < qm) {
19194                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19195                                                 qm = q;
19196                                                 cent(k) = float(i);
19197                                         }
19198                                 }
19199                         }
19200                 }
19201         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19202         }
19203         out[N+K] = disp;
19204         out[N+K+1] = float(it);
19205         return  out;
19206 }
19207 #undef  cent
19208 #undef  assign
19209 */
19210 #define  cent(i)     out[i+N]
19211 #define  assign(i)   out[i]
19212 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19213         int nx = d->get_xsize();
19214         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19215         vector<float> out(N+K+2);
19216         if(N*(N-1)/2 != nx) {
19217                 //print  "  incorrect dimension"
19218                 return out;}
19219         //  assign random objects as centers
19220         for(int i=0; i<N; i++) assign(i) = float(i);
19221         // shuffle
19222         for(int i=0; i<N; i++) {
19223                 int j = Util::get_irand(0,N-1);
19224                 float temp = assign(i);
19225                 assign(i) = assign(j);
19226                 assign(j) = temp;
19227         }
19228         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19229         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19230         //
19231         for(int i=0; i<N; i++) assign(i) = 0.0f;
19232         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19233         bool change = true;
19234         int it = -1;
19235         int ct = -1;
19236         while(change && disp < dispold || ct > 0) {
19237 
19238                 change = false;
19239                 dispold = disp;
19240                 it++;
19241 
19242                 // dispersion is a sum of distance from objects to object center
19243                 disp = 0.0f;
19244                 ct = 0;
19245                 for(int i=0; i<N; i++) {
19246                         qm = 1.0e23f;
19247                         for(int k=0; k<K; k++) {
19248                                 if(float(i) == cent(k)) {
19249                                         qm = 0.0f;
19250                                         na = (float)k;
19251                                 } else {
19252                                         float dt = (*d)(mono(i,int(cent(k))));
19253                                         if(dt < qm) {
19254                                                 qm = dt;
19255                                                 na = (float)k;
19256                                         }
19257                                 }
19258                         }
19259 
19260 
19261                         // Simulated annealing
19262                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19263                             na = (float)(Util::get_irand(0, K));
19264                             qm = (*d)(mono(i,int(na)));
19265                             ct++;
19266                         }
19267 
19268                         disp += qm;
19269 
19270                         if(na != assign(i)) {
19271                                 assign(i) = na;
19272                                 change = true;
19273                         }
19274                 }
19275 
19276                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
19277                 T = T*F;
19278 
19279         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19280                 //print disp
19281                 //print  assign
19282                 // find centers
19283                 for(int k=0; k<K; k++) {
19284                         qm = 1.0e23f;
19285                         for(int i=0; i<N; i++) {
19286                                 if(assign(i) == float(k)) {
19287                                         float q = 0.0;
19288                                         for(int j=0; j<N; j++) {
19289                                                 if(assign(j) == float(k)) {
19290                                                                 //it cannot be the same object
19291                                                         if(i != j)  q += (*d)(mono(i,j));
19292                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19293                                                 }
19294                                         }
19295                                         if(q < qm) {
19296                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19297                                                 qm = q;
19298                                                 cent(k) = float(i);
19299                                         }
19300                                 }
19301                         }
19302                 }
19303         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19304         }
19305         out[N+K] = disp;
19306         out[N+K+1] = float(it);
19307         return  out;
19308 }
19309 #undef  cent
19310 #undef  assign
19311 /*
19312 #define  groupping(i,k)   group[i + k*m]
19313 vector<float> Util::cluster_equalsize(EMData* d, int m) {
19314         int nx = d->get_xsize();
19315         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19316         int K = N/m;
19317         //cout<<"  K  "<<K<<endl;
19318         vector<float> group(N+1);
19319         if(N*(N-1)/2 != nx) {
19320                 //print  "  incorrect dimension"
19321                 return group;}
19322         bool active[N];
19323         for(int i=0; i<N; i++) active[i] = true;
19324 
19325         float dm, qd;
19326         int   ppi, ppj;
19327         for(int k=0; k<K; k++) {
19328                 // find two most similiar objects among active
19329                 cout<<"  k  "<<k<<endl;
19330                 dm = 1.0e23;
19331                 for(int i=1; i<N; i++) {
19332                         if(active[i]) {
19333                                 for(int j=0; j<i; j++) {
19334                                         if(active[j]) {
19335                                                 qd = (*d)(mono(i,j));
19336                                                 if(qd < dm) {
19337                                                         dm = qd;
19338                                                         ppi = i;
19339                                                         ppj = j;
19340                                                 }
19341                                         }
19342                                 }
19343                         }
19344                 }
19345                 groupping(0,k) = float(ppi);
19346                 groupping(1,k) = float(ppj);
19347                 active[ppi] = false;
19348                 active[ppj] = false;
19349 
19350                 // find progressively objects most similar to those in the current list
19351                 for(int l=2; l<m; l++) {
19352                         //cout<<"  l  "<<l<<endl;
19353                         dm = 1.0e23;
19354                         for(int i=0; i<N; i++) {
19355                                 if(active[i]) {
19356                                         qd = 0.0;
19357                                         for(int j=0; j<l; j++) { //j in groupping[k]:
19358                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
19359                                                 int jj = int(groupping(j,k));
19360                         //cout<<"   "<<jj<<endl;
19361                                                 qd += (*d)(mono(i,jj));
19362                                         }
19363                                         if(qd < dm) {
19364                                                 dm = qd;
19365                                                 ppi = i;
19366                                         }
19367                                 }
19368                         }
19369                         groupping(l,k) = float(ppi);
19370                         active[ppi] = false;
19371                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
19372                 }
19373                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
19374         }
19375         // there might be remaining objects when N is not divisible by m, simply put them in one group
19376         if(N%m != 0) {
19377                 int j = K*m;
19378                 K++;
19379                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
19380                 for(int i=0; i<N; i++) {
19381                         if(active[i]) {
19382                                 group[j] = float(i);
19383                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
19384                                 j++;
19385                         }
19386                 }
19387         }
19388 
19389         int  cent[K];
19390          // find centers
19391         for(int k=0; k<K; k++) {
19392                 float qm = 1.0e23f;
19393                 for(int i=0; i<N; i++) {
19394                         if(group[i] == float(k)) {
19395                                 qd = 0.0;
19396                                 for(int j=0; j<N; j++) {
19397                                         if(group[j] == float(k)) {
19398                                                 //it cannot be the same object
19399                                                 if(i != j)  qd += (*d)(mono(i,j));
19400                                         }
19401                                 }
19402                                 if(qd < qm) {
19403                                         qm = qd;
19404                                         cent[k] = i;
19405                                 }
19406                         }
19407                 }
19408         }
19409         // dispersion is a sum of distances from objects to object center
19410         float disp = 0.0f;
19411         for(int i=0; i<N; i++) {
19412                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
19413         }
19414         group[N] = disp;
19415         return  group;
19416 }
19417 #undef  groupping
19418 */
19419 
19420 vector<float> Util::cluster_equalsize(EMData* d) {
19421         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19422         int nx = d->get_xsize();
19423         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19424         int K = N/2;
19425         vector<float> group(N);
19426         if(N*(N-1)/2 != nx) {
19427                 //print  "  incorrect dimension"
19428                 return group;}
19429         //bool active[N];       //this does not compile in VS2005. --Grant Tang
19430         bool * active = new bool[N];
19431         for(int i=0; i<N; i++) active[i] = true;
19432 
19433         float dm, qd;
19434         int   ppi = 0, ppj = 0;
19435         for(int k=0; k<K; k++) {
19436                 // find pairs of most similiar objects among active
19437                 //cout<<"  k  "<<k<<endl;
19438                 dm = 1.0e23f;
19439                 for(int i=1; i<N; i++) {
19440                         if(active[i]) {
19441                                 for(int j=0; j<i; j++) {
19442                                         if(active[j]) {
19443                                                 qd = (*d)(i*(i - 1)/2 + j);
19444                                                 if(qd < dm) {
19445                                                         dm = qd;
19446                                                         ppi = i;
19447                                                         ppj = j;
19448                                                 }
19449                                         }
19450                                 }
19451                         }
19452                 }
19453                 group[2*k] = float(ppi);
19454                 group[1+2*k] = float(ppj);
19455                 active[ppi] = false;
19456                 active[ppj] = false;
19457         }
19458 
19459         delete [] active;
19460         active = NULL;
19461         return  group;
19462 }
19463 /*
19464 #define son(i,j)=i*(i-1)/2+j
19465 vector<float> Util::cluster_equalsize(EMData* d) {
19466         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19467         int nx = d->get_xsize();
19468         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19469         int K = N/2;
19470         vector<float> group(N);
19471         if(N*(N-1)/2 != nx) {
19472                 //print  "  incorrect dimension"
19473                 return group;}
19474         //bool active[N];
19475         int  active[N];
19476         for(int i=0; i<N; i++) active[i] = i;
19477 
19478         float dm, qd;
19479         int   ppi = 0, ppj = 0, ln = N;
19480         for(int k=0; k<K; k++) {
19481                 // find pairs of most similiar objects among active
19482                 //cout<<"  k:  "<<k<<endl;
19483                 dm = 1.0e23;
19484                 for(int i=1; i<ln; i++) {
19485                         for(int j=0; j<i; j++) {
19486                                 //qd = (*d)(mono(active[i],active[j]));
19487                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
19488                                 if(qd < dm) {
19489                                         dm = qd;
19490                                         ppi = i;
19491                                         ppj = j;
19492                                 }
19493                         }
19494                 }
19495                 group[2*k]   = float(active[ppi]);
19496                 group[1+2*k] = float(active[ppj]);
19497                 //  Shorten the list
19498                 if(ppi > ln-3 || ppj > ln - 3) {
19499                         if(ppi > ln-3 && ppj > ln - 3) {
19500                         } else if(ppi > ln-3) {
19501                                 if(ppi == ln -1) active[ppj] = active[ln-2];
19502                                 else             active[ppj] = active[ln-1];
19503                         } else { // ppj>ln-3
19504                                 if(ppj == ln -1) active[ppi] = active[ln-2];
19505                                 else             active[ppi] = active[ln-1];
19506                         }
19507                 } else {
19508                         active[ppi] = active[ln-1];
19509                         active[ppj] = active[ln-2];
19510                 }
19511                 ln = ln - 2;
19512         }
19513         return  group;
19514 }
19515 
19516 */
19517 #define data(i,j) group[i*ny+j]
19518 vector<float> Util::vareas(EMData* d) {
19519         const float step=0.001f;
19520         int ny = d->get_ysize();
19521         //  input emdata should have size 2xN, where N is number of points
19522         //  output vector should be 2xN, first element is the number of elements
19523         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
19524         vector<float> group(2*ny);
19525         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
19526         int K = int(1.0f/step) +1;
19527         int hit = 0;
19528         for(int kx=0; kx<=K; kx++) {
19529                 float tx = kx*step;
19530                 for(int ky=0; ky<=K; ky++) {
19531                         float ty = ky*step;
19532                         float dm = 1.0e23f;
19533                         for(int i=0; i<ny; i++) {
19534                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
19535                                 if( qd < dm) {
19536                                         dm = qd;
19537                                         hit = i;
19538                                 }
19539                         }
19540                         data(0,hit) += 1.0f;
19541                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
19542                 }
19543         }
19544         return  group;
19545 }
19546 #undef data
19547 
19548 EMData* Util::get_slice(EMData *vol, int dim, int index) {
19549 
19550         int nx = vol->get_xsize();
19551         int ny = vol->get_ysize();
19552         int nz = vol->get_zsize();
19553         float *vol_data = vol->get_data();
19554         int new_nx, new_ny;
19555 
19556         if (nz == 1)
19557                 throw ImageDimensionException("Error: Input must be a 3-D object");
19558         if ((dim < 1) || (dim > 3))
19559                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
19560         if (((dim == 1) && (index < 0 || index > nx-1)) ||
19561           ((dim == 1) && (index < 0 || index > nx-1)) ||
19562           ((dim == 1) && (index < 0 || index > nx-1)))
19563                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
19564 
19565         if (dim == 1) {
19566                 new_nx = ny;
19567                 new_ny = nz;
19568         } else if (dim == 2) {
19569                 new_nx = nx;
19570                 new_ny = nz;
19571         } else {
19572                 new_nx = nx;
19573                 new_ny = ny;
19574         }
19575 
19576         EMData *slice = new EMData();
19577         slice->set_size(new_nx, new_ny, 1);
19578         float *slice_data = slice->get_data();
19579 
19580         if (dim == 1) {
19581                 for (int x=0; x<new_nx; x++)
19582                         for (int y=0; y<new_ny; y++)
19583                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
19584         } else if (dim == 2) {
19585                 for (int x=0; x<new_nx; x++)
19586                         for (int y=0; y<new_ny; y++)
19587                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
19588         } else {
19589                 for (int x=0; x<new_nx; x++)
19590                         for (int y=0; y<new_ny; y++)
19591                                 slice_data[y*new_nx+x] = vol_data[(index*ny+y)*nx+x];
19592         }
19593 
19594         return slice;
19595 }
19596 
19597 void Util::image_mutation(EMData *img, float mutation_rate) {
19598         int nx = img->get_xsize();
19599         float min = img->get_attr("minimum");
19600         float max = img->get_attr("maximum");
19601         float* img_data = img->get_data();
19602         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
19603         return;
19604 }
19605 
19606 
19607 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19608 
19609         if (is_mirror != 0) {
19610                 for (int i=0; i<len_list; i++) {
19611                         int r = rand()%10000;
19612                         float f = r/10000.0f;
19613                         if (f < mutation_rate) list[i] = 1-list[i];
19614                 }
19615         } else {
19616                 map<int, vector<int> >  graycode;
19617                 map<vector<int>, int> rev_graycode;
19618                 vector <int> gray;
19619 
19620                 int K=1;
19621                 for (int i=0; i<L; i++) K*=2;
19622 
19623                 for (int k=0; k<K; k++) {
19624                         int shift = 0;
19625                         vector <int> gray;
19626                         for (int i=L-1; i>-1; i--) {
19627                                 int t = ((k>>i)%2-shift)%2;
19628                                 gray.push_back(t);
19629                                 shift += t-2;
19630                         }
19631                         graycode[k] = gray;
19632                         rev_graycode[gray] = k;
19633                 }
19634 
19635                 float gap = (K-1)/(max_val-min_val);
19636                 for (int i=0; i<len_list; i++) {
19637                         float val = list[i];
19638                         if (val < min_val) { val = min_val; }
19639                         else if  (val > max_val) { val = max_val; }
19640                         int k = int((val-min_val)*gap+0.5);
19641                         vector<int> gray = graycode[k];
19642                         bool changed = false;
19643                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19644                                 int r = rand()%10000;
19645                                 float f = r/10000.0f;
19646                                 if (f < mutation_rate) {
19647                                         *p = 1-*p;
19648                                         changed = true;
19649                                 }
19650                         }
19651                         if (changed) {
19652                                 k = rev_graycode[gray];
19653                                 list[i] = k/gap+min_val;
19654                         }
19655                 }
19656         }
19657 
19658 }
19659 
19660 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19661 
19662         if (is_mirror != 0) {
19663                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19664                         int r = rand()%10000;
19665                         float f = r/10000.0f;
19666                         if (f < mutation_rate) *q = 1-*q;
19667                 }
19668         } else {
19669                 map<int, vector<int> >  graycode;
19670                 map<vector<int>, int> rev_graycode;
19671                 vector <int> gray;
19672 
19673                 int K=1;
19674                 for (int i=0; i<L; i++) K*=2;
19675 
19676                 for (int k=0; k<K; k++) {
19677                         int shift = 0;
19678                         vector <int> gray;
19679                         for (int i=L-1; i>-1; i--) {
19680                                 int t = ((k>>i)%2-shift)%2;
19681                                 gray.push_back(t);
19682                                 shift += t-2;
19683                         }
19684                         graycode[k] = gray;
19685                         rev_graycode[gray] = k;
19686                 }
19687 
19688                 float gap = (K-1)/(max_val-min_val);
19689                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19690                         float val = *q;
19691                         if (val < min_val) { val = min_val; }
19692                         else if  (val > max_val) { val = max_val; }
19693                         int k = int((val-min_val)*gap+0.5);
19694                         vector<int> gray = graycode[k];
19695                         bool changed = false;
19696                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19697                                 int r = rand()%10000;
19698                                 float f = r/10000.0f;
19699                                 if (f < mutation_rate) {
19700                                         *p = 1-*p;
19701                                         changed = true;
19702                                 }
19703                         }
19704                         if (changed) {
19705                                 k = rev_graycode[gray];
19706                                 *q = k/gap+min_val;
19707                         }
19708                 }
19709         }
19710         return list;
19711 }
19712 
19713 
19714 void Util::bb_enumerate_(int* argParts, int* dimClasses, int nParts, int K, int T,int n_guesses, int* Levels) {
19715 
19716         int* Indices = new int[nParts*K];
19717         // Make a vector of nParts vectors of K int* each
19718         vector <vector <int*> > Parts(nParts,vector<int*>(K));
19719         int ind_c = 0;
19720 
19721         for (int i=0; i < nParts; i++){
19722                 for(int j = 0; j < K; j++){
19723                         Parts[i][j]=argParts + ind_c;
19724                         Indices[i*K + j] = ind_c;
19725                         ind_c = ind_c + *(dimClasses+i*K + j);
19726 
19727                 }
19728         }
19729 
19730         // make a copy of vector Parts for debugging purposes.
19731         // comment out if not debugging
19732         vector <vector <int*> > oldParts = Parts;
19733 
19734         // in the following we call initial_prune with Parts which is a vector. This is not the most
19735         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
19736         // the running time for 7 partitions with 288 classes per partition is on the order of a couple of minutes at most, i'll just leave it for now.....
19737         Util::initial_prune(Parts, dimClasses, nParts, K,T);
19738         //**********************************************************************************************************************************************
19739 
19740         // figure out the partition with the smallest number of classes. that will be the MAXIMUM number of matches we can find
19741         int numLevels = Parts[0].size();// initialize to number of classes in the first partition
19742         for (int i=1; i < nParts; i++){
19743                 if (Parts[i].size() < numLevels) numLevels = Parts[i].size();
19744         }
19745 
19746         // To maintain feasibility there can be at most
19747         // numLevel matches in the optimal solution.
19748 
19749         // int* Levels = new int[numLevels]; // Levels[i] corresponds to the number of possibilities we consider for the i-th match, and this
19750                                           // determines how many branches occur at that level.
19751         // numLevels is at most K. Since Levels is pre-allocated in python code with K elements, it should be fine.
19752         //for(int i =0; i < numLevels; i++)
19753         //      Levels[i] = 1;
19754         // modify argParts so the dummy variable of each class is set to 1 if the class is not removed by initial_prune, and -1 otherwise.
19755         // since the branch function is extremely computationally intensive, we have to pass it argParts (the 1-dimensional array) instead
19756         // of the vector Parts (which doesn't allow for direct access).
19757 
19758         // Indices[i*K + j] is the number of offsets from the beginning of argParts of the first element of the j-th class of hte i-th partition
19759 
19760         for(int i = 0; i < nParts; i++){
19761                 for(int j=0; j < K; j++){
19762                         *(argParts + Indices[i*K + j]+1) = -1;
19763                 }
19764         }
19765 
19766         int num_classes;
19767         int old_index;
19768         for(int i=0; i<nParts; i++){
19769                 num_classes = Parts[i].size();// number of classes in partition i after pruning
19770                 //cout<<"num_classes: "<< num_classes<<"\n";
19771                 for (int j=0; j < num_classes; j++){
19772                         old_index = *(Parts[i][j]);
19773                         //cout << "old_index: " << old_index<<"\n";
19774                         *(argParts + Indices[i*K + old_index]+1) = 1;
19775                 }
19776         }
19777 
19778         // 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
19779         // and the rest is the list of matches
19780         // in one dimensional form.
19781         cout <<"begin partition matching\n";
19782         int* output = Util::branch(argParts, Indices,dimClasses, nParts, K, T,Levels, numLevels,0,n_guesses);
19783         cout <<"done with partition matching \n";
19784         cout<<"total cost: "<<*output<<"\n";
19785         cout<<"number of matches: "<<*(output+1)<<"\n";
19786         // 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
19787         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
19788 }
19789 
19790 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
19791         //cout<<"sanitycheck called\n";
19792         int total_cost = *output;
19793         int num_matches = *(output+1);
19794 
19795         int cost=0;
19796         int* intx;
19797         int intx_size;
19798         int* intx_next;
19799         int intx_next_size;
19800         int curclass;
19801         int curclass_size;
19802         //cout<<"cost by match: [";
19803         for(int i = 0; i < num_matches; i++){
19804                 curclass = *(output+2+ i*nParts);
19805                 // check feasibility
19806                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
19807                 *(argParts + Indices[curclass]+1) = -5;
19808                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
19809                 curclass_size = *(dimClasses+curclass)-2;
19810                 intx = new int[curclass_size];
19811                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
19812                 intx_size = curclass_size;
19813 
19814                 for (int j=1; j < nParts; j++){
19815                       curclass = *(output+2+ i*nParts+j);
19816                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
19817                       *(argParts + Indices[j*K+curclass]+1)=-5;
19818                       // compute the intersection of intx and class curclass of partition j of the i-th match
19819                       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);
19820                       intx_next = new int[intx_next_size];
19821                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
19822                       delete[] intx;
19823                       intx=intx_next;
19824                       intx_size= intx_next_size;
19825                       if (j==nParts-1) delete[] intx_next;
19826                 }
19827 
19828                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
19829                 //cout <<intx_next_size<<",";
19830                 cost = cost + intx_next_size;
19831         }
19832         //cout<<"]\n";
19833         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
19834 
19835         return 1;
19836 
19837 }
19838 
19839 int* Util::branch(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses) {
19840         // Base Case: we're at a leaf, no more feasible matches possible
19841 
19842         if (curlevel > nLevels-1){
19843                 int* res = new int[2];
19844                 *res = 0;
19845                 *(res+1)=0;
19846                 return res;
19847         }
19848 
19849         // We may still find more feasible matchings with cost gt T, so explore level curlevel
19850         int nBranches = *(Levels + curlevel);
19851 
19852         // call findTopLargest to get the nBranches feasible matchings with the largest weight (gt T) over all other feasible matches
19853         // matchlist is in one dimensional array form......
19854 
19855         int* matchlist = new int[nBranches*nParts];
19856         int* costlist = new int[nBranches];// cost of each of the nBranches matches. If cost[i] < T then that means findTopLargest found less than i+1 matches
19857                                            // with cost > T
19858 
19859         // initialize elements of costlist to 0
19860         for (int i=0; i < nBranches; i++) *(costlist+i)=0;
19861 
19862         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
19863         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
19864 
19865         Util::findTopLargest(argParts,Indices, dimClasses, nParts, K,  T, matchlist, nBranches,costlist,n_guesses);
19866 
19867         // if there are no feasible matches with cost gt T, then return 0
19868         if (costlist[0]<= T){
19869                 int* res = new int[2];
19870                 *res = 0;
19871                 *(res+1)=0;
19872                 return res;
19873         }
19874 
19875         int* maxreturn = new int[2];//initialize to placeholder
19876         *maxreturn=0;
19877         *(maxreturn+1)=0;
19878 
19879         // some temporary variables
19880         int old_index;
19881         int totalcost;
19882         int nmatches;
19883         //int offset;
19884 
19885         for(int i=0; i < nBranches ; i++){
19886 
19887                 // consider the i-th match returned by findTopLargest
19888 
19889                 if (costlist[i] <= T) break;
19890 
19891                 // 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.
19892                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
19893 
19894                 for(int j=0; j < nParts; j++){
19895                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
19896                         old_index=matchlist[i*nParts + j];
19897                         *(argParts + Indices[j*K+old_index] + 1) = -2;
19898                 }
19899 
19900 
19901                 int* ret = Util::branch(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses);
19902 
19903                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
19904                 totalcost = costlist[i] + *ret;
19905 
19906                 // *************************************************************************************
19907                 // for debugging purposes
19908 
19909                 // debug 1: for multi-branching in levels i less some pre-specified maxLevel. Assume maxLevel is pretty small else way too many print outs to be useful
19910                    bool debug1 = 0;
19911                    if (debug1){
19912                        int maxLevel = 2;
19913                         if (curlevel < maxLevel) cout<<"total cost level" << curlevel<<": "<<totalcost<<"\n";
19914                    }
19915 
19916                 // debug 2: for multi-branching in ALL (or many ...) levels. This is data specific so it's all hard coded
19917                    bool debug2 = 0;
19918                    if (debug2){
19919                         int skip1 = 5;
19920                         int max1=20;
19921                          if ((curlevel <= max1) && (curlevel%skip1 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
19922 
19923                         int skip2 = 10;
19924                         int max2 = 70;
19925                         if ((curlevel > max1 )&&(curlevel <= max2) && (curlevel%skip2 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
19926                    }
19927                 // *************************************************************************************
19928 
19929 
19930                 // if totalcost > maxcost, or some variatio thereof, then copy the stuff over to maxreturn.
19931                 // There are several possibilities here:
19932                 //    Option 1: Simply compare costs and take the largest one.
19933                 //    Option 2: At each level, if two costs are equal, then take the one which contains fewer matches, and thus
19934                 //              ensuring matches with larger weights. The motivation for this is largely the (possibly naive) assumption that
19935                 //              if we take the average of a larger number of images, then the averaged image will be "better".
19936                 //    Option 3: Do option 2 only on the highest level, i.e., curlevel=0
19937 
19938                  if (totalcost > *maxreturn) // option 1
19939                 // if ((totalcost > *maxreturn) || ( (curlevel==0) && (totalcost == *maxreturn) && (*(ret+1)+1 < *(maxreturn+1)) )) // option 3
19940 
19941                 //if ((totalcost > *maxreturn) || ( (totalcost == *maxreturn) && (*(ret+1)+1 < *(maxreturn+1)) )) // option 2
19942                 {
19943                         nmatches = 1 + *(ret+1);
19944                         delete[] maxreturn; // get rid of the old maxreturn
19945                         maxreturn = new int[2+nmatches*nParts];
19946                         *maxreturn = totalcost;
19947                         *(maxreturn + 1)= nmatches;
19948                         int nret = 2+(nmatches-1)*nParts;
19949                         for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
19950                         for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
19951                 }
19952 
19953 
19954                 delete[] ret;
19955 
19956                 // unmark the marked classes in preparation for the next iteration
19957 
19958                 for(int j=0; j < nParts; j++){
19959                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
19960                         old_index=matchlist[i*nParts + j];
19961                         *(argParts + Indices[j*K+old_index] + 1) = 1;
19962                 }
19963 
19964         }
19965 
19966         delete[] matchlist;
19967         delete[] costlist;
19968         return maxreturn;
19969 
19970 }
19971 
19972 int Util::findTopLargest(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int max_num_matches, int* costlist, int n_guesses){
19973         int guess;
19974         int* curmax = new int[nParts+1]; // first element is the max weight and the subsequent elements is the match with the weight.
19975         int newT=T;
19976         int num_found=0;
19977 
19978         for(int i=0; i < max_num_matches; i++){
19979                 guess = Util::generatesubmax(argParts, Indices,dimClasses,nParts, K, T,  n_guesses);
19980 
19981                 if (T < guess) newT = guess -1;
19982                 // find the feasible match with the largest weight and put results in curmax
19983                 Util::search2(argParts, Indices,dimClasses,nParts, K, newT,curmax);
19984                 if (*curmax <= T){
19985                         max_num_matches=i;
19986                         break;
19987                 }
19988                 else {
19989                         *(costlist+i) = *curmax;
19990 
19991                         for (int j=0; j<nParts; j++){
19992                                 *(matchlist+i*nParts+j) = *(curmax+1+j);
19993                                 *(argParts + Indices[j*K+*(curmax+1+j)] + 1) = -3;// mark the classes in curmax as unavailable using -3 (remember to change it back)
19994 
19995                         }
19996                         num_found = num_found+1;
19997                 }
19998 
19999         }
20000 
20001 
20002         delete[] curmax;
20003         // go through the selected classes (in matchlist) and reset to 1
20004 
20005         for (int i=0 ; i < max_num_matches; i++){
20006                 for (int j = 0; j < nParts; j++){
20007                         *(argParts + Indices[j*K+*(matchlist+i*nParts +j)] + 1) = 1;
20008                 }
20009 
20010         }
20011 
20012 
20013         return num_found;
20014 }
20015 
20016 
20017 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curmax){
20018         // initialize the current max weight to 0
20019         *curmax= 0;
20020         // some temp variables
20021         bool flag;
20022         int nintx;
20023         int* dummy;
20024         int* ret;
20025 
20026         for(int a=0; a<K; a++)
20027         {
20028                 // check that class a of partition 0 is active and has greater than newT elements. If not the case, then skip to the next class
20029                 if (*(argParts + Indices[a] + 1) < 1) continue;
20030                 if (*(dimClasses + a)-2 <= newT) continue;
20031 
20032                 // 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
20033 
20034                 for( int i=1; i < nParts; i++){
20035                         flag = 0; // if flag stays 0 then no class in this partition is active, which implies no feasible match with class a of part 0
20036                         for(int j=0; j < K; j++){
20037                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20038                                 if (*(dimClasses + i*K+j)-2 <= newT) {*(argParts + Indices[i*K+j] + 1) =-4; continue;}
20039                                 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);
20040                                 if (nintx > newT) flag=1;
20041                                 else *(argParts + Indices[i*K+j] + 1) =-4;
20042                         }
20043                         if (flag==0) {break;}
20044                 }
20045 
20046                 // explore determines the feasible match with the largest weight greater than newT
20047                 if (flag > 0){ // Each partition has one or more active class
20048                         ret=Util::explore2(argParts, Indices, dimClasses, nParts, K, newT, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2, *(dimClasses+a)-2,0);
20049 
20050                         if (*ret > *curmax){
20051                                 *curmax = *ret;
20052                                 *(curmax+1)=a;
20053                                 for (int cp =0; cp < nParts-1; cp++) *(curmax+2+cp) = *(ret+1+cp);
20054 
20055                         }
20056                         delete[] ret;
20057                 }
20058                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
20059                 for( int i=1; i < nParts; i++){
20060                         for(int j=0; j < K; j++){
20061                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20062 
20063                         }
20064                 }
20065 
20066 
20067 
20068         }
20069 
20070 
20071 }
20072 
20073 
20074 int* Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curintx, int size_curintx, int* next, int size_next, int depth){
20075 // depth is the level which is going to be explored in the current iteration
20076         int* curintx2;
20077 
20078         int nintx = size_curintx;
20079 
20080         // take the intx of next and cur
20081         if (depth >0){
20082                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20083                 if (nintx <= newT) {curintx2 = new int[1]; *curintx2=0;return curintx2;}
20084         }
20085 
20086         // we're at a leaf so return.
20087         if (depth == (nParts-1)) { curintx2 = new int[1]; *curintx2 = nintx; return curintx2;}
20088 
20089 
20090         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20091 
20092         if (depth > 0){
20093                 curintx2 = new int[nintx]; // put the intersection set in here
20094                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20095         }
20096 
20097         if (depth == 0){
20098                 // set curintx2 to curintx
20099                 curintx2 = new int[size_curintx];
20100                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20101         }
20102 
20103 
20104         // recursion (non-leaf case)
20105         depth=depth+1;
20106         int* curmax = new int[nParts-depth+1];
20107         *curmax=0;
20108         int* ret;
20109         // we now consider each of the classes in partition depth in turn
20110         for (int i=0; i < K; i++){
20111 
20112                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
20113                 size_next = (*(dimClasses + depth*K+i ))-2;
20114                 if (size_next <= newT) continue;
20115                 ret = Util::explore2(argParts,Indices, dimClasses, nParts, K, newT, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth);
20116                 if (*ret > *curmax && *ret > newT){
20117                         *curmax = *ret;
20118                         *(curmax+1)=i;
20119                         for (int j=0; j<nParts-depth-1; j++) { *(curmax+2 + j) = *(ret+1+j);}
20120                 }
20121                 delete[] ret;
20122         }
20123 
20124         delete[] curintx2;
20125         return curmax;
20126 }
20127 
20128 
20129 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20130         //cout<<"initial_prune\n";
20131         // simple initial pruning. For class indClass of partition indPart:
20132         // 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
20133         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
20134 
20135         // 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
20136 
20137         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
20138         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
20139 
20140         int* dummy;
20141         int* cref;
20142         int cref_size;
20143         int* ccomp;
20144         int ccomp_size;
20145         int nintx;
20146         for (int i=0; i < nParts; i++){
20147                 for (int j =0; j < K; j++){
20148 
20149                         // consider class Parts[i][j]
20150                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
20151                         cref_size = (*(dimClasses+i*K+(*cref)))-2;
20152 
20153 
20154                         if (cref_size <= T){
20155 
20156                                 *cref = -1;
20157                                 continue;
20158                         }
20159                         bool done = 0;
20160                         for (int a = 0; a < nParts; a++){
20161                                 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
20162                                 bool hasActive=0;
20163                                 for (int b=0; b < Parts[a].size(); b++){
20164                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
20165                                         // remember first element of each class is the index of the class
20166                                         ccomp = Parts[a][b];
20167                                         ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20168                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20169 
20170 
20171                                         if (nintx <= T)
20172                                                 *(ccomp+1) = 0; // class Parts[a][b] is 'inactive' for cref
20173                                         else{
20174                                                 *(ccomp+1)=1; // class Parts[a][b] is 'active' for cref
20175                                                 hasActive=1;
20176                                         }
20177                                 }
20178                                 // see if partition a has at least one active class.if not then we're done with cref
20179                                 if (hasActive < 1){
20180                                    done=1;
20181                                    break;
20182                                 }
20183 
20184                         }
20185 
20186                         if (done > 0){
20187                                 // remove class j from partition i
20188 
20189                                 *cref = -1; // mark for deletion later
20190                                 continue; // move on to class Parts[i][j+1]
20191                         }
20192 
20193                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
20194                         // 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.
20195 
20196                         // (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.
20197                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
20198                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
20199 
20200                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
20201                         //bool found = 1;
20202                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20203 
20204                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
20205                                 // Parts[i].erase(Parts[i].begin()+j);
20206                                 *cref = -1;
20207                         }
20208                 }
20209 
20210                 // Erase from Parts[i] all the classes that's being designated for erasure
20211 
20212                 for (int d = K-1; d > -1; d--){
20213                         if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20214                 }
20215 
20216         }
20217 
20218         // Print out how many classes are left in each partition
20219         //for (int i =0; i < nParts; i++)
20220         //      cout << Parts[i].size()<<", ";
20221         //cout << "\n";
20222 }
20223 
20224 
20225 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) {
20226 
20227 
20228         if (size_next <= T) return 0;
20229 
20230         // take the intx of next and cur
20231         int* curintx2;
20232         int nintx = Util::k_means_cont_table_(curintx,next+2, curintx2, size_curintx, size_next,0);
20233         if (nintx <= T) return 0;
20234 
20235         int old_depth=depth;
20236         if (depth == partref) depth = depth + 1; // we skip classes in partref
20237         if (depth == (nParts)) { if (old_depth>0) return 1;}
20238 
20239         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20240 
20241         curintx2 = new int[nintx]; // put the intersection set in here
20242         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20243 
20244         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
20245 
20246         // we now consider each of the classes in partition (depth+1) in turn
20247         bool gt_thresh;
20248         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
20249 
20250         for (int i=0; i < num_classes; i++){
20251                 if (*(Parts[depth][i]+1) < 1) continue; // class is not active so move on
20252                 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20253                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20254                 if (gt_thresh) return 1;
20255         }
20256         delete[] curintx2;
20257         return 0;
20258 }
20259 
20260 
20261 int Util::generatesubmax(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int n_guesses){
20262         int guess=0;
20263 
20264         int* perm = new int[nParts];
20265         for(int i=0; i<nParts; i++) perm[i]=i;
20266 
20267         // some temporary variables
20268         int* intx;
20269         int* intx_next;
20270         int nintx;
20271         int nintxmax=0;
20272         int class_max, class_max_next;
20273         int intx_size, part, part_next;
20274         int ipold,indsw;
20275 
20276         for(int i=0; i< n_guesses; i++){
20277                 // shuffle perm array
20278                 for(int ip = 0; ip<nParts; ip++){
20279                         indsw = (rand())%nParts;
20280                         // swap ip(th) element with the (indsw)th element
20281                         ipold = perm[ip];
20282                         perm[ip]=perm[indsw];
20283                         perm[indsw]=ipold;
20284                 }
20285 
20286 
20287                 // find the two classes in partitions perm[0] and perm[1] that yield the largest intersection
20288                 part=*perm;
20289                 part_next=*(perm+1);
20290                 for (int a=0; a < K; a++)
20291                 {
20292                         if (*(argParts + Indices[part*K+a] + 1) < 1) continue;
20293                         for (int b=0; b < K; b++)
20294                         {
20295                                 if (*(argParts + Indices[part_next*K + b] + 1) < 1) continue;
20296                                 nintx = Util::k_means_cont_table_(argParts + Indices[part*K+a]+2,argParts + Indices[part_next*K + b]+2, intx, *(dimClasses + part*K+a)-2,  *(dimClasses + part_next*K + b)-2,0);
20297                                 if (nintx <= nintxmax) continue;
20298                                 nintxmax = nintx;
20299                                 class_max = a;
20300                                 class_max_next = b;
20301                         }
20302                 }
20303 
20304                 // no more....
20305                 if (nintxmax < 1) {continue;}
20306 
20307                 if (nParts > 2){
20308                         intx = new int[nintxmax];
20309                         intx_size = nintxmax;
20310                         Util::k_means_cont_table_(argParts + Indices[part*K+class_max]+2,argParts + Indices[part_next*K + class_max_next]+2, intx, *(dimClasses + part*K+class_max)-2, *(dimClasses+part_next*K+class_max_next)-2,1); // get intx
20311                 }
20312 
20313                 // for each subsequent partition perm[i], i>=2, find the partition that yields the largest weight with the current intx
20314                 for(int j = 2; j < nParts; j++){
20315                         part = *(perm+j);
20316                         nintxmax=0;
20317                         for(int a = 0; a < K; a++){
20318                                 if (*(argParts + Indices[part*K+a] + 1) < 1) continue; // skip inactive classes
20319                                 nintx =  Util::k_means_cont_table_(intx, argParts + Indices[part*K + a]+2, intx_next, intx_size,  (*(dimClasses + part*K+a))-2,0);
20320                                 if (nintx <= nintxmax) continue;
20321                                 nintxmax = nintx;
20322                                 class_max = a;
20323                         }
20324 
20325                         // no more stuff....
20326                         if (nintxmax < 1) {
20327 
20328                                 delete[] intx;
20329                                 break;
20330                         }
20331 
20332 
20333                         intx_next = new int[nintxmax];
20334                         Util::k_means_cont_table_(intx, argParts + Indices[part*K + class_max]+2, intx_next, intx_size,  *(dimClasses + part*K+class_max)-2,1);
20335                         delete[] intx;
20336                         intx = intx_next;
20337                         intx_size = nintxmax;
20338                         if (j==nParts - 1) delete[] intx_next;
20339 
20340                 }
20341 
20342                 if (nintxmax > guess) guess = nintxmax;
20343 
20344         }
20345         delete[] perm;
20346         return guess;
20347 }
20348 
20349 
20350 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int nTop, int n_guesses, bool doMPI, int* Levels) {
20351 
20352         // 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
20353         // 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
20354         // Make a vector of nParts vectors of K int* each
20355          int* Indices = new int[nParts*K];
20356          int ind_c = 0;
20357          for (int i=0; i < nParts; i++){
20358                  for(int j = 0; j < K; j++){
20359                          Indices[i*K + j] = ind_c;
20360                          ind_c = ind_c + *(dimClasses+i*K + j);
20361 
20362                  }
20363          }
20364 
20365         // return top weighted matches for mpi version
20366         if (nTop > 0 && doMPI > 0){
20367                  // find the nTop largest matches (not required to be mutually feasible)
20368                 int* matchlist = new int[nTop*nParts];
20369                 int* costlist=new int[nTop];
20370                 for (int i=0; i< nTop; i++) {*(costlist+i) = 0;}
20371                 int matchesFound = Util::findTopLargest(argParts,Indices, dimClasses, nParts, K,  T, matchlist, nTop,costlist,n_guesses);
20372                 vector<int> ret(nTop*(nParts+1) + 1);
20373                 ret[0] = matchesFound;
20374                 int m = nParts + 1;
20375                 // For each match in matchlist and its corresponding cost in costlist, put them in ret
20376                 for(int i=0; i < nTop; i++){
20377                         ret[1+i*m] = *(costlist+i);
20378                         for (int j=0; j < nParts; j++){
20379                                 ret[1+i*m + 1 + j] = matchlist[i*nParts + j];
20380                         }
20381                 }
20382 
20383                 return ret;
20384 
20385         }
20386 
20387         // do initial pruning on argParts and return the pruned partitions
20388 
20389         // Make a vector of nParts vectors of K int* each
20390         vector <vector <int*> > Parts(nParts,vector<int*>(K));
20391         ind_c = 0;
20392         int argParts_size=0;
20393         for (int i=0; i < nParts; i++){
20394                 for(int j = 0; j < K; j++){
20395                         Parts[i][j]=argParts + ind_c;
20396                         ind_c = ind_c + *(dimClasses+i*K + j);
20397                         argParts_size = argParts_size + *(dimClasses+i*K + j);
20398 
20399                 }
20400         }
20401 
20402         // in the following we call initial_prune with Parts which is a vector. This is not the most
20403         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
20404         // 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.....
20405 
20406         Util::initial_prune(Parts, dimClasses, nParts, K,T);
20407         for(int i = 0; i < nParts; i++){
20408                 for(int j=0; j < K; j++){
20409                         *(argParts + Indices[i*K + j]+1) = -1;
20410                 }
20411         }
20412 
20413         int num_classes;
20414         int old_index;
20415         for(int i=0; i<nParts; i++){
20416                 num_classes = Parts[i].size();// number of classes in partition i after pruning
20417                 for (int j=0; j < num_classes; j++){
20418                         old_index = *(Parts[i][j]);
20419                         //cout << "old_index: " << old_index<<"\n";
20420                         *(argParts + Indices[i*K + old_index]+1) = 1;
20421                 }
20422         }
20423 
20424 
20425         if (doMPI > 0){
20426                 // turn argParts into vector ret and return ret
20427                 vector<int> ret(argParts_size);
20428                 for(int i=0; i < argParts_size; i++)
20429                         ret[i]= (*(argParts+i));
20430 
20431                 return ret;
20432         }
20433 
20434         // if we're not doing mpi then keep going and call branchMPI and return the output
20435         //cout <<"begin partition matching\n";
20436         int* dummy;
20437         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,Levels, K,0,n_guesses,-1, dummy);
20438         //cout <<"done with partition matching \n";
20439         //cout<<"total cost: "<<*output<<"\n";
20440         //cout<<"number of matches: "<<*(output+1)<<"\n";
20441         // 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
20442         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20443 
20444         // something is wrong with output of branchMPI!
20445         if (correct < 1){
20446                 cout << "something is wrong with output of branchMPI!\n";
20447                 vector<int> ret(1);
20448                 ret[0]=-1;
20449                 return ret;
20450         }
20451 
20452         // output is not nonsense, so now put it into a single dimension vector and return
20453         // 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
20454         // and the rest is the list of matches. output is one dimensional
20455 
20456         int output_size = 2+ *(output+1) * nParts;
20457         vector<int> ret(output_size);
20458         for (int i = 0; i < output_size; i++){
20459                 ret[i]=*(output+i);
20460         }
20461         return ret;
20462 
20463 }
20464 
20465 vector<int> Util::branchMPIpy_(int* argParts, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int n_guesses, int nFirst, int* firstmatches){
20466         //cout<<"branchMPIpy_ called\n";
20467         // if nLevels == K, then we  compute nLevels - which is the number of active classes of the partition with the smallest number of active classes
20468         // this is not really necessary but would save a call to findTopLargest in branchMPI
20469         int num_active;
20470         int* Indices = new int[nParts*K];
20471         // Make a vector of nParts vectors of K int* each
20472         int ind_c = 0;
20473         for (int i=0; i < nParts; i++){
20474                 num_active = 0;
20475                 for(int j = 0; j < K; j++){
20476                         Indices[i*K + j] = ind_c; // offset from argParts of the first element of the jth class of the i-th partition
20477                         if (*(argParts+ind_c + 1) == 1) num_active = num_active + 1;
20478                         ind_c = ind_c + *(dimClasses+i*K + j);
20479                 }
20480 
20481                 if (num_active < nLevels) {nLevels = num_active;}
20482         }
20483 
20484 
20485         //add in code for dynamically changing levels
20486 
20487         //cout<<"num levels "<<nLevels<<"\n";
20488         //cout<<"calling branchMPI\n";
20489 
20490         int* output = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T,  Levels,  nLevels, 0,n_guesses, nFirst,firstmatches);
20491 
20492         // call sanity check on outupt to make sure the returned matches are feasible with cost over the threshold T
20493         //cout<<"total cost: "<<*output<<"\n";
20494         //cout<<"number of matches: "<<*(output+1)<<"\n";
20495         // 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
20496         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20497 
20498         // something is wrong with output of branchMPI!
20499         if (correct < 1){
20500                 cout << "something is wrong with output of branchMPI!\n";
20501                 vector<int> ret(1);
20502                 ret[0]=-1;
20503                 return ret;
20504         }
20505 
20506         // output is not nonsense, so now put it into a single dimension vector and return
20507         // 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
20508         // and the rest is the list of matches. output is one dimensional
20509 
20510         int output_size = 2+ *(output+1) * nParts;
20511         vector<int> ret(output_size);
20512         for (int i = 0; i < output_size; i++){
20513                 ret[i]=*(output+i);
20514         }
20515         return ret;
20516 }
20517 
20518 
20519 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses, int nFirst, int* firstmatches) {
20520 
20521         // Base Case: we're at a leaf, no more feasible matches possible
20522         if (curlevel > nLevels-1){
20523                 int* res = new int[2];
20524                 *res = 0;
20525                 *(res+1)=0;
20526                 return res;
20527         }
20528 
20529 
20530         // We may still find more feasible matchings with cost gt T, so explore level curlevel
20531         int nBranches = *(Levels + curlevel);
20532 
20533         // MPI: the first match is already chosen in MPI version, so we are going to branch only once at level 0
20534         if (curlevel==0 && nFirst > 0)
20535         {
20536                 nBranches = nFirst;
20537         }
20538 
20539         // call findTopLargest to get the nBranches feasible matchings with the largest weight (gt T) over all other feasible matches
20540 
20541         int* matchlist = new int[nBranches*nParts];
20542         int* costlist = new int[nBranches];// cost of each of the nBranches matches. If cost[i] < T then that means findTopLargest found less than i+1 matches
20543                                            // with cost > T
20544 
20545         for (int i=0; i < nBranches; i++)
20546                 *(costlist+i)=0;
20547 
20548         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
20549         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
20550 
20551         // MPI: first match is already chosen, so copy the match in first match over to matchlist, compute weight of match, and set costlist to the weight
20552         if (curlevel == 0 && nFirst > 0){
20553                 for(int i = 0; i < nBranches; i++){
20554                         *(costlist+i) = *(firstmatches +i*(nParts+1));
20555                         for (int j=0; j< nParts; j++)
20556                                 *(matchlist + i*nParts +j) = *(firstmatches +i*(nParts+1) + 1 + j);
20557                 }
20558         }
20559         else
20560                 Util::findTopLargest(argParts,Indices, dimClasses, nParts, K,  T, matchlist, nBranches,costlist,n_guesses);
20561 
20562         // if there are no feasible matches with cost gt T, then return 0
20563         if (costlist[0]<= T){
20564                 int* res = new int[2];
20565                 *res = 0;
20566                 *(res+1)=0;
20567                 return res;
20568         }
20569 
20570         int* maxreturn = new int[2];//initialize to placeholder
20571         *maxreturn=0;
20572         *(maxreturn+1)=0;
20573 
20574         // some temporary variables
20575         int old_index;
20576         int totalcost;
20577         int nmatches;
20578         //int offset;
20579 
20580         for(int i=0; i < nBranches ; i++){
20581 
20582                 // consider the i-th match returned by findTopLargest
20583 
20584                 if (costlist[i] <= T) break;
20585 
20586                 // 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.
20587                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
20588 
20589                 for(int j=0; j < nParts; j++){
20590                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20591                         old_index=matchlist[i*nParts + j];
20592                         *(argParts + Indices[j*K+old_index] + 1) = -2;
20593                 }
20594 
20595 
20596                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses, nFirst, firstmatches);
20597 
20598                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
20599                 totalcost = costlist[i] + *ret;
20600 
20601 
20602                  if (totalcost > *maxreturn) // option 1
20603                 {
20604                         nmatches = 1 + *(ret+1);
20605                         delete[] maxreturn; // get rid of the old maxreturn
20606                         maxreturn = new int[2+nmatches*nParts];
20607                         *maxreturn = totalcost;
20608                         *(maxreturn + 1)= nmatches;
20609                         int nret = 2+(nmatches-1)*nParts;
20610                         for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20611                         for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
20612                 }
20613 
20614 
20615                 delete[] ret;
20616 
20617                 // unmark the marked classes in preparation for the next iteration
20618 
20619                 for(int j=0; j < nParts; j++){
20620                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20621                         old_index=matchlist[i*nParts + j];
20622                         *(argParts + Indices[j*K+old_index] + 1) = 1;
20623                 }
20624 
20625         }
20626 
20627         delete[] matchlist;
20628         delete[] costlist;
20629 
20630         return maxreturn;
20631 
20632 }

Generated on Fri Apr 30 15:38:58 2010 for EMAN2 by  doxygen 1.3.9.1