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 #ifdef _WIN32
00036 #pragma warning(disable:4819)
00037 #include <malloc.h>
00038 #endif  //_WIN32
00039 
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <boost/shared_ptr.hpp>
00044 #include <cstdio>
00045 #include <cstdlib>
00046 #include <cassert>
00047 #include "emdata.h"
00048 #include "util.h"
00049 #include "fundamentals.h"
00050 #include "lapackblas.h"
00051 #include "lbfgsb.h"
00052 using namespace EMAN;
00053 #include "steepest.h"
00054 #include "emassert.h"
00055 #include "randnum.h"
00056 
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <gsl/gsl_sf_bessel.h>
00059 #include <cmath>
00060 //#include <omp.h>
00061 using namespace std;
00062 using std::complex;
00063 
00064 /* Subroutine */ 
00065 int circum_(double *, double *, double *, double *, int *);
00066 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00067 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00068 
00069 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00070 //  flip true:  find statistics under the mask (mask >0.5)
00071 //  flip false: find statistics ourside the mask (mask <0.5)
00072 {
00073         ENTERFUNC;
00074         vector<float> stats;
00075         float *Volptr, *maskptr,MAX,MIN;
00076         long double Sum1,Sum2;
00077         long count;
00078 
00079         MAX = -FLT_MAX;
00080         MIN =  FLT_MAX;
00081         count = 0L;
00082         Sum1  = 0.0L;
00083         Sum2  = 0.0L;
00084 
00085         if (mask == NULL) {
00086                 //Vol->update_stat();
00087                 stats.push_back(Vol->get_attr("mean"));
00088                 stats.push_back(Vol->get_attr("sigma"));
00089                 stats.push_back(Vol->get_attr("minimum"));
00090                 stats.push_back(Vol->get_attr("maximum"));
00091                 return stats;
00092         }
00093 
00094         /* Check if the sizes of the mask and image are same */
00095 
00096         size_t nx = Vol->get_xsize();
00097         size_t ny = Vol->get_ysize();
00098         size_t nz = Vol->get_zsize();
00099 
00100         size_t mask_nx = mask->get_xsize();
00101         size_t mask_ny = mask->get_ysize();
00102         size_t mask_nz = mask->get_zsize();
00103 
00104         if  (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00105                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00106 
00107  /*       if (nx != mask_nx ||
00108             ny != mask_ny ||
00109             nz != mask_nz  ) {
00110            // should throw an exception here!!! (will clean it up later CY)
00111            fprintf(stderr, "The dimension of the image does not match the dimension of the mask!\n");
00112            fprintf(stderr, " nx = %d, mask_nx = %d\n", nx, mask_nx);
00113            fprintf(stderr, " ny = %d, mask_ny = %d\n", ny, mask_ny);
00114            fprintf(stderr, " nz = %d, mask_nz = %d\n", nz, mask_nz);
00115            exit(1);
00116         }
00117  */
00118         Volptr = Vol->get_data();
00119         maskptr = mask->get_data();
00120 
00121         for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00122                 if ((maskptr[i]>0.5f) == flip) {
00123                         Sum1 += Volptr[i];
00124                         Sum2 += Volptr[i]*double(Volptr[i]);
00125                         MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00126                         MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00127                         count++;
00128                 }
00129         }
00130 
00131         if (count == 0) {
00132                 LOGERR("Invalid mask");
00133                 throw ImageFormatException( "Invalid mask");
00134         }
00135 
00136         float avg = static_cast<float>(Sum1/count);
00137         float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00138 
00139         stats.push_back(avg);
00140         stats.push_back(sig);
00141         stats.push_back(MIN);
00142         stats.push_back(MAX);
00143 
00144         return stats;
00145 }
00146 
00147 
00148 //----------------------------------------------------------------------------------------------------------
00149 
00150 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00151 {
00152         ENTERFUNC;
00153 
00154         if (!EMUtil::is_same_size(V1, V2)) {
00155                 LOGERR("images not same size");
00156                 throw ImageFormatException( "images not same size");
00157         }
00158 
00159         size_t nx = V1->get_xsize();
00160         size_t ny = V1->get_ysize();
00161         size_t nz = V1->get_zsize();
00162         size_t size = (size_t)nx*ny*nz;
00163 
00164         EMData *BD = new EMData();
00165         BD->set_size(nx, ny, nz);
00166 
00167         float *params = new float[2];
00168 
00169         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00170         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00171         int nvox = 0L;
00172 
00173         V1ptr = V1->get_data();
00174         V2ptr = V2->get_data();
00175         BDptr = BD->get_data();
00176 
00177 
00178         if(!mask){
00179                 EMData * Mask = new EMData();
00180                 Mask->set_size(nx,ny,nz);
00181                 Mask->to_one();
00182                 MASKptr = Mask->get_data();
00183         } else {
00184                 if (!EMUtil::is_same_size(V1, mask)) {
00185                         LOGERR("mask not same size");
00186                         throw ImageFormatException( "mask not same size");
00187                 }
00188 
00189                 MASKptr = mask->get_data();
00190         }
00191 
00192 
00193 
00194 //       calculation of S1,S2,S3,S3,nvox
00195 
00196         for (size_t i = 0L;i < size; i++) {
00197               if (MASKptr[i]>0.5f) {
00198                S1 += V1ptr[i]*V2ptr[i];
00199                S2 += V1ptr[i]*V1ptr[i];
00200                S3 += V2ptr[i];
00201                S4 += V1ptr[i];
00202                nvox ++;
00203               }
00204         }
00205 
00206         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00207                 A =1.0f ;
00208         } else {
00209                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00210         }
00211         B = static_cast<float> (A*S4  -  S3)/nvox;
00212 
00213         // calculation of the difference image
00214 
00215         for (size_t i = 0L;i < size; i++) {
00216              if (MASKptr[i]>0.5f) {
00217                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00218              }  else  {
00219                BDptr[i] = 0.f;
00220              }
00221         }
00222 
00223         BD->update();
00224 
00225         params[0] = A;
00226         params[1] = B;
00227 
00228         Dict BDnParams;
00229         BDnParams["imdiff"] = BD;
00230         BDnParams["A"] = params[0];
00231         BDnParams["B"] = params[1];
00232 
00233         EXITFUNC;
00234         return BDnParams;
00235  }
00236 
00237 //----------------------------------------------------------------------------------------------------------
00238 
00239 
00240 
00241 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00242 {
00243         ENTERFUNC;
00244         int Mid= (Size+1)/2;
00245 
00246         if (flag==0) { // This is the real function
00247                 EMData* ImBW = new EMData();
00248                 ImBW->set_size(Size,Size,1);
00249                 ImBW->to_zero();
00250 
00251                 float tempIm;
00252                 float x,y;
00253 
00254                 for (int ix=(1-Mid);  ix<Mid; ix++){
00255                         for (int iy=(1-Mid);  iy<Mid; iy++){
00256                                 x = (float)ix;
00257                                 y = (float)iy;
00258                         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)) );
00259                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00260                         }
00261                 }
00262                 ImBW->update();
00263                 ImBW->set_complex(false);
00264                 ImBW->set_ri(true);
00265 
00266                 return ImBW;
00267         }
00268         else if (flag==1) {  // This is the Fourier Transform
00269                 EMData* ImBWFFT = new EMData();
00270                 ImBWFFT ->set_size(2*Size,Size,1);
00271                 ImBWFFT ->to_zero();
00272 
00273                 float r,s;
00274 
00275                 for (int ir=(1-Mid);  ir<Mid; ir++){
00276                         for (int is=(1-Mid);  is<Mid; is++){
00277                                 r = (float)ir;
00278                                 s = (float)is;
00279                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00280                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00281                         }
00282                 }
00283                 ImBWFFT->update();
00284                 ImBWFFT->set_complex(true);
00285                 ImBWFFT->set_ri(true);
00286                 ImBWFFT->set_shuffled(true);
00287                 ImBWFFT->set_fftodd(true);
00288 
00289                 return ImBWFFT;
00290         }
00291         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00292                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00293                 float C=cos(alpha);
00294                 float S=sin(alpha);
00295                 float D= sqrt(S*S*b*b + C*C*a*a);
00296                 //float D2 = D*D;   PAP - to get rid of warning
00297 
00298                 float P = p * C *a*a/D ;
00299                 float Q = q * S *b*b/D ;
00300 
00301                 if (flag==2) {
00302                         EMData* pofalpha = new EMData();
00303                         pofalpha ->set_size(Size,1,1);
00304                         pofalpha ->to_zero();
00305 
00306                         float Norm0 =  D*(float)sqrt(2*pi);
00307                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00308                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00309                         float sD;
00310 
00311                         for (int is=(1-Mid);  is<Mid; is++){
00312                                 sD = is/D ;
00313                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00314                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00315                         }
00316                         pofalpha-> update();
00317                         pofalpha-> set_complex(false);
00318                         pofalpha-> set_ri(true);
00319 
00320                         return pofalpha;
00321                 }
00322                 if (flag==3) { // This is the projection in Fourier Space
00323                         float vD;
00324 
00325                         EMData* pofalphak = new EMData();
00326                         pofalphak ->set_size(2*Size,1,1);
00327                         pofalphak ->to_zero();
00328 
00329                         for (int iv=(1-Mid);  iv<Mid; iv++){
00330                                 vD = iv*D ;
00331                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00332                         }
00333                         pofalphak-> update();
00334                         pofalphak-> set_complex(false);
00335                         pofalphak-> set_ri(true);
00336 
00337                         return pofalphak;
00338                 }
00339         }
00340         else if (flag==4) {
00341                 cout <<" FH under construction";
00342                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00343                 EMData* TryFH= OutFT -> real2FH(4.0);
00344                 return TryFH;
00345         } else {
00346                 cout <<" flag must be 0,1,2,3, or 4";
00347         }
00348 
00349         EXITFUNC;
00350         return 0;
00351 }
00352 
00353 
00354 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00355 {
00356 
00357         float x0= x[0];
00358         float x1= x[1];
00359         float x2= x[2];
00360         float y0= y[0];
00361         float y1= y[1];
00362         float y2= y[2];
00363         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00364         float xn  = x[n];
00365         float xnm1= x[n-1];
00366         float xnm2= x[n-2];
00367         float yn  = y[n];
00368         float ynm1= y[n-1];
00369         float ynm2= y[n-2];
00370         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00371         float *y2d = new float[n];
00372         Util::spline(x,y,n,yp1,ypn,y2d);
00373         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00374         delete [] y2d;
00375         return;
00376 }
00377 
00378 
00379 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00380 {
00381         int i,k;
00382         float p, qn, sig, un, *u;
00383         u = new float[n-1];
00384 
00385         if (yp1 > .99e30){
00386                 y2[0]=u[0]=0.0;
00387         } else {
00388                 y2[0]=-.5f;
00389                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00390         }
00391 
00392         for (i=1; i < n-1; i++) {
00393                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00394                 p = sig*y2[i-1] + 2.0f;
00395                 y2[i]  = (sig-1.0f)/p;
00396                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00397                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00398         }
00399 
00400         if (ypn>.99e30){
00401                 qn=0; un=0;
00402         } else {
00403                 qn= .5f;
00404                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00405         }
00406         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00407         for (k=n-2; k>=0; k--){
00408                 y2[k]=y2[k]*y2[k+1]+u[k];
00409         }
00410         delete [] u;
00411 }
00412 
00413 
00414 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00415 {
00416         int klo, khi, k;
00417         float h, b, a;
00418 
00419 //      klo=0; // can try to put here
00420         for (int j=0; j<m;j++){
00421                 klo=0;
00422                 khi=n-1;
00423                 while (khi-klo >1) {
00424                         k=(khi+klo) >>1;
00425                         if  (xa[k]>xq[j]){ khi=k;}
00426                         else { klo=k;}
00427                 }
00428                 h=xa[khi]- xa[klo];
00429                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00430                 a =(xa[khi]-xq[j])/h;
00431                 b=(xq[j]-xa[klo])/h;
00432                 yq[j]=a*ya[klo] + b*ya[khi]
00433                         + ((a*a*a-a)*y2a[klo]
00434                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00435         }
00436 //      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]);
00437 }
00438 
00439 
00440 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00441                float *weightofkValsSorted, int Size, int *SizeReturned)
00442 {
00443         int iMax = (int) floor( (Size-1.0)/2 +.01);
00444         int CountMax = (iMax+2)*(iMax+1)/2;
00445         int Count=-1;
00446         float *kVals     = new float[CountMax];
00447         float *weightMat = new float[CountMax];
00448         int *PermMat     = new   int[CountMax];
00449         SizeReturned[0] = CountMax;
00450 
00451 //      printf("Aa \n");        fflush(stdout);
00452         for (int jkx=0; jkx< iMax+1; jkx++) {
00453                 for (int jky=0; jky< jkx+1; jky++) {
00454                         Count++;
00455                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00456                         weightMat[Count]=  1.0;
00457                         if (jkx!=0)  { weightMat[Count] *=2;}
00458                         if (jky!=0)  { weightMat[Count] *=2;}
00459                         if (jkx!=jky){ weightMat[Count] *=2;}
00460                         PermMat[Count]=Count+1;
00461                 }
00462         }
00463 
00464         int lkVals = Count+1;
00465 //      printf("Cc \n");fflush(stdout);
00466 
00467         sort_mat(&kVals[0],&kVals[Count],
00468              &PermMat[0],  &PermMat[Count]);  //PermMat is
00469                                 //also returned as well as kValsSorted
00470         fflush(stdout);
00471 
00472         int newInd;
00473 
00474         for (int iP=0; iP < lkVals ; iP++ ) {
00475                 newInd =  PermMat[iP];
00476                 PermMatTr[newInd-1] = iP+1;
00477         }
00478 
00479 //      printf("Ee \n"); fflush(stdout);
00480 
00481         int CountA=-1;
00482         int CountB=-1;
00483 
00484         while (CountB< (CountMax-1)) {
00485                 CountA++;
00486                 CountB++;
00487 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00488                 kValsSorted[CountA] = kVals[CountB] ;
00489                 if (CountB<(CountMax-1) ) {
00490                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00491                                 SizeReturned[0]--;
00492                                 for (int iP=0; iP < lkVals; iP++){
00493 //                                      printf("iP=%d \n", iP);fflush(stdout);
00494                                         if  (PermMatTr[iP]>CountA+1) {
00495                                                 PermMatTr[iP]--;
00496                                         }
00497                                 }
00498                                 CountB++;
00499                         }
00500                 }
00501         }
00502 
00503 
00504         for (int CountD=0; CountD < CountMax; CountD++) {
00505             newInd = PermMatTr[CountD];
00506             weightofkValsSorted[newInd-1] += weightMat[CountD];
00507         }
00508 
00509 }
00510 
00511 
00512 vector<float>
00513 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00514 {
00515         vector<float> angles;
00516         float psi = 0.0;
00517         if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00518                 t1 =  0.0f;
00519                 t2 = 90.0f;
00520         }
00521         if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00522                 p1 =   0.0f;
00523                 p2 = 359.9f;
00524         }
00525         bool skip = ((t1 < 90.0) && (90.0 == t2) && (0.0 == p1) && (p2 > 180.0));
00526         for (float theta = t1; theta <= t2; theta += delta) {
00527                 float detphi;
00528                 int lt;
00529                 if ((0.0 == theta)||(180.0 == theta)) {
00530                         detphi = 360.0f;
00531                         lt = 1;
00532                 } else {
00533                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00534                         lt = int((p2 - p1)/detphi)-1;
00535                         if (lt < 1) lt = 1;
00536                         detphi = (p2 - p1)/lt;
00537                 }
00538                 for (int i = 0; i < lt; i++) {
00539                         float phi = p1 + i*detphi;
00540                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00541                         angles.push_back(phi);
00542                         angles.push_back(theta);
00543                         angles.push_back(psi);
00544                 }
00545         }
00546         return angles;
00547 }
00548 
00549 
00550 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00551 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00552 {
00553 
00554 //  purpose: quadratic interpolation
00555 //
00556 //  parameters:       xx,yy treated as circularly closed.
00557 //                    fdata - image 1..nxdata, 1..nydata
00558 //
00559 //                    f3    fc       f0, f1, f2, f3 are the values
00560 //                     +             at the grid points.  x is the
00561 //                     + x           point at which the function
00562 //              f2++++f0++++f1       is to be estimated. (it need
00563 //                     +             not be in the first quadrant).
00564 //                     +             fc - the outer corner point
00565 //                    f4             nearest x.
00566 c
00567 //                                   f0 is the value of the fdata at
00568 //                                   fdata(i,j), it is the interior mesh
00569 //                                   point nearest  x.
00570 //                                   the coordinates of f0 are (x0,y0),
00571 //                                   the coordinates of f1 are (xb,y0),
00572 //                                   the coordinates of f2 are (xa,y0),
00573 //                                   the coordinates of f3 are (x0,yb),
00574 //                                   the coordinates of f4 are (x0,ya),
00575 //                                   the coordinates of fc are (xc,yc),
00576 c
00577 //                   o               hxa, hxb are the mesh spacings
00578 //                   +               in the x-direction to the left
00579 //                  hyb              and right of the center point.
00580 //                   +
00581 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00582 //                   +               in the y-direction.
00583 //                  hya
00584 //                   +               hxc equals either  hxb  or  hxa
00585 //                   o               depending on where the corner
00586 //                                   point is located.
00587 c
00588 //                                   construct the interpolant
00589 //                                   f = f0 + c1*(x-x0) +
00590 //                                       c2*(x-x0)*(x-x1) +
00591 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00592 //                                       + c5*(x-x0)*(y-y0)
00593 //
00594 //
00595 
00596     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00597     float quadri;
00598     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00599 
00600     x = xx;
00601     y = yy;
00602 
00603     // circular closure
00604         while ( x < 1.0 ) x += nxdata;
00605         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00606         while ( y < 1.0 ) y += nydata;
00607         while ( y >= (float)(nydata+1) )  y -= nydata;
00608 
00609 
00610     i   = (int) x;
00611     j   = (int) y;
00612 
00613     dx0 = x - i;
00614     dy0 = y - j;
00615 
00616     ip1 = i + 1;
00617     im1 = i - 1;
00618     jp1 = j + 1;
00619     jm1 = j - 1;
00620 
00621     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00622     if (im1 < 1)      im1 = im1 + nxdata;
00623     if (jp1 > nydata) jp1 = jp1 - nydata;
00624     if (jm1 < 1)      jm1 = jm1 + nydata;
00625 
00626     f0  = fdata(i,j);
00627     c1  = fdata(ip1,j) - f0;
00628     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00629     c3  = fdata(i,jp1) - f0;
00630     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00631 
00632     dxb = dx0 - 1;
00633     dyb = dy0 - 1;
00634 
00635     // hxc & hyc are either 1 or -1
00636     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00637     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00638 
00639     ic  = i + hxc;
00640     jc  = j + hyc;
00641 
00642     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00643     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00644 
00645     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00646             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00647 
00648     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00649 
00650     return quadri;
00651 }*/
00652 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00653 {
00654 //  purpose: quadratic interpolation
00655 //  Optimized for speed, circular closer removed, checking of ranges removed
00656         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00657         float  quadri;
00658         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00659 
00660         x = xx;
00661         y = yy;
00662 
00663         //     any xx and yy
00664         while ( x < 1.0 )                 x += nxdata;
00665         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00666         while ( y < 1.0 )                 y += nydata;
00667         while ( y >= (float)(nydata+1) )  y -= nydata;
00668 
00669         i   = (int) x;
00670         j   = (int) y;
00671 
00672         dx0 = x - i;
00673         dy0 = y - j;
00674 
00675         ip1 = i + 1;
00676         im1 = i - 1;
00677         jp1 = j + 1;
00678         jm1 = j - 1;
00679 
00680         if (ip1 > nxdata) ip1 -= nxdata;
00681         if (im1 < 1)      im1 += nxdata;
00682         if (jp1 > nydata) jp1 -= nydata;
00683         if (jm1 < 1)      jm1 += nydata;
00684 
00685         f0  = fdata(i,j);
00686         c1  = fdata(ip1,j) - f0;
00687         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00688         c3  = fdata(i,jp1) - f0;
00689         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00690 
00691         dxb = dx0 - 1;
00692         dyb = dy0 - 1;
00693 
00694         // hxc & hyc are either 1 or -1
00695         if (dx0 >= 0) hxc = 1; else hxc = -1;
00696         if (dy0 >= 0) hyc = 1; else hyc = -1;
00697 
00698         ic  = i + hxc;
00699         jc  = j + hyc;
00700 
00701         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00702         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00703 
00704         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00705                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00706 
00707 
00708         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00709 
00710         return quadri;
00711 }
00712 
00713 #undef fdata
00714 
00715 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00716 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00717 {
00718 //  purpose: quadratic interpolation
00719 //  Optimized for speed, circular closer removed, checking of ranges removed
00720         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00721         float  quadri;
00722         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00723 
00724         x = xx;
00725         y = yy;
00726 
00727         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00728         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00729               x = (float)xnew;
00730                   y = (float)ynew;
00731      }
00732 
00733 
00734         i   = (int) x;
00735         j   = (int) y;
00736 
00737         dx0 = x - i;
00738         dy0 = y - j;
00739 
00740         ip1 = i + 1;
00741         im1 = i - 1;
00742         jp1 = j + 1;
00743         jm1 = j - 1;
00744 
00745         if (ip1 > nxdata) ip1 -= nxdata;
00746         if (im1 < 1)      im1 += nxdata;
00747         if (jp1 > nydata) jp1 -= nydata;
00748         if (jm1 < 1)      jm1 += nydata;
00749 
00750         f0  = fdata(i,j);
00751         c1  = fdata(ip1,j) - f0;
00752         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00753         c3  = fdata(i,jp1) - f0;
00754         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00755 
00756         dxb = dx0 - 1;
00757         dyb = dy0 - 1;
00758 
00759         // hxc & hyc are either 1 or -1
00760         if (dx0 >= 0) hxc = 1; else hxc = -1;
00761         if (dy0 >= 0) hyc = 1; else hyc = -1;
00762 
00763         ic  = i + hxc;
00764         jc  = j + hyc;
00765 
00766         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00767         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00768 
00769         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00770                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00771 
00772 
00773         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00774 
00775         return quadri;
00776 }
00777 
00778 #undef fdata
00779 
00780 
00781 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00782         int K = kb.get_window_size();
00783         int kbmin = -K/2;
00784         int kbmax = -kbmin;
00785         int kbc = kbmax+1;
00786 
00787         float pixel =0.0f;
00788         float w=0.0f;
00789 
00790         delx = restrict1(delx, nx);
00791         int inxold = int(round(delx));
00792         if ( ny < 2 ) {  //1D
00793                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00794                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00795                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00796                 float tablex4 = kb.i0win_tab(delx-inxold);
00797                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00798                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00799                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00800 
00801                 int x1, x2, x3, x4, x5, x6, x7;
00802 
00803                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00804                         x1 = (inxold-3+nx)%nx;
00805                         x2 = (inxold-2+nx)%nx;
00806                         x3 = (inxold-1+nx)%nx;
00807                         x4 = (inxold  +nx)%nx;
00808                         x5 = (inxold+1+nx)%nx;
00809                         x6 = (inxold+2+nx)%nx;
00810                         x7 = (inxold+3+nx)%nx;
00811                 } else {
00812                         x1 = inxold-3;
00813                         x2 = inxold-2;
00814                         x3 = inxold-1;
00815                         x4 = inxold;
00816                         x5 = inxold+1;
00817                         x6 = inxold+2;
00818                         x7 = inxold+3;
00819                 }
00820 
00821                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00822                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00823                         data[x7]*tablex7 ;
00824 
00825                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00826         } else if ( nz < 2 ) {  // 2D
00827                 dely = restrict1(dely, ny);
00828                 int inyold = int(round(dely));
00829                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00830                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00831                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00832                 float tablex4 = kb.i0win_tab(delx-inxold);
00833                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00834                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00835                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00836 
00837                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00838                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00839                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00840                 float tabley4 = kb.i0win_tab(dely-inyold);
00841                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00842                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00843                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00844 
00845                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00846 
00847                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
00848                         x1 = (inxold-3+nx)%nx;
00849                         x2 = (inxold-2+nx)%nx;
00850                         x3 = (inxold-1+nx)%nx;
00851                         x4 = (inxold  +nx)%nx;
00852                         x5 = (inxold+1+nx)%nx;
00853                         x6 = (inxold+2+nx)%nx;
00854                         x7 = (inxold+3+nx)%nx;
00855 
00856                         y1 = ((inyold-3+ny)%ny)*nx;
00857                         y2 = ((inyold-2+ny)%ny)*nx;
00858                         y3 = ((inyold-1+ny)%ny)*nx;
00859                         y4 = ((inyold  +ny)%ny)*nx;
00860                         y5 = ((inyold+1+ny)%ny)*nx;
00861                         y6 = ((inyold+2+ny)%ny)*nx;
00862                         y7 = ((inyold+3+ny)%ny)*nx;
00863                 } else {
00864                         x1 = inxold-3;
00865                         x2 = inxold-2;
00866                         x3 = inxold-1;
00867                         x4 = inxold;
00868                         x5 = inxold+1;
00869                         x6 = inxold+2;
00870                         x7 = inxold+3;
00871 
00872                         y1 = (inyold-3)*nx;
00873                         y2 = (inyold-2)*nx;
00874                         y3 = (inyold-1)*nx;
00875                         y4 = inyold*nx;
00876                         y5 = (inyold+1)*nx;
00877                         y6 = (inyold+2)*nx;
00878                         y7 = (inyold+3)*nx;
00879                 }
00880 
00881                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00882                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00883                              data[x7+y1]*tablex7 ) * tabley1 +
00884                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00885                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00886                              data[x7+y2]*tablex7 ) * tabley2 +
00887                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00888                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00889                              data[x7+y3]*tablex7 ) * tabley3 +
00890                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00891                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00892                              data[x7+y4]*tablex7 ) * tabley4 +
00893                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00894                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00895                              data[x7+y5]*tablex7 ) * tabley5 +
00896                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00897                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00898                              data[x7+y6]*tablex7 ) * tabley6 +
00899                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00900                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00901                              data[x7+y7]*tablex7 ) * tabley7;
00902 
00903                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00904                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00905         } else {  //  3D
00906                 dely = restrict1(dely, ny);
00907                 int inyold = int(Util::round(dely));
00908                 delz = restrict1(delz, nz);
00909                 int inzold = int(Util::round(delz));
00910 
00911                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00912                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00913                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00914                 float tablex4 = kb.i0win_tab(delx-inxold);
00915                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00916                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00917                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00918 
00919                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00920                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00921                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00922                 float tabley4 = kb.i0win_tab(dely-inyold);
00923                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00924                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00925                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00926 
00927                 float tablez1 = kb.i0win_tab(delz-inzold+3);
00928                 float tablez2 = kb.i0win_tab(delz-inzold+2);
00929                 float tablez3 = kb.i0win_tab(delz-inzold+1);
00930                 float tablez4 = kb.i0win_tab(delz-inzold);
00931                 float tablez5 = kb.i0win_tab(delz-inzold-1);
00932                 float tablez6 = kb.i0win_tab(delz-inzold-2);
00933                 float tablez7 = kb.i0win_tab(delz-inzold-3);
00934 
00935                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00936 
00937                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
00938                         x1 = (inxold-3+nx)%nx;
00939                         x2 = (inxold-2+nx)%nx;
00940                         x3 = (inxold-1+nx)%nx;
00941                         x4 = (inxold  +nx)%nx;
00942                         x5 = (inxold+1+nx)%nx;
00943                         x6 = (inxold+2+nx)%nx;
00944                         x7 = (inxold+3+nx)%nx;
00945 
00946                         y1 = ((inyold-3+ny)%ny)*nx;
00947                         y2 = ((inyold-2+ny)%ny)*nx;
00948                         y3 = ((inyold-1+ny)%ny)*nx;
00949                         y4 = ((inyold  +ny)%ny)*nx;
00950                         y5 = ((inyold+1+ny)%ny)*nx;
00951                         y6 = ((inyold+2+ny)%ny)*nx;
00952                         y7 = ((inyold+3+ny)%ny)*nx;
00953 
00954                         z1 = ((inzold-3+nz)%nz)*nx*ny;
00955                         z2 = ((inzold-2+nz)%nz)*nx*ny;
00956                         z3 = ((inzold-1+nz)%nz)*nx*ny;
00957                         z4 = ((inzold  +nz)%nz)*nx*ny;
00958                         z5 = ((inzold+1+nz)%nz)*nx*ny;
00959                         z6 = ((inzold+2+nz)%nz)*nx*ny;
00960                         z7 = ((inzold+3+nz)%nz)*nx*ny;
00961                 } else {
00962                         x1 = inxold-3;
00963                         x2 = inxold-2;
00964                         x3 = inxold-1;
00965                         x4 = inxold;
00966                         x5 = inxold+1;
00967                         x6 = inxold+2;
00968                         x7 = inxold+3;
00969 
00970                         y1 = (inyold-3)*nx;
00971                         y2 = (inyold-2)*nx;
00972                         y3 = (inyold-1)*nx;
00973                         y4 = inyold*nx;
00974                         y5 = (inyold+1)*nx;
00975                         y6 = (inyold+2)*nx;
00976                         y7 = (inyold+3)*nx;
00977 
00978                         z1 = (inzold-3)*nx*ny;
00979                         z2 = (inzold-2)*nx*ny;
00980                         z3 = (inzold-1)*nx*ny;
00981                         z4 = inzold*nx*ny;
00982                         z5 = (inzold+1)*nx*ny;
00983                         z6 = (inzold+2)*nx*ny;
00984                         z7 = (inzold+3)*nx*ny;
00985                 }
00986 
00987                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00988                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00989                              data[x7+y1+z1]*tablex7 ) * tabley1 +
00990                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00991                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00992                              data[x7+y2+z1]*tablex7 ) * tabley2 +
00993                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00994                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00995                              data[x7+y3+z1]*tablex7 ) * tabley3 +
00996                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00997                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00998                              data[x7+y4+z1]*tablex7 ) * tabley4 +
00999                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01000                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01001                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01002                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01003                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01004                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01005                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01006                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01007                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01008                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01009                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01010                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01011                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01012                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01013                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01014                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01015                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01016                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01017                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01018                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01019                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01020                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01021                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01022                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01023                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01024                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01025                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01026                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01027                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01028                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01029                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01030                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01031                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01032                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01033                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01034                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01035                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01036                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01037                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01038                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01039                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01040                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01041                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01042                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01043                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01044                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01045                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01046                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01047                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01048                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01049                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01050                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01051                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01052                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01053                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01054                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01055                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01056                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01057                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01058                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01059                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01060                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01061                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01062                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01063                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01064                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01065                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01066                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01067                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01068                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01069                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01070                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01071                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01072                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01073                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01074                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01075                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01076                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01077                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01078                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01079                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01080                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01081                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01082                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01083                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01084                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01085                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01086                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01087                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01088                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01089                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01090                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01091                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01092                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01093                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01094                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01095                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01096                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01097                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01098                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01099                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01100                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01101                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01102                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01103                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01104                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01105                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01106                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01107                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01108                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01109                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01110                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01111                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01112                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01113                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01114                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01115                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01116                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01117                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01118                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01119                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01120                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01121                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01122                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01123                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01124                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01125                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01126                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01127                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01128                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01129                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01130                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01131                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01132                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01133                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01134 
01135                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01136                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01137                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01138         }
01139         return pixel/w;
01140 }
01141 
01142 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) {
01143         int K = kb.get_window_size();
01144         int kbmin = -K/2;
01145         int kbmax = -kbmin;
01146         int kbc = kbmax+1;
01147 
01148         float pixel =0.0f;
01149         float w=0.0f;
01150 
01151     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01152         delx = restrict1(delx, nx);
01153         int inxold = int(round(delx));
01154         if ( ny < 2 ) {  //1D
01155                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01156                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01157                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01158                 float tablex4 = kb.i0win_tab(delx-inxold);
01159                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01160                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01161                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01162 
01163                 int x1, x2, x3, x4, x5, x6, x7;
01164 
01165                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01166                         x1 = (inxold-3+nx)%nx;
01167                         x2 = (inxold-2+nx)%nx;
01168                         x3 = (inxold-1+nx)%nx;
01169                         x4 = (inxold  +nx)%nx;
01170                         x5 = (inxold+1+nx)%nx;
01171                         x6 = (inxold+2+nx)%nx;
01172                         x7 = (inxold+3+nx)%nx;
01173                 } else {
01174                         x1 = inxold-3;
01175                         x2 = inxold-2;
01176                         x3 = inxold-1;
01177                         x4 = inxold;
01178                         x5 = inxold+1;
01179                         x6 = inxold+2;
01180                         x7 = inxold+3;
01181                 }
01182 
01183                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01184                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01185                         data[x7]*tablex7 ;
01186 
01187                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01188         } else if ( nz < 2 ) {  // 2D
01189 
01190                 delx = argdelx;
01191                 // 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
01192                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01193                 delx = (float)xnew*2.0f;
01194                 dely = (float)ynew*2.0f;
01195                 }
01196 
01197                 int inxold = int(round(delx));
01198                 int inyold = int(round(dely));
01199 
01200                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01201                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01202                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01203                 float tablex4 = kb.i0win_tab(delx-inxold);
01204                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01205                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01206                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01207 
01208                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01209                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01210                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01211                 float tabley4 = kb.i0win_tab(dely-inyold);
01212                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01213                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01214                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01215 
01216                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01217 
01218                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01219                         x1 = (inxold-3+nx)%nx;
01220                         x2 = (inxold-2+nx)%nx;
01221                         x3 = (inxold-1+nx)%nx;
01222                         x4 = (inxold  +nx)%nx;
01223                         x5 = (inxold+1+nx)%nx;
01224                         x6 = (inxold+2+nx)%nx;
01225                         x7 = (inxold+3+nx)%nx;
01226 
01227                         y1 = ((inyold-3+ny)%ny)*nx;
01228                         y2 = ((inyold-2+ny)%ny)*nx;
01229                         y3 = ((inyold-1+ny)%ny)*nx;
01230                         y4 = ((inyold  +ny)%ny)*nx;
01231                         y5 = ((inyold+1+ny)%ny)*nx;
01232                         y6 = ((inyold+2+ny)%ny)*nx;
01233                         y7 = ((inyold+3+ny)%ny)*nx;
01234                 } else {
01235                         x1 = inxold-3;
01236                         x2 = inxold-2;
01237                         x3 = inxold-1;
01238                         x4 = inxold;
01239                         x5 = inxold+1;
01240                         x6 = inxold+2;
01241                         x7 = inxold+3;
01242 
01243                         y1 = (inyold-3)*nx;
01244                         y2 = (inyold-2)*nx;
01245                         y3 = (inyold-1)*nx;
01246                         y4 = inyold*nx;
01247                         y5 = (inyold+1)*nx;
01248                         y6 = (inyold+2)*nx;
01249                         y7 = (inyold+3)*nx;
01250                 }
01251 
01252                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01253                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01254                              data[x7+y1]*tablex7 ) * tabley1 +
01255                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01256                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01257                              data[x7+y2]*tablex7 ) * tabley2 +
01258                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01259                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01260                              data[x7+y3]*tablex7 ) * tabley3 +
01261                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01262                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01263                              data[x7+y4]*tablex7 ) * tabley4 +
01264                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01265                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01266                              data[x7+y5]*tablex7 ) * tabley5 +
01267                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01268                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01269                              data[x7+y6]*tablex7 ) * tabley6 +
01270                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01271                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01272                              data[x7+y7]*tablex7 ) * tabley7;
01273 
01274                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01275                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01276         } else {  //  3D
01277                 dely = restrict1(dely, ny);
01278                 int inyold = int(Util::round(dely));
01279                 delz = restrict1(delz, nz);
01280                 int inzold = int(Util::round(delz));
01281 
01282                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01283                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01284                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01285                 float tablex4 = kb.i0win_tab(delx-inxold);
01286                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01287                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01288                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01289 
01290                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01291                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01292                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01293                 float tabley4 = kb.i0win_tab(dely-inyold);
01294                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01295                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01296                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01297 
01298                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01299                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01300                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01301                 float tablez4 = kb.i0win_tab(delz-inzold);
01302                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01303                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01304                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01305 
01306                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01307 
01308                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01309                         x1 = (inxold-3+nx)%nx;
01310                         x2 = (inxold-2+nx)%nx;
01311                         x3 = (inxold-1+nx)%nx;
01312                         x4 = (inxold  +nx)%nx;
01313                         x5 = (inxold+1+nx)%nx;
01314                         x6 = (inxold+2+nx)%nx;
01315                         x7 = (inxold+3+nx)%nx;
01316 
01317                         y1 = ((inyold-3+ny)%ny)*nx;
01318                         y2 = ((inyold-2+ny)%ny)*nx;
01319                         y3 = ((inyold-1+ny)%ny)*nx;
01320                         y4 = ((inyold  +ny)%ny)*nx;
01321                         y5 = ((inyold+1+ny)%ny)*nx;
01322                         y6 = ((inyold+2+ny)%ny)*nx;
01323                         y7 = ((inyold+3+ny)%ny)*nx;
01324 
01325                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01326                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01327                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01328                         z4 = ((inzold  +nz)%nz)*nx*ny;
01329                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01330                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01331                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01332                 } else {
01333                         x1 = inxold-3;
01334                         x2 = inxold-2;
01335                         x3 = inxold-1;
01336                         x4 = inxold;
01337                         x5 = inxold+1;
01338                         x6 = inxold+2;
01339                         x7 = inxold+3;
01340 
01341                         y1 = (inyold-3)*nx;
01342                         y2 = (inyold-2)*nx;
01343                         y3 = (inyold-1)*nx;
01344                         y4 = inyold*nx;
01345                         y5 = (inyold+1)*nx;
01346                         y6 = (inyold+2)*nx;
01347                         y7 = (inyold+3)*nx;
01348 
01349                         z1 = (inzold-3)*nx*ny;
01350                         z2 = (inzold-2)*nx*ny;
01351                         z3 = (inzold-1)*nx*ny;
01352                         z4 = inzold*nx*ny;
01353                         z5 = (inzold+1)*nx*ny;
01354                         z6 = (inzold+2)*nx*ny;
01355                         z7 = (inzold+3)*nx*ny;
01356                 }
01357 
01358                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01359                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01360                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01361                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01362                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01363                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01364                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01365                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01366                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01367                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01368                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01369                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01370                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01371                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01372                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01373                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01374                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01375                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01376                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01377                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01378                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01379                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01380                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01381                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01382                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01383                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01384                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01385                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01386                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01387                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01388                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01389                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01390                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01391                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01392                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01393                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01394                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01395                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01396                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01397                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01398                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01399                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01400                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01401                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01402                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01403                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01404                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01405                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01406                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01407                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01408                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01409                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01410                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01411                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01412                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01413                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01414                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01415                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01416                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01417                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01418                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01419                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01420                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01421                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01422                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01423                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01424                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01425                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01426                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01427                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01428                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01429                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01430                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01431                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01432                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01433                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01434                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01435                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01436                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01437                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01438                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01439                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01440                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01441                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01442                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01443                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01444                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01445                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01446                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01447                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01448                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01449                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01450                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01451                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01452                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01453                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01454                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01455                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01456                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01457                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01458                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01459                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01460                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01461                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01462                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01463                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01464                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01465                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01466                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01467                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01468                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01469                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01470                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01471                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01472                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01473                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01474                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01475                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01476                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01477                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01478                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01479                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01480                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01481                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01482                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01483                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01484                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01485                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01486                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01487                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01488                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01489                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01490                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01491                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01492                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01493                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01494                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01495                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01496                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01497                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01498                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01499                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01500                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01501                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01502                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01503                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01504                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01505 
01506                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01507                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01508                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01509         }
01510         return pixel/w;
01511 }
01512 
01513 /*
01514 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01515 
01516         int nxreal = nx - 2;
01517         if (nxreal != ny)
01518                 throw ImageDimensionException("extractpoint requires ny == nx");
01519         int nhalf = nxreal/2;
01520         int kbsize = kb.get_window_size();
01521         int kbmin = -kbsize/2;
01522         int kbmax = -kbmin;
01523         bool flip = (nuxnew < 0.f);
01524         if (flip) {
01525                 nuxnew *= -1;
01526                 nuynew *= -1;
01527         }
01528         // put (xnew,ynew) on a grid.  The indices will be wrong for
01529         // the Fourier elements in the image, but the grid sizing will
01530         // be correct.
01531         int ixn = int(Util::round(nuxnew));
01532         int iyn = int(Util::round(nuynew));
01533         // set up some temporary weighting arrays
01534         float* wy0 = new float[kbmax - kbmin + 1];
01535         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01536         float* wx0 = new float[kbmax - kbmin + 1];
01537         float* wx = wx0 - kbmin;
01538         for (int i = kbmin; i <= kbmax; i++) {
01539                         int iyp = iyn + i;
01540                         wy[i] = kb.i0win_tab(nuynew - iyp);
01541                         int ixp = ixn + i;
01542                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01543         }
01544         // restrict loops to non-zero elements
01545         int iymin = 0;
01546         for (int iy = kbmin; iy <= -1; iy++) {
01547                 if (wy[iy] != 0.f) {
01548                         iymin = iy;
01549                         break;
01550                 }
01551         }
01552         int iymax = 0;
01553         for (int iy = kbmax; iy >= 1; iy--) {
01554                 if (wy[iy] != 0.f) {
01555                         iymax = iy;
01556                         break;
01557                 }
01558         }
01559         int ixmin = 0;
01560         for (int ix = kbmin; ix <= -1; ix++) {
01561                 if (wx[ix] != 0.f) {
01562                         ixmin = ix;
01563                         break;
01564                 }
01565         }
01566         int ixmax = 0;
01567         for (int ix = kbmax; ix >= 1; ix--) {
01568                 if (wx[ix] != 0.f) {
01569                         ixmax = ix;
01570                         break;
01571                 }
01572         }
01573         float wsum = 0.0f;
01574         for (int iy = iymin; iy <= iymax; iy++)
01575                 for (int ix = ixmin; ix <= ixmax; ix++)
01576                         wsum += wx[ix]*wy[iy];
01577 
01578         complex<float> result(0.f,0.f);
01579         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01580                 // (xin,yin) not within window border from the edge
01581                 for (int iy = iymin; iy <= iymax; iy++) {
01582                         int iyp = iyn + iy;
01583                         for (int ix = ixmin; ix <= ixmax; ix++) {
01584                                 int ixp = ixn + ix;
01585                                 float w = wx[ix]*wy[iy];
01586                                 complex<float> val = fimage->cmplx(ixp,iyp);
01587                                 result += val*w;
01588                         }
01589                 }
01590         } else {
01591                 // points that "stick out"
01592                 for (int iy = iymin; iy <= iymax; iy++) {
01593                         int iyp = iyn + iy;
01594                         for (int ix = ixmin; ix <= ixmax; ix++) {
01595                                 int ixp = ixn + ix;
01596                                 bool mirror = false;
01597                                 int ixt= ixp, iyt= iyp;
01598                                 if (ixt < 0) {
01599                                         ixt = -ixt;
01600                                         iyt = -iyt;
01601                                         mirror = !mirror;
01602                                 }
01603                                 if (ixt > nhalf) {
01604                                         ixt = nxreal - ixt;
01605                                         iyt = -iyt;
01606                                         mirror = !mirror;
01607                                 }
01608                                 if (iyt > nhalf-1)  iyt -= nxreal;
01609                                 if (iyt < -nhalf)   iyt += nxreal;
01610                                 float w = wx[ix]*wy[iy];
01611                                 complex<float> val = fimage->cmplx(ixt,iyt);
01612                                 if (mirror)  result += conj(val)*w;
01613                                 else         result += val*w;
01614                         }
01615                 }
01616         }
01617         if (flip)  result = conj(result)/wsum;
01618         else result /= wsum;
01619         delete [] wx0;
01620         delete [] wy0;
01621         return result;
01622 }*/
01623 
01624 /*
01625 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01626 
01627         int nxreal = nx - 2;
01628         if (nxreal != ny)
01629                 throw ImageDimensionException("extractpoint requires ny == nx");
01630         int nhalf = nxreal/2;
01631         bool flip = false;
01632         if (nuxnew < 0.f) {
01633                 nuxnew *= -1;
01634                 nuynew *= -1;
01635                 flip = true;
01636         }
01637         if (nuynew >= nhalf-0.5)  {
01638                 nuynew -= nxreal;
01639         } else if (nuynew < -nhalf-0.5) {
01640                 nuynew += nxreal;
01641         }
01642 
01643         // put (xnew,ynew) on a grid.  The indices will be wrong for
01644         // the Fourier elements in the image, but the grid sizing will
01645         // be correct.
01646         int ixn = int(Util::round(nuxnew));
01647         int iyn = int(Util::round(nuynew));
01648 
01649         // set up some temporary weighting arrays
01650         static float wy[7];
01651         static float wx[7];
01652 
01653         float iynn = nuynew - iyn;
01654         wy[0] = kb.i0win_tab(iynn+3);
01655         wy[1] = kb.i0win_tab(iynn+2);
01656         wy[2] = kb.i0win_tab(iynn+1);
01657         wy[3] = kb.i0win_tab(iynn);
01658         wy[4] = kb.i0win_tab(iynn-1);
01659         wy[5] = kb.i0win_tab(iynn-2);
01660         wy[6] = kb.i0win_tab(iynn-3);
01661 
01662         float ixnn = nuxnew - ixn;
01663         wx[0] = kb.i0win_tab(ixnn+3);
01664         wx[1] = kb.i0win_tab(ixnn+2);
01665         wx[2] = kb.i0win_tab(ixnn+1);
01666         wx[3] = kb.i0win_tab(ixnn);
01667         wx[4] = kb.i0win_tab(ixnn-1);
01668         wx[5] = kb.i0win_tab(ixnn-2);
01669         wx[6] = kb.i0win_tab(ixnn-3);
01670 
01671         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]);
01672 
01673         complex<float> result(0.f,0.f);
01674         for (int iy = 0; iy < 7; iy++) {
01675                 int iyp = iyn + iy - 3 ;
01676                 for (int ix = 0; ix < 7; ix++) {
01677                         int ixp = ixn + ix - 3;
01678                         float w = wx[ix]*wy[iy];
01679                         complex<float> val = fimage->cmplx(ixp,iyp);
01680                         result += val*w;
01681                 }
01682         }
01683 
01684         if (flip)  result = conj(result)/wsum;
01685         else result /= wsum;
01686 
01687         return result;
01688 }*/
01689 
01690 
01691 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01692 
01693         int nxreal = nx - 2;
01694         if (nxreal != ny)
01695                 throw ImageDimensionException("extractpoint requires ny == nx");
01696         int nhalf = nxreal/2;
01697         bool flip = (nuxnew < 0.f);
01698         if (flip) {
01699                 nuxnew *= -1;
01700                 nuynew *= -1;
01701         }
01702         if (nuynew >= nhalf-0.5)  {
01703                 nuynew -= nxreal;
01704         } else if (nuynew < -nhalf-0.5) {
01705                 nuynew += nxreal;
01706         }
01707 
01708         // put (xnew,ynew) on a grid.  The indices will be wrong for
01709         // the Fourier elements in the image, but the grid sizing will
01710         // be correct.
01711         int ixn = int(Util::round(nuxnew));
01712         int iyn = int(Util::round(nuynew));
01713 
01714         // set up some temporary weighting arrays
01715         static float wy[7];
01716         static float wx[7];
01717 
01718         float iynn = nuynew - iyn;
01719         wy[0] = kb.i0win_tab(iynn+3);
01720         wy[1] = kb.i0win_tab(iynn+2);
01721         wy[2] = kb.i0win_tab(iynn+1);
01722         wy[3] = kb.i0win_tab(iynn);
01723         wy[4] = kb.i0win_tab(iynn-1);
01724         wy[5] = kb.i0win_tab(iynn-2);
01725         wy[6] = kb.i0win_tab(iynn-3);
01726 
01727         float ixnn = nuxnew - ixn;
01728         wx[0] = kb.i0win_tab(ixnn+3);
01729         wx[1] = kb.i0win_tab(ixnn+2);
01730         wx[2] = kb.i0win_tab(ixnn+1);
01731         wx[3] = kb.i0win_tab(ixnn);
01732         wx[4] = kb.i0win_tab(ixnn-1);
01733         wx[5] = kb.i0win_tab(ixnn-2);
01734         wx[6] = kb.i0win_tab(ixnn-3);
01735 
01736         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]);
01737 
01738         complex<float> result(0.f,0.f);
01739         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01740                 // (xin,yin) not within window border from the edge
01741                 for (int iy = 0; iy < 7; iy++) {
01742                         int iyp = iyn + iy - 3 ;
01743                         for (int ix = 0; ix < 7; ix++) {
01744                                 int ixp = ixn + ix - 3;
01745                                 float w = wx[ix]*wy[iy];
01746                                 complex<float> val = fimage->cmplx(ixp,iyp);
01747                                 result += val*w;
01748                         }
01749                 }
01750         } else {
01751                 // points that "stick out"
01752                 for (int iy = 0; iy < 7; iy++) {
01753                         int iyp = iyn + iy - 3;
01754                         for (int ix = 0; ix < 7; ix++) {
01755                                 int ixp = ixn + ix - 3;
01756                                 bool mirror = false;
01757                                 int ixt = ixp, iyt = iyp;
01758                                 if (ixt < 0) {
01759                                         ixt = -ixt;
01760                                         iyt = -iyt;
01761                                         mirror = !mirror;
01762                                 }
01763                                 if (ixt > nhalf) {
01764                                         ixt = nxreal - ixt;
01765                                         iyt = -iyt;
01766                                         mirror = !mirror;
01767                                 }
01768                                 if (iyt > nhalf-1)  iyt -= nxreal;
01769                                 if (iyt < -nhalf)   iyt += nxreal;
01770                                 float w = wx[ix]*wy[iy];
01771                                 complex<float> val = fimage->cmplx(ixt,iyt);
01772                                 if (mirror)  result += conj(val)*w;
01773                                 else         result += val*w;
01774                         }
01775                 }
01776         }
01777         if (flip)  result = conj(result)/wsum;
01778         else result /= wsum;
01779         return result;
01780 }
01781 
01782 /*
01783 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01784 
01785         int nxreal = nx - 2;
01786         if (nxreal != ny)
01787                 throw ImageDimensionException("extractpoint requires ny == nx");
01788         int nhalf = nxreal/2;
01789         bool flip = (nuxnew < 0.f);
01790         if (flip) {
01791                 nuxnew *= -1;
01792                 nuynew *= -1;
01793         }
01794         // put (xnew,ynew) on a grid.  The indices will be wrong for
01795         // the Fourier elements in the image, but the grid sizing will
01796         // be correct.
01797         int ixn = int(Util::round(nuxnew));
01798         int iyn = int(Util::round(nuynew));
01799         // set up some temporary weighting arrays
01800         static float wy[7];
01801         static float wx[7];
01802 
01803         float iynn = nuynew - iyn;
01804         wy[0] = kb.i0win_tab(iynn+3);
01805         wy[1] = kb.i0win_tab(iynn+2);
01806         wy[2] = kb.i0win_tab(iynn+1);
01807         wy[3] = kb.i0win_tab(iynn);
01808         wy[4] = kb.i0win_tab(iynn-1);
01809         wy[5] = kb.i0win_tab(iynn-2);
01810         wy[6] = kb.i0win_tab(iynn-3);
01811 
01812         float ixnn = nuxnew - ixn;
01813         wx[0] = kb.i0win_tab(ixnn+3);
01814         wx[1] = kb.i0win_tab(ixnn+2);
01815         wx[2] = kb.i0win_tab(ixnn+1);
01816         wx[3] = kb.i0win_tab(ixnn);
01817         wx[4] = kb.i0win_tab(ixnn-1);
01818         wx[5] = kb.i0win_tab(ixnn-2);
01819         wx[6] = kb.i0win_tab(ixnn-3);
01820 
01821         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]);
01822 
01823         complex<float> result(0.f,0.f);
01824 
01825         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01826                 // (xin,yin) not within window border from the edge
01827                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01828                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01829                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01830                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01831                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
01832                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
01833                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
01834                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
01835                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
01836                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
01837                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
01838                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
01839                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
01840                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
01841                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
01842                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
01843                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
01844                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
01845                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
01846                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
01847                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
01848                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
01849                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
01850                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
01851                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
01852                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
01853                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
01854                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
01855                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
01856                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
01857                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
01858                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
01859                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
01860                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
01861                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
01862                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
01863                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
01864                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
01865                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
01866                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
01867                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
01868                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
01869                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
01870                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
01871                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
01872                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
01873                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
01874                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
01875                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
01876 
01877         } else {
01878                 // points that "stick out"
01879                 for (int iy = 0; iy < 7; iy++) {
01880                         int iyp = iyn + iy - 3;
01881                         for (int ix = 0; ix < 7; ix++) {
01882                                 int ixp = ixn + ix - 3;
01883                                 bool mirror = false;
01884                                 int ixt= ixp, iyt= iyp;
01885                                 if (ixt < 0) {
01886                                         ixt = -ixt;
01887                                         iyt = -iyt;
01888                                         mirror = !mirror;
01889                                 }
01890                                 if (ixt > nhalf) {
01891                                         ixt = nxreal - ixt;
01892                                         iyt = -iyt;
01893                                         mirror = !mirror;
01894                                 }
01895                                 if (iyt > nhalf-1)  iyt -= nxreal;
01896                                 if (iyt < -nhalf)   iyt += nxreal;
01897                                 float w = wx[ix]*wy[iy];
01898                                 complex<float> val = fimage->cmplx(ixt,iyt);
01899                                 if (mirror)  result += conj(val)*w;
01900                                 else         result += val*w;
01901                         }
01902                 }
01903         }
01904         if (flip)  result = conj(result)/wsum;
01905         else result /= wsum;
01906         return result;
01907 }*/
01908 
01909 
01910 float Util::triquad(float R, float S, float T, float* fdata)
01911 {
01912 
01913     const float C2 = 0.5f;    //1.0 / 2.0;
01914     const float C4 = 0.25f;   //1.0 / 4.0;
01915     const float C8 = 0.125f;  //1.0 / 8.0;
01916 
01917     float  RS   = R * S;
01918     float  ST   = S * T;
01919     float  RT   = R * T;
01920     float  RST  = R * ST;
01921 
01922     float  RSQ  = 1-R*R;
01923     float  SSQ  = 1-S*S;
01924     float  TSQ  = 1-T*T;
01925 
01926     float  RM1  = (1-R);
01927     float  SM1  = (1-S);
01928     float  TM1  = (1-T);
01929 
01930     float  RP1  = (1+R);
01931     float  SP1  = (1+S);
01932     float  TP1  = (1+T);
01933 
01934     float triquad =
01935     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
01936         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
01937         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
01938         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
01939         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
01940         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
01941         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
01942         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
01943         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
01944 //
01945         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
01946         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
01947         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
01948         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
01949                       RSQ  * SSQ  * TSQ * fdata[13] +
01950         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
01951         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
01952         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
01953         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
01954  //
01955         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
01956         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
01957         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
01958         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
01959         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
01960         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
01961         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
01962         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
01963         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
01964      return triquad;
01965 }
01966 
01967 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01968                 : M(M_), fc(fc_), ntable(ntable_) {
01969         // Sinc-Blackman kernel
01970         build_sBtable();
01971 }
01972 
01973 void Util::sincBlackman::build_sBtable() {
01974         sBtable.resize(ntable+1);
01975         int ltab = int(round(float(ntable)/1.25f));
01976         int M2 = M/2;
01977         fltb = float(ltab)/M2;
01978         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01979         float x = 1.0e-7f;
01980         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)));
01981         for (int i=1; i <= ltab; i++) {
01982                 x = float(i)/fltb;
01983                 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)));
01984                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
01985         }
01986 }
01987 
01988 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01989                                          int N_, float vtable_, int ntable_)
01990                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01991                   ntable(ntable_) {
01992         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
01993         if (0.f == v) v = float(K)/2;
01994         if (0.f == vtable) vtable = v;
01995         alphar = alpha*r;
01996         fac = static_cast<float>(twopi)*alphar*v;
01997         vadjust = 1.0f*v;
01998         facadj = static_cast<float>(twopi)*alphar*vadjust;
01999         build_I0table();
02000 }
02001 
02002 float Util::KaiserBessel::i0win(float x) const {
02003         float val0 = float(gsl_sf_bessel_I0(facadj));
02004         float absx = fabs(x);
02005         if (absx > vadjust) return 0.f;
02006         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02007         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02008         return res;
02009 }
02010 
02011 void Util::KaiserBessel::build_I0table() {
02012         i0table.resize(ntable+1); // i0table[0:ntable]
02013         int ltab = int(round(float(ntable)/1.25f));
02014         fltb = float(ltab)/(K/2);
02015         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02016         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02017         for (int i=0; i <= ltab; i++) {
02018                 float s = float(i)/fltb/N;
02019                 if (s < vadjust) {
02020                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02021                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02022                 } else {
02023                         i0table[i] = 0.f;
02024                 }
02025 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02026         }
02027 }
02028 
02029 float Util::KaiserBessel::I0table_maxerror() {
02030         float maxdiff = 0.f;
02031         for (int i = 1; i <= ntable; i++) {
02032                 float diff = fabs(i0table[i] - i0table[i-1]);
02033                 if (diff > maxdiff) maxdiff = diff;
02034         }
02035         return maxdiff;
02036 }
02037 
02038 float Util::KaiserBessel::sinhwin(float x) const {
02039         float val0 = sinh(fac)/fac;
02040         float absx = fabs(x);
02041         if (0.0 == x) {
02042                 float res = 1.0f;
02043                 return res;
02044         } else if (absx == alphar) {
02045                 return 1.0f/val0;
02046         } else if (absx < alphar) {
02047                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02048                 float facrt = fac*rt;
02049                 float res = (sinh(facrt)/facrt)/val0;
02050                 return res;
02051         } else {
02052                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02053                 float facrt = fac*rt;
02054                 float res = (sin(facrt)/facrt)/val0;
02055                 return res;
02056         }
02057 }
02058 
02059 float Util::FakeKaiserBessel::i0win(float x) const {
02060         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02061         float absx = fabs(x);
02062         if (absx > vadjust) return 0.f;
02063         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02064         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02065         return res;
02066 }
02067 
02068 void Util::FakeKaiserBessel::build_I0table() {
02069         i0table.resize(ntable+1); // i0table[0:ntable]
02070         int ltab = int(round(float(ntable)/1.1f));
02071         fltb = float(ltab)/(K/2);
02072         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02073         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02074         for (int i=0; i <= ltab; i++) {
02075                 float s = float(i)/fltb/N;
02076                 if (s < vadjust) {
02077                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02078                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02079                 } else {
02080                         i0table[i] = 0.f;
02081                 }
02082         }
02083 }
02084 
02085 float Util::FakeKaiserBessel::sinhwin(float x) const {
02086         float val0 = sinh(fac)/fac;
02087         float absx = fabs(x);
02088         if (0.0 == x) {
02089                 float res = 1.0f;
02090                 return res;
02091         } else if (absx == alphar) {
02092                 return 1.0f/val0;
02093         } else if (absx < alphar) {
02094                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02095                 float facrt = fac*rt;
02096                 float res = (sinh(facrt)/facrt)/val0;
02097                 return res;
02098         } else {
02099                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02100                 float facrt = fac*rt;
02101                 float res = (sin(facrt)/facrt)/val0;
02102                 return res;
02103         }
02104 }
02105 
02106 #if 0 // 1-st order KB window
02107 float Util::FakeKaiserBessel::sinhwin(float x) const {
02108         //float val0 = sinh(fac)/fac;
02109         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02110         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02111         float absx = fabs(x);
02112         if (0.0 == x) {
02113                 //float res = 1.0f;
02114                 float res = val0;
02115                 return res;
02116         } else if (absx == alphar) {
02117                 //return 1.0f/val0;
02118                 return prefix;
02119         } else if (absx < alphar) {
02120                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02121                 //float facrt = fac*rt;
02122                 float facrt = facadj*rt;
02123                 //float res = (sinh(facrt)/facrt)/val0;
02124                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02125                 return res;
02126         } else {
02127                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02128                 //float facrt = fac*rt;
02129                 float facrt = facadj*rt;
02130                 //float res = (sin(facrt)/facrt)/val0;
02131                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02132                 return res;
02133         }
02134 }
02135 #endif // 0
02136 
02137 
02138 
02139 #define  circ(i)         circ[i-1]
02140 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02141 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02142 
02143 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02144         int nsam = image->get_xsize();
02145         int nrow = image->get_ysize();
02146         int nring = numr.size()/3;
02147         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02148         EMData* out = new EMData();
02149         out->set_size(lcirc,1,1);
02150         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02151         float *xim  = image->get_data();
02152         float *circ = out->get_data();
02153 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02154    return out;
02155 }
02156 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02157           float *circ, int lcirc, int nring, char mode)
02158 {*/
02159 /*
02160 c
02161 c  purpose:
02162 c
02163 c  resmaple to polar coordinates
02164 c
02165 */
02166         //  dimension         xim(nsam,nrow),circ(lcirc)
02167         //  integer           numr(3,nring)
02168 
02169         double dfi, dpi;
02170         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02171         float  yq, xold, yold, fi, x, y;
02172 
02173         ns2 = nsam/2+1;
02174         nr2 = nrow/2+1;
02175         dpi = 2.0*atan(1.0);
02176 
02177         for (i=1;i<=nring;i++) {
02178                 // radius of the ring
02179                 inr = numr(1,i);
02180                 yq  = static_cast<float>(inr);
02181                 l   = numr(3,i);
02182                 if (mode == 'h' || mode == 'H')  lt = l/2;
02183                 else                             lt = l/4;
02184 
02185                 nsim           = lt-1;
02186                 dfi            = dpi/(nsim+1);
02187                 kcirc          = numr(2,i);
02188                 xold           = 0.0f;
02189                 yold           = static_cast<float>(inr);
02190                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02191                 xold           = static_cast<float>(inr);
02192                 yold           = 0.0f;
02193                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02194 
02195                 if (mode == 'f' || mode == 'F') {
02196                         xold              = 0.0f;
02197                         yold              = static_cast<float>(-inr);
02198                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02199                         xold              = static_cast<float>(-inr);
02200                         yold              = 0.0f;
02201                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02202                 }
02203 
02204                 for (j=1;j<=nsim;j++) {
02205                         fi               = static_cast<float>(dfi*j);
02206                         x                = sin(fi)*yq;
02207                         y                = cos(fi)*yq;
02208                         xold             = x;
02209                         yold             = y;
02210                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02211                         xold             =  y;
02212                         yold             = -x;
02213                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02214 
02215                         if (mode == 'f' || mode == 'F')  {
02216                                 xold                = -x;
02217                                 yold                = -y;
02218                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02219                                 xold                = -y;
02220                                 yold                =  x;
02221                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02222                         }
02223                 }
02224         }
02225         return  out;
02226 }
02227 
02228 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02229         int nsam = image->get_xsize();
02230         int nrow = image->get_ysize();
02231         int nring = numr.size()/3;
02232         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02233         EMData* out = new EMData();
02234         out->set_size(lcirc,1,1);
02235         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02236         float *xim  = image->get_data();
02237         float *circ = out->get_data();
02238         double dpi, dfi;
02239         int    it, jt, inr, l, nsim, kcirc, lt;
02240         float  xold, yold, fi, x, y;
02241 
02242         //     cns2 and cnr2 are predefined centers
02243         //     no need to set to zero, all elements are defined
02244         dpi = 2*atan(1.0);
02245         for (it=1; it<=nring; it++) {
02246                 // radius of the ring
02247                 inr = numr(1,it);
02248 
02249                 // "F" means a full circle interpolation
02250                 // "H" means a half circle interpolation
02251 
02252                 l = numr(3,it);
02253                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02254                 else                              lt = l / 4;
02255 
02256                 nsim  = lt - 1;
02257                 dfi   = dpi / (nsim+1);
02258                 kcirc = numr(2,it);
02259                 xold  = 0.0f+cns2;
02260                 yold  = inr+cnr2;
02261 
02262                 Assert( kcirc <= lcirc );
02263                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02264 
02265                 xold  = inr+cns2;
02266                 yold  = 0.0f+cnr2;
02267                 Assert( lt+kcirc <= lcirc );
02268                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02269 
02270                 if ( mode == 'f' || mode == 'F' ) {
02271                         xold = 0.0f+cns2;
02272                         yold = -inr+cnr2;
02273                         Assert( lt+lt+kcirc <= lcirc );
02274                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02275 
02276                         xold = -inr+cns2;
02277                         yold = 0.0f+cnr2;
02278                         Assert(lt+lt+lt+kcirc <= lcirc );
02279                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02280                 }
02281 
02282                 for (jt=1; jt<=nsim; jt++) {
02283                         fi   = static_cast<float>(dfi * jt);
02284                         x    = sin(fi) * inr;
02285                         y    = cos(fi) * inr;
02286 
02287                         xold = x+cns2;
02288                         yold = y+cnr2;
02289 
02290                         Assert( jt+kcirc <= lcirc );
02291                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02292 
02293                         xold = y+cns2;
02294                         yold = -x+cnr2;
02295 
02296                         Assert( jt+lt+kcirc <= lcirc );
02297                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02298 
02299                         if ( mode == 'f' || mode == 'F' ) {
02300                                 xold = -x+cns2;
02301                                 yold = -y+cnr2;
02302 
02303                                 Assert( jt+lt+lt+kcirc <= lcirc );
02304                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02305 
02306                                 xold = -y+cns2;
02307                                 yold = x+cnr2;
02308 
02309                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02310                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02311                         }
02312                 } // end for jt
02313         } //end for it
02314         return out;
02315 }
02316 
02317 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02318 {
02319 /*
02320 c  purpose: linear interpolation
02321   Optimized for speed, circular closer removed, checking of ranges removed
02322 */
02323     float bilinear;
02324     int   ixold, iyold;
02325 
02326 /*
02327         float xdif, ydif, xrem, yrem;
02328         ixold   = (int) floor(xold);
02329         iyold   = (int) floor(yold);
02330         ydif = yold - iyold;
02331         yrem = 1.0f - ydif;
02332 
02333         //  May want to insert if?
02334 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02335 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02336 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02337         xdif = xold - ixold;
02338         xrem = 1.0f- xdif;
02339 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02340 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02341 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02342         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02343                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02344 
02345     return bilinear;
02346 }
02347 */
02348         float xdif, ydif;
02349 
02350         ixold   = (int) xold;
02351         iyold   = (int) yold;
02352         ydif = yold - iyold;
02353 
02354         //  May want to insert it?
02355 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02356 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02357 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02358         xdif = xold - ixold;
02359         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02360                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02361                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02362 
02363         return bilinear;
02364 }
02365 
02366 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02367              int  *numr, float *circ, int , int  nring, char  mode) {
02368         double dpi, dfi;
02369         int    it, jt, inr, l, nsim, kcirc, lt;
02370         float  xold, yold, fi, x, y;
02371 
02372         //     cns2 and cnr2 are predefined centers
02373         //     no need to set to zero, all elements are defined
02374 
02375         dpi = 2*atan(1.0);
02376         for (it=1; it<=nring; it++) {
02377                 // radius of the ring
02378                 inr = numr(1,it);
02379 
02380                 l = numr(3,it);
02381                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02382                 else                              lt = l / 4;
02383 
02384                 nsim  = lt - 1;
02385                 dfi   = dpi / (nsim+1);
02386                 kcirc = numr(2,it);
02387 
02388 
02389                 xold  = 0.0f+cns2;
02390                 yold  = inr+cnr2;
02391 
02392                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02393 
02394                 xold  = inr+cns2;
02395                 yold  = 0.0f+cnr2;
02396                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02397 
02398                 if ( mode == 'f' || mode == 'F' ) {
02399                         xold = 0.0f+cns2;
02400                         yold = -inr+cnr2;
02401                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02402 
02403                         xold = -inr+cns2;
02404                         yold = 0.0f+cnr2;
02405                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02406                 }
02407 
02408                 for (jt=1; jt<=nsim; jt++) {
02409                         fi   = static_cast<float>(dfi * jt);
02410                         x    = sin(fi) * inr;
02411                         y    = cos(fi) * inr;
02412 
02413                         xold = x+cns2;
02414                         yold = y+cnr2;
02415                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02416 
02417                         xold = y+cns2;
02418                         yold = -x+cnr2;
02419                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02420 
02421                         if ( mode == 'f' || mode == 'F' ) {
02422                                 xold = -x+cns2;
02423                                 yold = -y+cnr2;
02424                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02425 
02426                                 xold = -y+cns2;
02427                                 yold = x+cnr2;
02428                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02429                         }
02430                 } // end for jt
02431         } //end for it
02432 }
02433 /*
02434 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02435              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02436 {
02437    double dpi, dfi;
02438    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02439    float  yq, fi, x, y;
02440 
02441    //     cns2 and cnr2 are predefined centers
02442    //     no need to set to zero, all elements are defined
02443 
02444    dpi = 2*atan(1.0);
02445    for (it=1; it<=nring; it++) {
02446       // radius of the ring
02447       inr = numr(1,it);
02448       yq  = inr;
02449 
02450       l = numr(3,it);
02451       if ( mode == 'h' || mode == 'H' ) {
02452          lt = l / 2;
02453       }
02454       else { // if ( mode == 'f' || mode == 'F' )
02455          lt = l / 4;
02456       }
02457 
02458       nsim  = lt - 1;
02459       dfi   = dpi / (nsim+1);
02460       kcirc = numr(2,it);
02461 
02462 
02463         xold = (int) (0.0+cns2);
02464         yold = (int) (inr+cnr2);
02465 
02466         circ(kcirc) = xim(xold, yold);
02467 
02468       xold = (int) (inr+cns2);
02469       yold = (int) (0.0+cnr2);
02470       circ(lt+kcirc) = xim(xold, yold);
02471 
02472       if ( mode == 'f' || mode == 'F' ) {
02473          xold  = (int) (0.0+cns2);
02474          yold = (int) (-inr+cnr2);
02475          circ(lt+lt+kcirc) = xim(xold, yold);
02476 
02477          xold  = (int) (-inr+cns2);
02478          yold = (int) (0.0+cnr2);
02479          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02480       }
02481 
02482       for (jt=1; jt<=nsim; jt++) {
02483          fi   = dfi * jt;
02484          x    = sin(fi) * yq;
02485          y    = cos(fi) * yq;
02486 
02487          xold  = (int) (x+cns2);
02488          yold = (int) (y+cnr2);
02489          circ(jt+kcirc) = xim(xold, yold);
02490 
02491          xold  = (int) (y+cns2);
02492          yold = (int) (-x+cnr2);
02493          circ(jt+lt+kcirc) = xim(xold, yold);
02494 
02495          if ( mode == 'f' || mode == 'F' ) {
02496             xold  = (int) (-x+cns2);
02497             yold = (int) (-y+cnr2);
02498             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02499 
02500             xold  = (int) (-y+cns2);
02501             yold = (int) (x+cnr2);
02502             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02503          }
02504       } // end for jt
02505    } //end for it
02506 }
02507 */
02508 //xim((int) floor(xold), (int) floor(yold))
02509 #undef  xim
02510 
02511 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02512 // input image is twice the size of the original image
02513         int nring = numr.size()/3;
02514         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02515         EMData* out = new EMData();
02516         out->set_size(lcirc,1,1);
02517         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02518         float *circ = out->get_data();
02519         float *fimage = image->get_data();
02520         int nx = image->get_xsize();
02521         int ny = image->get_ysize();
02522         int nz = image->get_zsize();
02523         double dpi, dfi;
02524         int    it, jt, inr, l, nsim, kcirc, lt;
02525         float  yq, xold, yold, fi, x, y;
02526 
02527         //     cns2 and cnr2 are predefined centers
02528         //     no need to set to zero, all elements are defined
02529 
02530         dpi = 2*atan(1.0);
02531         for (it=1;it<=nring;it++) {
02532                 // radius of the ring
02533                 inr = numr(1,it);
02534                 yq  = static_cast<float>(inr);
02535 
02536                 l = numr(3,it);
02537                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02538                 else                               lt = l / 4;
02539 
02540                 nsim  = lt - 1;
02541                 dfi   = dpi / (nsim+1);
02542                 kcirc = numr(2,it);
02543                 xold  = 0.0f;
02544                 yold  = static_cast<float>(inr);
02545                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02546 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02547 
02548                 xold  = static_cast<float>(inr);
02549                 yold  = 0.0f;
02550                 circ(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+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02552 
02553         if ( mode == 'f' || mode == 'F' ) {
02554                 xold = 0.0f;
02555                 yold = static_cast<float>(-inr);
02556                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02557 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02558 
02559                 xold = static_cast<float>(-inr);
02560                 yold = 0.0f;
02561                 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);
02562 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02563         }
02564 
02565         for (jt=1;jt<=nsim;jt++) {
02566                 fi   = static_cast<float>(dfi * jt);
02567                 x    = sin(fi) * yq;
02568                 y    = cos(fi) * yq;
02569 
02570                 xold = x;
02571                 yold = y;
02572                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02573 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02574 
02575                 xold = y;
02576                 yold = -x;
02577                 circ(jt+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+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02579 
02580         if ( mode == 'f' || mode == 'F' ) {
02581                 xold = -x;
02582                 yold = -y;
02583                 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);
02584 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02585 
02586                 xold = -y;
02587                 yold = x;
02588                 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);
02589 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02590         }
02591         } // end for jt
02592         } //end for it
02593         return  out;
02594 }
02595 
02596 /*
02597 
02598         A set of 1-D power-of-two FFTs
02599         Pawel & Chao 01/20/06
02600 
02601 fftr_q(xcmplx,nv)
02602   single precision
02603 
02604  dimension xcmplx(2,iabs(nv)/2);
02605  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02606  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02607 
02608 
02609 fftr_d(xcmplx,nv)
02610   double precision
02611 
02612  dimension xcmplx(2,iabs(nv)/2);
02613  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02614  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02615 
02616 
02617 
02618 */
02619 #define  tab1(i)      tab1[i-1]
02620 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02621 #define  br(i)        br[i-1]
02622 #define  bi(i)        bi[i-1]
02623 //-----------------------------------------
02624 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02625 {
02626         double rni,sgn,tr1,tr2,ti1,ti2;
02627         double cc,c,ss,s,t,x2,x3,x4,x5;
02628         int    b3,b4,b5,b6,b7,b56;
02629         int    n, k, l, j, i, ix0, ix1, status=0;
02630 
02631         const double tab1[] = {
02632                 9.58737990959775e-5,
02633                 1.91747597310703e-4,
02634                 3.83495187571395e-4,
02635                 7.66990318742704e-4,
02636                 1.53398018628476e-3,
02637                 3.06795676296598e-3,
02638                 6.13588464915449e-3,
02639                 1.22715382857199e-2,
02640                 2.45412285229123e-2,
02641                 4.90676743274181e-2,
02642                 9.80171403295604e-2,
02643                 1.95090322016128e-1,
02644                 3.82683432365090e-1,
02645                 7.07106781186546e-1,
02646                 1.00000000000000,
02647         };
02648 
02649         n=(int)pow(2.0f,ln);
02650 
02651         k=abs(ks);
02652         l=16-ln;
02653         b3=n*k;
02654         b6=b3;
02655         b7=k;
02656         if (ks > 0) {
02657                 sgn=1.0f;
02658         } else {
02659                 sgn=-1.0f;
02660                 rni=1.0f/(float)(n);
02661                 j=1;
02662                 for (i=1; i<=n; i++) {
02663                         br(j)=br(j)*rni;
02664                         bi(j)=bi(j)*rni;
02665                         j=j+k;
02666                 }
02667         }
02668 
02669 L12:
02670    b6=b6/2;
02671    b5=b6;
02672    b4=2*b6;
02673    b56=b5-b6;
02674 
02675 L14:
02676    tr1=br(b5+1);
02677    ti1=bi(b5+1);
02678    tr2=br(b56+1);
02679    ti2=bi(b56+1);
02680 
02681    br(b5+1)=tr2-tr1;
02682    bi(b5+1)=ti2-ti1;
02683    br(b56+1)=tr1+tr2;
02684    bi(b56+1)=ti1+ti2;
02685 
02686    b5=b5+b4;
02687    b56=b5-b6;
02688    if ( b5 <= b3 )  goto  L14;
02689    if ( b6 == b7 )  goto  L20;
02690 
02691    b4=b7;
02692    cc=2.0f*pow(tab1(l),2);
02693    c=1.0f-cc;
02694    l++;
02695    ss=sgn*tab1(l);
02696    s=ss;
02697 
02698 L16:
02699    b5=b6+b4;
02700    b4=2*b6;
02701    b56=b5-b6;
02702 
02703 L18:
02704    tr1=br(b5+1);
02705    ti1=bi(b5+1);
02706    tr2=br(b56+1);
02707    ti2=bi(b56+1);
02708    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02709    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02710    br(b56+1)=tr1+tr2;
02711    bi(b56+1)=ti1+ti2;
02712 
02713    b5=b5+b4;
02714    b56=b5-b6;
02715    if ( b5 <= b3 )  goto  L18;
02716    b4=b5-b6;
02717    b5=b4-b3;
02718    c=-c;
02719    b4=b6-b5;
02720    if ( b5 < b4 )  goto  L16;
02721    b4=b4+b7;
02722    if ( b4 >= b5 ) goto  L12;
02723 
02724    t=c-cc*c-ss*s;
02725    s=s+ss*c-cc*s;
02726    c=t;
02727    goto  L16;
02728 
02729 L20:
02730    ix0=b3/2;
02731    b3=b3-b7;
02732    b4=0;
02733    b5=0;
02734    b6=ix0;
02735    ix1=0;
02736    if (b6 == b7) goto EXIT;
02737 
02738 L22:
02739    b4=b3-b4;
02740    b5=b3-b5;
02741    x2=br(b4+1);
02742    x3=br(b5+1);
02743    x4=bi(b4+1);
02744    x5=bi(b5+1);
02745    br(b4+1)=x3;
02746    br(b5+1)=x2;
02747    bi(b4+1)=x5;
02748    bi(b5+1)=x4;
02749    if(b6 < b4)  goto  L22;
02750 
02751 L24:
02752    b4=b4+b7;
02753    b5=b6+b5;
02754    x2=br(b4+1);
02755    x3=br(b5+1);
02756    x4=bi(b4+1);
02757    x5=bi(b5+1);
02758    br(b4+1)=x3;
02759    br(b5+1)=x2;
02760    bi(b4+1)=x5;
02761    bi(b5+1)=x4;
02762    ix0=b6;
02763 
02764 L26:
02765    ix0=ix0/2;
02766    ix1=ix1-ix0;
02767    if( ix1 >= 0)  goto L26;
02768 
02769    ix0=2*ix0;
02770    b4=b4+b7;
02771    ix1=ix1+ix0;
02772    b5=ix1;
02773    if ( b5 >= b4)  goto  L22;
02774    if ( b4 < b6)   goto  L24;
02775 
02776 EXIT:
02777    status = 0;
02778 }
02779 
02780 // -----------------------------------------------------------------
02781 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02782 {
02783         //  dimension  br(1),bi(1)
02784 
02785         int b3,b4,b5,b6,b7,b56;
02786         int n, k, l, j, i, ix0, ix1;
02787         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02788         int status=0;
02789 
02790         const float tab1[] = {
02791                 9.58737990959775e-5f,
02792                 1.91747597310703e-4f,
02793                 3.83495187571395e-4f,
02794                 7.66990318742704e-4f,
02795                 1.53398018628476e-3f,
02796                 3.06795676296598e-3f,
02797                 6.13588464915449e-3f,
02798                 1.22715382857199e-2f,
02799                 2.45412285229123e-2f,
02800                 4.90676743274181e-2f,
02801                 9.80171403295604e-2f,
02802                 1.95090322016128e-1f,
02803                 3.82683432365090e-1f,
02804                 7.07106781186546e-1f,
02805                 1.00000000000000f,
02806         };
02807 
02808         n=(int)pow(2.0f,ln);
02809 
02810         k=abs(ks);
02811         l=16-ln;
02812         b3=n*k;
02813         b6=b3;
02814         b7=k;
02815         if( ks > 0 ) {
02816                 sgn=1.0f;
02817         } else {
02818                 sgn=-1.0f;
02819                 rni=1.0f/(float)n;
02820                 j=1;
02821                 for (i=1; i<=n; i++) {
02822                         br(j)=br(j)*rni;
02823                         bi(j)=bi(j)*rni;
02824                         j=j+k;
02825                 }
02826         }
02827 L12:
02828    b6=b6/2;
02829    b5=b6;
02830    b4=2*b6;
02831    b56=b5-b6;
02832 L14:
02833    tr1=br(b5+1);
02834    ti1=bi(b5+1);
02835 
02836    tr2=br(b56+1);
02837    ti2=bi(b56+1);
02838 
02839    br(b5+1)=tr2-tr1;
02840    bi(b5+1)=ti2-ti1;
02841    br(b56+1)=tr1+tr2;
02842    bi(b56+1)=ti1+ti2;
02843 
02844    b5=b5+b4;
02845    b56=b5-b6;
02846    if ( b5 <= b3 )  goto  L14;
02847    if ( b6 == b7 )  goto  L20;
02848 
02849    b4=b7;
02850    cc=2.0f*pow(tab1(l),2);
02851    c=1.0f-cc;
02852    l++;
02853    ss=sgn*tab1(l);
02854    s=ss;
02855 L16:
02856    b5=b6+b4;
02857    b4=2*b6;
02858    b56=b5-b6;
02859 L18:
02860    tr1=br(b5+1);
02861    ti1=bi(b5+1);
02862    tr2=br(b56+1);
02863    ti2=bi(b56+1);
02864    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02865    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02866    br(b56+1)=tr1+tr2;
02867    bi(b56+1)=ti1+ti2;
02868 
02869    b5=b5+b4;
02870    b56=b5-b6;
02871    if(b5 <= b3)  goto L18;
02872    b4=b5-b6;
02873    b5=b4-b3;
02874    c=-c;
02875    b4=b6-b5;
02876    if(b5 < b4)  goto  L16;
02877    b4=b4+b7;
02878    if(b4 >= b5) goto  L12;
02879 
02880    t=c-cc*c-ss*s;
02881    s=s+ss*c-cc*s;
02882    c=t;
02883    goto  L16;
02884 L20:
02885    ix0=b3/2;
02886    b3=b3-b7;
02887    b4=0;
02888    b5=0;
02889    b6=ix0;
02890    ix1=0;
02891    if ( b6 == b7) goto EXIT;
02892 L22:
02893    b4=b3-b4;
02894    b5=b3-b5;
02895    x2=br(b4+1);
02896    x3=br(b5+1);
02897    x4=bi(b4+1);
02898    x5=bi(b5+1);
02899    br(b4+1)=x3;
02900    br(b5+1)=x2;
02901    bi(b4+1)=x5;
02902    bi(b5+1)=x4;
02903    if (b6 < b4) goto  L22;
02904 L24:
02905    b4=b4+b7;
02906    b5=b6+b5;
02907    x2=br(b4+1);
02908    x3=br(b5+1);
02909    x4=bi(b4+1);
02910    x5=bi(b5+1);
02911    br(b4+1)=x3;
02912    br(b5+1)=x2;
02913    bi(b4+1)=x5;
02914    bi(b5+1)=x4;
02915    ix0=b6;
02916 L26:
02917    ix0=ix0/2;
02918    ix1=ix1-ix0;
02919    if(ix1 >= 0)  goto  L26;
02920 
02921    ix0=2*ix0;
02922    b4=b4+b7;
02923    ix1=ix1+ix0;
02924    b5=ix1;
02925    if (b5 >= b4)  goto  L22;
02926    if (b4 < b6)   goto  L24;
02927 EXIT:
02928    status = 0;
02929 }
02930 
02931 void  Util::fftr_q(float *xcmplx, int nv)
02932 {
02933    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02934 
02935         int nu, inv, nu1, n, isub, n2, i1, i2, i;
02936         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02937 
02938         const float tab1[] = {
02939                 9.58737990959775e-5f,
02940                 1.91747597310703e-4f,
02941                 3.83495187571395e-4f,
02942                 7.66990318742704e-4f,
02943                 1.53398018628476e-3f,
02944                 3.06795676296598e-3f,
02945                 6.13588464915449e-3f,
02946                 1.22715382857199e-2f,
02947                 2.45412285229123e-2f,
02948                 4.90676743274181e-2f,
02949                 9.80171403295604e-2f,
02950                 1.95090322016128e-1f,
02951                 3.82683432365090e-1f,
02952                 7.07106781186546e-1f,
02953                 1.00000000000000f,
02954         };
02955 
02956         nu=abs(nv);
02957         inv=nv/nu;
02958         nu1=nu-1;
02959         n=(int)pow(2.f,nu1);
02960         isub=16-nu1;
02961 
02962         ss=-tab1(isub);
02963         cc=-2.0f*pow(tab1(isub-1),2.f);
02964         c=1.0f;
02965         s=0.0f;
02966         n2=n/2;
02967         if ( inv > 0) {
02968                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02969                 tr=xcmplx(1,1);
02970                 ti=xcmplx(2,1);
02971                 xcmplx(1,1)=tr+ti;
02972                 xcmplx(2,1)=tr-ti;
02973                 for (i=1;i<=n2;i++) {
02974                         i1=i+1;
02975                         i2=n-i+1;
02976                         tr1=xcmplx(1,i1);
02977                         tr2=xcmplx(1,i2);
02978                         ti1=xcmplx(2,i1);
02979                         ti2=xcmplx(2,i2);
02980                         t=(cc*c-ss*s)+c;
02981                         s=(cc*s+ss*c)+s;
02982                         c=t;
02983                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02984                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02985                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02986                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02987                 }
02988         } else {
02989                 tr=xcmplx(1,1);
02990                 ti=xcmplx(2,1);
02991                 xcmplx(1,1)=0.5f*(tr+ti);
02992                 xcmplx(2,1)=0.5f*(tr-ti);
02993                 for (i=1; i<=n2; i++) {
02994                         i1=i+1;
02995                         i2=n-i+1;
02996                         tr1=xcmplx(1,i1);
02997                         tr2=xcmplx(1,i2);
02998                         ti1=xcmplx(2,i1);
02999                         ti2=xcmplx(2,i2);
03000                         t=(cc*c-ss*s)+c;
03001                         s=(cc*s+ss*c)+s;
03002                         c=t;
03003                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03004                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03005                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03006                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03007                 }
03008                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03009         }
03010 }
03011 
03012 // -------------------------------------------
03013 void  Util::fftr_d(double *xcmplx, int nv)
03014 {
03015         // double precision  x(2,1)
03016         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03017         double tr1,tr2,ti1,ti2,tr,ti;
03018         double cc,c,ss,s,t;
03019         const double tab1[] = {
03020                 9.58737990959775e-5,
03021                 1.91747597310703e-4,
03022                 3.83495187571395e-4,
03023                 7.66990318742704e-4,
03024                 1.53398018628476e-3,
03025                 3.06795676296598e-3,
03026                 6.13588464915449e-3,
03027                 1.22715382857199e-2,
03028                 2.45412285229123e-2,
03029                 4.90676743274181e-2,
03030                 9.80171403295604e-2,
03031                 1.95090322016128e-1,
03032                 3.82683432365090e-1,
03033                 7.07106781186546e-1,
03034                 1.00000000000000,
03035         };
03036 
03037         nu=abs(nv);
03038         inv=nv/nu;
03039         nu1=nu-1;
03040         n=(int)pow(2.0f,nu1);
03041         isub=16-nu1;
03042         ss=-tab1(isub);
03043         cc=-2.0*pow(tab1(isub-1),2);
03044         c=1.0f;
03045         s=0.0f;
03046         n2=n/2;
03047 
03048         if ( inv > 0 ) {
03049                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03050                 tr=xcmplx(1,1);
03051                 ti=xcmplx(2,1);
03052                 xcmplx(1,1)=tr+ti;
03053                 xcmplx(2,1)=tr-ti;
03054                 for (i=1;i<=n2;i++) {
03055                         i1=i+1;
03056                         i2=n-i+1;
03057                         tr1=xcmplx(1,i1);
03058                         tr2=xcmplx(1,i2);
03059                         ti1=xcmplx(2,i1);
03060                         ti2=xcmplx(2,i2);
03061                         t=(cc*c-ss*s)+c;
03062                         s=(cc*s+ss*c)+s;
03063                         c=t;
03064                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03065                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03066                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03067                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03068                 }
03069         } else {
03070                 tr=xcmplx(1,1);
03071                 ti=xcmplx(2,1);
03072                 xcmplx(1,1)=0.5*(tr+ti);
03073                 xcmplx(2,1)=0.5*(tr-ti);
03074                 for (i=1; i<=n2; i++) {
03075                         i1=i+1;
03076                         i2=n-i+1;
03077                         tr1=xcmplx(1,i1);
03078                         tr2=xcmplx(1,i2);
03079                         ti1=xcmplx(2,i1);
03080                         ti2=xcmplx(2,i2);
03081                         t=(cc*c-ss*s)+c;
03082                         s=(cc*s+ss*c)+s;
03083                         c=t;
03084                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03085                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03086                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03087                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03088                 }
03089                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03090         }
03091 }
03092 #undef  tab1
03093 #undef  xcmplx
03094 #undef  br
03095 #undef  bi
03096 
03097 
03098 void Util::Frngs(EMData* circp, vector<int> numr){
03099         int nring = numr.size()/3;
03100         float *circ = circp->get_data();
03101         int i, l;
03102         for (i=1; i<=nring;i++) {
03103 
03104 #ifdef _WIN32
03105                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03106 #else
03107                 l=(int)(log2(numr(3,i)));
03108 #endif  //_WIN32
03109 
03110                 fftr_q(&circ(numr(2,i)),l);
03111         }
03112 }
03113 
03114 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03115         int nring = numr.size()/3;
03116         float *circ = circp->get_data();
03117         int i, l;
03118         for (i=1; i<=nring;i++) {
03119 
03120 #ifdef _WIN32
03121                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03122 #else
03123                 l=(int)(log2(numr(3,i)));
03124 #endif  //_WIN32
03125 
03126                 fftr_q(&circ(numr(2,i)),-l);
03127         }
03128 }
03129 #undef  circ
03130 
03131 void Util::Applyws(EMData* circp, vector<int> numr, vector<float> wr)
03132 {       /*
03133           Apply weights to FTs of rings
03134         */
03135         const int nring = numr.size() / 3;
03136         const int maxrin = numr.back();
03137         float *circ = circp->get_data();
03138         for (int i = 0; i < nring; ++i) {
03139                 const int numr3i = numr[2+i*3];
03140                 const int numr2i = numr[1+i*3]-1;
03141                 const float w = wr[i];
03142                 circ[numr2i] *= w;
03143                 if (numr3i == maxrin)  circ[numr2i+1] *= w;
03144                 else                   circ[numr2i+1] *= 0.5*w;
03145                 for (int j = 2+numr2i; j < numr3i+numr2i; ++j)  circ[j] *= w;
03146         }
03147 }
03148 
03149 #define  b(i)            b[i-1]
03150 void Util::prb1d(double *b, int npoint, float *pos) {
03151         double  c2,c3;
03152         int     nhalf;
03153 
03154         nhalf = npoint/2 + 1;
03155         *pos  = 0.0;
03156 
03157         if (npoint == 7) {
03158                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03159                      - 6.*b(6) + 31.*b(7);
03160                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03161         }
03162         else if (npoint == 5) {
03163                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03164                    + 46.*b(5) ) / (-70.);
03165                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03166         }
03167         else if (npoint == 3) {
03168                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03169                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03170         }
03171         //else if (npoint == 9) {
03172         else  { // at least one has to be true!!
03173                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03174                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03175                      + 1092.*b(9) ) / (-4620.);
03176                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03177                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03178         }
03179         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03180 }
03181 #undef  b
03182 
03183 #define  circ1(i)        circ1[i-1]
03184 #define  circ2(i)        circ2[i-1]
03185 #define  t(i)            t[i-1]
03186 #define  q(i)            q[i-1]
03187 #define  b(i)            b[i-1]
03188 #define  t7(i)           t7[i-1]
03189 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03190         //  neg = 0 straight,  neg = 1 mirrored
03191         int nring = numr.size()/3;
03192         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03193         int maxrin = numr[numr.size()-1];
03194         double qn;   float  tot;
03195         float *circ1 = circ1p->get_data();
03196         float *circ2 = circ2p->get_data();
03197 /*
03198 c checks single position, neg is flag for checking mirrored position
03199 c
03200 c  input - fourier transforms of rings!
03201 c  first set is conjugated (mirrored) if neg
03202 c  circ1 already multiplied by weights!
03203 c       automatic arrays
03204         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03205         double precision  q(maxrin)
03206         double precision  t7(-3:3)
03207 */
03208         float *t;
03209         double t7[7], *q;
03210         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03211         float  pos;
03212 
03213 #ifdef _WIN32
03214         ip = -(int)(log((float)maxrin)/log(2.0f));
03215 #else
03216         ip = -(int) (log2(maxrin));
03217 #endif  //_WIN32
03218 
03219         q = (double*)calloc(maxrin, sizeof(double));
03220         t = (float*)calloc(maxrin, sizeof(float));
03221 
03222 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03223         for (i=1; i<=nring; i++) {
03224                 numr3i = numr(3,i);
03225                 numr2i = numr(2,i);
03226 
03227                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03228 
03229                 if (numr3i != maxrin) {
03230                          // test .ne. first for speed on some compilers
03231                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03232                         t(2)            = 0.0;
03233 
03234                         if (neg) {
03235                                 // first set is conjugated (mirrored)
03236                                 for (j=3;j<=numr3i;j=j+2) {
03237                                         jc = j+numr2i-1;
03238                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03239                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03240                                 }
03241                         } else {
03242                                 for (j=3;j<=numr3i;j=j+2) {
03243                                         jc = j+numr2i-1;
03244                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03245                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03246                                 }
03247                         }
03248                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03249                 } else {
03250                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03251                         if (neg) {
03252                                 // first set is conjugated (mirrored)
03253                                 for (j=3;j<=maxrin;j=j+2) {
03254                                         jc = j+numr2i-1;
03255                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03256                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03257                                 }
03258                         } else {
03259                                 for (j=3;j<=maxrin;j=j+2) {
03260                                         jc = j+numr2i-1;
03261                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03262                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03263                                 }
03264                         }
03265                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03266                 }
03267         }
03268 
03269         fftr_d(q,ip);
03270 
03271         qn = -1.0e20;
03272         for (j=1;j<=maxrin;j++) {
03273            if (q(j) >= qn) {
03274                   qn = q(j); jtot = j;
03275            }
03276         }
03277 
03278         for (k=-3; k<=3; k++) {
03279                 j = (jtot+k+maxrin-1)%maxrin + 1;
03280                 t7(k+4) = q(j);
03281         }
03282 
03283         prb1d(t7,7,&pos);
03284 
03285         tot = (float)jtot + pos;
03286 
03287         if (q) free(q);
03288         if (t) free(t);
03289 
03290         Dict retvals;
03291         retvals["qn"] = qn;
03292         retvals["tot"] = tot;
03293         return  retvals;
03294 }
03295 
03296 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03297    //  neg = 0 straight,  neg = 1 mirrored
03298         int nring = numr.size()/3;
03299         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03300         int maxrin = numr[numr.size()-1];
03301         double qn;   float  tot;
03302         float *circ1 = circ1p->get_data();
03303         float *circ2 = circ2p->get_data();
03304 /*
03305 c checks single position, neg is flag for checking mirrored position
03306 c
03307 c  input - fourier transforms of rings!
03308 c  first set is conjugated (mirrored) if neg
03309 c  multiplication by weights!
03310 c       automatic arrays
03311         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03312         double precision  q(maxrin)
03313         double precision  t7(-3:3)
03314 */
03315         float *t;
03316         double t7[7], *q;
03317         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03318         float  pos;
03319 
03320 #ifdef _WIN32
03321         ip = -(int)(log((float)maxrin)/log(2.0f));
03322 #else
03323         ip = -(int) (log2(maxrin));
03324 #endif  //_WIN32
03325 
03326         q = (double*)calloc(maxrin, sizeof(double));
03327         t = (float*)calloc(maxrin, sizeof(float));
03328 
03329 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03330         for (i=1;i<=nring;i++) {
03331                 numr3i = numr(3,i);
03332                 numr2i = numr(2,i);
03333 
03334                 t(1) = circ1(numr2i) * circ2(numr2i);
03335 
03336                 if (numr3i != maxrin) {
03337                         // test .ne. first for speed on some compilers
03338                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03339                         t(2)      = 0.0;
03340 
03341                         if (neg) {
03342                                 // first set is conjugated (mirrored)
03343                                 for (j=3; j<=numr3i; j=j+2) {
03344                                         jc = j+numr2i-1;
03345                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03346                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03347                                 }
03348                         } else {
03349                                 for (j=3; j<=numr3i; j=j+2) {
03350                                         jc = j+numr2i-1;
03351                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03352                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03353                                 }
03354                         }
03355                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03356                 } else {
03357                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03358                         if (neg) {
03359                                 // first set is conjugated (mirrored)
03360                                 for (j=3; j<=maxrin; j=j+2) {
03361                                         jc = j+numr2i-1;
03362                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03363                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03364                                 }
03365                         } else {
03366                                 for (j=3; j<=maxrin; j=j+2) {
03367                                 jc = j+numr2i-1;
03368                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03369                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03370                                 }
03371                         }
03372                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03373                 }
03374         }
03375 
03376         fftr_d(q,ip);
03377 
03378         qn = -1.0e20;
03379         for (j=1;j<=maxrin;j++) {
03380                 //cout << j << "  " << q(j) << endl;
03381                 if (q(j) >= qn) {
03382                         qn = q(j);
03383                         jtot = j;
03384                 }
03385         }
03386 
03387         for (k=-3; k<=3; k++) {
03388                 j = (jtot+k+maxrin-1)%maxrin + 1;
03389                 t7(k+4) = q(j);
03390         }
03391 
03392         prb1d(t7,7,&pos);
03393 
03394         tot = (float)jtot + pos;
03395 
03396         //if (q) free(q);
03397         if (t) free(t);
03398 
03399         Dict retvals;
03400         //tot = 1;
03401         //qn = q(1);
03402         retvals["qn"] = qn;
03403         retvals["tot"] = tot;
03404 
03405         if (q) free(q);
03406 
03407         return  retvals;
03408 }
03409 
03410 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03411         int nring = numr.size()/3;
03412         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03413         int maxrin = numr[numr.size()-1];
03414         double qn; float tot; double qm; float tmt;
03415         float *circ1 = circ1p->get_data();
03416         float *circ2 = circ2p->get_data();
03417 /*
03418 c
03419 c  checks both straight & mirrored positions
03420 c
03421 c  input - fourier transforms of rings!!
03422 c  circ1 already multiplied by weights!
03423 c
03424 */
03425 
03426         // dimension             circ1(lcirc),circ2(lcirc)
03427 
03428         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03429         double *t, *q, t7[7];
03430 
03431         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03432         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03433 
03434         qn  = 0.0f;
03435         qm  = 0.0f;
03436         tot = 0.0f;
03437         tmt = 0.0f;
03438 #ifdef _WIN32
03439         ip = -(int)(log((float)maxrin)/log(2.0f));
03440 #else
03441         ip = -(int)(log2(maxrin));
03442 #endif  //_WIN32
03443   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03444 
03445         //  c - straight  = circ1 * conjg(circ2)
03446         //  zero q array
03447 
03448         q = (double*)calloc(maxrin,sizeof(double));
03449 
03450         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03451         //   zero t array
03452         t = (double*)calloc(maxrin,sizeof(double));
03453 
03454    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03455         for (i=1; i<=nring; i++) {
03456 
03457                 numr3i = numr(3,i);   // Number of samples of this ring
03458                 numr2i = numr(2,i);   // The beginning point of this ring
03459 
03460                 t1   = circ1(numr2i) * circ2(numr2i);
03461                 q(1) += t1;
03462                 t(1) += t1;
03463 
03464                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03465                 if (numr3i == maxrin)  {
03466                         q(2) += t1;
03467                         t(2) += t1;
03468                 } else {
03469                         q(numr3i+1) += t1;
03470                         t(numr3i+1) += t1;
03471                 }
03472 
03473                 for (j=3; j<=numr3i; j += 2) {
03474                         jc     = j+numr2i-1;
03475 
03476 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03477 //                                ----- -----    ----- -----
03478 //                                 t1     t2      t3    t4
03479 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03480 //                                    ----- -----    ----- -----
03481 //                                     t1    t2       t3    t4
03482 
03483                         c1     = circ1(jc);
03484                         c2     = circ1(jc+1);
03485                         d1     = circ2(jc);
03486                         d2     = circ2(jc+1);
03487 
03488                         t1     = c1 * d1;
03489                         t2     = c2 * d2;
03490                         t3     = c1 * d2;
03491                         t4     = c2 * d1;
03492 
03493                         q(j)   += t1 + t2;
03494                         q(j+1) += -t3 + t4;
03495                         t(j)   += t1 - t2;
03496                         t(j+1) += -t3 - t4;
03497                 }
03498         }
03499         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03500         fftr_d(q,ip);
03501 
03502         qn  = -1.0e20;
03503         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03504                 if (q(j) >= qn) {
03505                         qn  = q(j);
03506                         jtot = j;
03507                 }
03508         }
03509 
03510         for (k=-3; k<=3; k++) {
03511                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03512                 t7(k+4) = q(j);
03513         }
03514 
03515         // interpolate
03516         prb1d(t7,7,&pos);
03517         tot = (float)(jtot)+pos;
03518         // Do not interpolate
03519         //tot = (float)(jtot);
03520 
03521         // mirrored
03522         fftr_d(t,ip);
03523 
03524         // find angle
03525         qm = -1.0e20;
03526         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03527                 if ( t(j) >= qm ) {
03528                         qm   = t(j);
03529                         jtot = j;
03530                 }
03531         }
03532 
03533         for (k=-3; k<=3; k++)  {
03534                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03535                 t7(k+4) = t(j);
03536         }
03537 
03538         // interpolate
03539 
03540         prb1d(t7,7,&pos);
03541         tmt = float(jtot) + pos;
03542         // Do not interpolate
03543         //tmt = float(jtot);
03544 
03545         free(t);
03546         free(q);
03547 
03548         Dict retvals;
03549         retvals["qn"] = qn;
03550         retvals["tot"] = tot;
03551         retvals["qm"] = qm;
03552         retvals["tmt"] = tmt;
03553         return retvals;
03554 }
03555 
03556 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03557         int nring = numr.size()/3;
03558         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03559         int maxrin = numr[numr.size()-1];
03560         double qn; float tot; double qm; float tmt;
03561         float *circ1 = circ1p->get_data();
03562         float *circ2 = circ2p->get_data();
03563 /*
03564 c
03565 c  checks both straight & mirrored positions
03566 c
03567 c  input - fourier transforms of rings!!
03568 c  circ1 already multiplied by weights!
03569 c
03570 */
03571 
03572         // dimension             circ1(lcirc),circ2(lcirc)
03573 
03574         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03575         double *t, *q;
03576 
03577         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03578         float t1, t2, t3, t4, c1, c2, d1, d2;
03579 
03580         qn  = 0.0f;
03581         qm  = 0.0f;
03582         tot = 0.0f;
03583         tmt = 0.0f;
03584 #ifdef _WIN32
03585         ip = -(int)(log((float)maxrin)/log(2.0f));
03586 #else
03587         ip = -(int)(log2(maxrin));
03588 #endif  //_WIN32
03589   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03590 
03591         //  c - straight  = circ1 * conjg(circ2)
03592         //  zero q array
03593 
03594         q = (double*)calloc(maxrin,sizeof(double));
03595 
03596         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03597         //   zero t array
03598         t = (double*)calloc(maxrin,sizeof(double));
03599 
03600    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03601         for (i=1; i<=nring; i++) {
03602 
03603                 numr3i = numr(3,i);   // Number of samples of this ring
03604                 numr2i = numr(2,i);   // The beginning point of this ring
03605 
03606                 t1   = circ1(numr2i) * circ2(numr2i);
03607                 q(1) += t1;
03608                 t(1) += t1;
03609 
03610                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03611                 if (numr3i == maxrin)  {
03612                         q(2) += t1;
03613                         t(2) += t1;
03614                 } else {
03615                         q(numr3i+1) += t1;
03616                         t(numr3i+1) += t1;
03617                 }
03618 
03619                 for (j=3; j<=numr3i; j += 2) {
03620                         jc     = j+numr2i-1;
03621 
03622 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03623 //                                ----- -----    ----- -----
03624 //                                 t1     t2      t3    t4
03625 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03626 //                                    ----- -----    ----- -----
03627 //                                     t1    t2       t3    t4
03628 
03629                         c1     = circ1(jc);
03630                         c2     = circ1(jc+1);
03631                         d1     = circ2(jc);
03632                         d2     = circ2(jc+1);
03633 
03634                         t1     = c1 * d1;
03635                         t2     = c2 * d2;
03636                         t3     = c1 * d2;
03637                         t4     = c2 * d1;
03638 
03639                         q(j)   += t1 + t2;
03640                         q(j+1) += -t3 + t4;
03641                         t(j)   += t1 - t2;
03642                         t(j+1) += -t3 - t4;
03643                 }
03644         }
03645         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03646         fftr_d(q,ip);
03647 
03648         qn  = -1.0e20;
03649 
03650         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03651         int jstep = static_cast<int>(delta/360.0*maxrin);
03652         if (jstep < 1) { jstep = 1; }
03653 
03654         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03655                 if (q(j) >= qn) {
03656                         qn  = q(j);
03657                         jtot = j;
03658                 }
03659         }
03660 
03661         //for (k=-3; k<=3; k++) {
03662         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03663         //      t7(k+4) = q(j);
03664         //}
03665 
03666         // interpolate
03667         //prb1d(t7,7,&pos);
03668         //tot = (float)(jtot)+pos;
03669         // Do not interpolate
03670         tot = (float)(jtot);
03671 
03672         // mirrored
03673         fftr_d(t,ip);
03674 
03675         // find angle
03676         qm = -1.0e20;
03677         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03678                 if ( t(j) >= qm ) {
03679                         qm   = t(j);
03680                         jtot = j;
03681                 }
03682         }
03683 
03684         //for (k=-3; k<=3; k++)  {
03685         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03686         //      t7(k+4) = t(j);
03687         //}
03688 
03689         // interpolate
03690 
03691         //prb1d(t7,7,&pos);
03692         //tmt = float(jtot) + pos;
03693         // Do not interpolate
03694         tmt = float(jtot);
03695 
03696         free(t);
03697         free(q);
03698 
03699         Dict retvals;
03700         retvals["qn"] = qn;
03701         retvals["tot"] = tot;
03702         retvals["qm"] = qm;
03703         retvals["tmt"] = tmt;
03704         return retvals;
03705 }
03706 
03707 
03708 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
03709 // flag 0 - straight, 1 - mirror
03710 
03711         int nring = numr.size()/3;
03712         int maxrin = numr[numr.size()-1];
03713         double qn; float tot;
03714         float *circ1 = circ1p->get_data();
03715         float *circ2 = circ2p->get_data();
03716 
03717         double *q;
03718 
03719         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03720         float c1, c2, d1, d2;
03721 
03722         qn  = 0.0f;
03723         tot = 0.0f;
03724 #ifdef _WIN32
03725         ip = -(int)(log((float)maxrin)/log(2.0f));
03726 #else
03727         ip = -(int)(log2(maxrin));
03728 #endif  //_WIN32
03729 
03730         //  c - straight  = circ1 * conjg(circ2)
03731         //  zero q array
03732 
03733         q = (double*)calloc(maxrin,sizeof(double));
03734         int neg = 1-2*flag;
03735    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03736 
03737         for (i=1; i<=nring; i++) {
03738 
03739                 numr3i = numr(3,i);   // Number of samples of this ring
03740                 numr2i = numr(2,i);   // The beginning point of this ring
03741 
03742                 q(1) += circ1(numr2i) * circ2(numr2i);
03743 
03744                 float t1 = circ1(numr2i+1) * circ2(numr2i+1);
03745                 if (numr3i == maxrin)   q(2) += t1;
03746                 else  q(numr3i+1) += t1;
03747 
03748                 for (j=3; j<=numr3i; j += 2) {
03749                         jc     = j+numr2i-1;
03750 
03751         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03752         //                                ----- -----    ----- -----
03753         //                                 t1     t2      t3    t4
03754 
03755                         c1     = circ1(jc);
03756                         c2     = circ1(jc+1);
03757                         d1     = circ2(jc);
03758                         d2     = circ2(jc+1);
03759 
03760                         q(j)   +=  c1 * d1 + c2 * d2*neg;
03761                         q(j+1) += -c1 * d2 + c2 * d1*neg;
03762                 }
03763         }
03764 
03765         fftr_d(q,ip);
03766 
03767         qn  = -1.0e20;
03768         // psi = 0 should correspond to psi_pos = 1 (meaning no change in in-plane rotation)
03769         int psi_pos = int(psi/360.0*maxrin+0.5) + 1;
03770         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03771 
03772         for (k=-psi_range; k<=psi_range; k++) {
03773                 j = ( k + psi_pos + maxrin - 1)%maxrin+1;
03774                 if (q(j) >= qn) {
03775                         qn  = q(j);
03776                         jtot = j;
03777                 }
03778         }
03779 
03780         tot = (float)(jtot);
03781         free(q);
03782 
03783         Dict retvals;
03784         retvals["qn"] = qn;
03785         retvals["tot"] = tot;
03786         return retvals;
03787 }
03788 
03789 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
03790 // Computes both straight and mirrored
03791 
03792         int nring = numr.size()/3;
03793         int maxrin = numr[numr.size()-1];
03794         double qn; float tot; double qm; float tmt;
03795         float *circ1 = circ1p->get_data();
03796         float *circ2 = circ2p->get_data();
03797 
03798         double *t, *q;
03799 
03800         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03801         float t1, t2, t3, t4, c1, c2, d1, d2;
03802 
03803         qn  = 0.0f;
03804         qm  = 0.0f;
03805         tot = 0.0f;
03806         tmt = 0.0f;
03807 #ifdef _WIN32
03808         ip = -(int)(log((float)maxrin)/log(2.0f));
03809 #else
03810         ip = -(int)(log2(maxrin));
03811 #endif  //_WIN32
03812 
03813         //  c - straight  = circ1 * conjg(circ2)
03814         //  zero q array
03815 
03816         q = (double*)calloc(maxrin,sizeof(double));
03817         
03818         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03819         //   zero t array
03820         t = (double*)calloc(maxrin,sizeof(double));
03821         
03822    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03823         
03824         for (i=1; i<=nring; i++) {
03825 
03826                 numr3i = numr(3,i);   // Number of samples of this ring
03827                 numr2i = numr(2,i);   // The beginning point of this ring
03828 
03829                 t1   = circ1(numr2i) * circ2(numr2i);
03830                 q(1) += t1;
03831                 t(1) += t1;
03832                 
03833                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03834                 if (numr3i == maxrin)  {
03835                         q(2) += t1;
03836                         t(2) += t1;
03837                 } else {
03838                         q(numr3i+1) += t1;
03839                         t(numr3i+1) += t1;
03840                 }
03841 
03842                 for (j=3; j<=numr3i; j += 2) {
03843                         jc     = j+numr2i-1;
03844 
03845         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03846         //                                ----- -----    ----- -----
03847         //                                 t1     t2      t3    t4
03848 
03849                         c1     = circ1(jc);
03850                         c2     = circ1(jc+1);
03851                         d1     = circ2(jc);
03852                         d2     = circ2(jc+1);
03853 
03854                         t1     = c1 * d1;
03855                         t3     = c1 * d2;
03856                         t2     = c2 * d2;
03857                         t4     = c2 * d1;
03858 
03859                         q(j)   +=  t1 + t2;
03860                         q(j+1) += -t3 + t4;
03861                         t(j)   +=  t1 - t2;
03862                         t(j+1) += -t3 - t4;
03863                 }
03864         }
03865 
03866         fftr_d(q,ip);
03867 
03868         qn  = -1.0e20;
03869         // psi = 0 should correspond to psi_pos = 1
03870         int psi_pos = int(psi/360.0*maxrin+0.5) + 1;
03871         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03872 
03873         for (k=-psi_range; k<=psi_range; k++) {
03874                 j = (k+psi_pos+maxrin-1)%maxrin+1;
03875                 if (q(j) >= qn) {
03876                         qn  = q(j);
03877                         jtot = j;
03878                 }
03879         }
03880 
03881         tot = (float)(jtot);
03882         free(q);
03883 
03884     // mirrored
03885         fftr_d(t,ip);
03886 
03887         qm  = -1.0e20;
03888 
03889         for (k=-psi_range; k<=psi_range; k++) {
03890                 j = (k+psi_pos+maxrin-1)%maxrin+1;
03891                 if (t(j) >= qm) {
03892                         qm  = t(j);
03893                         jtot = j;
03894                 }
03895         }
03896 
03897         tmt = (float)(jtot);
03898         free(t);
03899 
03900         Dict retvals;
03901         retvals["qn"] = qn;
03902         retvals["tot"] = tot;
03903         retvals["qm"] = qm;
03904         retvals["tmt"] = tmt;
03905         return retvals;
03906 }
03907 
03908 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03909         int nring = numr.size()/3;
03910         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03911         int maxrin = numr[numr.size()-1];
03912         double qn; float tot;
03913         float *circ1 = circ1p->get_data();
03914         float *circ2 = circ2p->get_data();
03915 /*
03916 c
03917 c  checks only straight position
03918 c
03919 c  input - fourier transforms of rings!!
03920 c  circ1 already multiplied by weights!
03921 c
03922 */
03923 
03924         // dimension             circ1(lcirc),circ2(lcirc)
03925 
03926         // q(maxrin), t7(-3:3)  //maxrin+2 removed
03927         double *q, t7[7];
03928 
03929         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03930         float c1, c2, d1, d2, pos;
03931 
03932         qn  = 0.0;
03933         tot = 0.0;
03934 #ifdef _WIN32
03935         ip = -(int)(log((float)maxrin)/log(2.0f));
03936 #else
03937    ip = -(int)(log2(maxrin));
03938 #endif  //_WIN32
03939         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03940 
03941         //  c - straight  = circ1 * conjg(circ2)
03942         //  zero q array
03943 
03944         q = (double*)calloc(maxrin,sizeof(double));
03945 
03946                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03947         for (i=1; i<=nring; i++) {
03948 
03949                 numr3i = numr(3,i);   // Number of samples of this ring
03950                 numr2i = numr(2,i);   // The beginning point of this ring
03951 
03952                 q(1) += circ1(numr2i) * circ2(numr2i);
03953 
03954                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
03955                 else                 q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
03956 
03957                 for (j=3; j<=numr3i; j += 2) {
03958                         jc     = j+numr2i-1;
03959 
03960 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03961 //                                ----- -----    ----- -----
03962 //                                 t1     t2      t3    t4
03963 
03964                         c1     = circ1(jc);
03965                         c2     = circ1(jc+1);
03966                         d1     = circ2(jc);
03967                         d2     = circ2(jc+1);
03968 
03969                         q(j)   +=  c1 * d1 + c2 * d2;
03970                         q(j+1) += -c1 * d2 + c2 * d1;
03971                 }
03972         }
03973 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
03974         fftr_d(q,ip);
03975 
03976         qn  = -1.0e20;
03977         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03978                 if (q(j) >= qn) {
03979                         qn  = q(j);
03980                         jtot = j;
03981                 }
03982         }
03983 
03984         for (k=-3; k<=3; k++)  {
03985                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03986                 t7(k+4) = q(j);
03987         }
03988 
03989         // interpolate
03990         prb1d(t7,7,&pos);
03991         tot = (float)(jtot)+pos;
03992         // Do not interpolate
03993         //*tot = (float)(jtot);
03994 
03995         free(q);
03996 
03997         Dict retvals;
03998         retvals["qn"] = qn;
03999         retvals["tot"] = tot;
04000         return retvals;
04001 }
04002 
04003 #define  dout(i,j)        dout[i+maxrin*j]
04004 #define  circ1b(i)        circ1b[i-1]
04005 #define  circ2b(i)        circ2b[i-1]
04006 
04007 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04008 
04009    // dimension         circ1(lcirc),circ2(lcirc)
04010 
04011         int   ip, jc, numr3i, numr2i, i, j;
04012         float t1, t2, t3, t4, c1, c2, d1, d2;
04013 
04014         int nring = numr.size()/3;
04015         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04016         int maxrin = numr[numr.size()-1];
04017 
04018         float* circ1b = circ1->get_data();
04019         float* circ2b = circ2->get_data();
04020 
04021         // t(maxrin), q(maxrin)  // removed +2
04022         double *t, *q;
04023 
04024         q = (double*)calloc(maxrin,sizeof(double));
04025         t = (double*)calloc(maxrin,sizeof(double));
04026 
04027 #ifdef _WIN32
04028         ip = -(int)(log((float)maxrin)/log(2.0f));
04029 #else
04030         ip = -(int)(log2(maxrin));
04031 #endif  //_WIN32
04032 
04033         //  q - straight  = circ1 * conjg(circ2)
04034 
04035         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04036 
04037         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04038 
04039         for (i=1; i<=nring; i++) {
04040 
04041                 numr3i = numr(3,i);
04042                 numr2i = numr(2,i);
04043 
04044                 t1   = circ1b(numr2i) * circ2b(numr2i);
04045                 q(1) += t1;
04046                 t(1) += t1;
04047 
04048                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04049                 if (numr3i == maxrin)  {
04050                         q(2) += t1;
04051                         t(2) += t1;
04052                 } else {
04053                         q(numr3i+1) += t1;
04054                         t(numr3i+1) += t1;
04055                 }
04056 
04057                 for (j=3; j<=numr3i; j=j+2) {
04058                         jc     = j+numr2i-1;
04059 
04060                         c1     = circ1b(jc);
04061                         c2     = circ1b(jc+1);
04062                         d1     = circ2b(jc);
04063                         d2     = circ2b(jc+1);
04064 
04065                         t1     = c1 * d1;
04066                         t3     = c1 * d2;
04067                         t2     = c2 * d2;
04068                         t4     = c2 * d1;
04069 
04070                         q(j)   +=  t1 + t2;
04071                         q(j+1) += -t3 + t4;
04072                         t(j)   +=  t1 - t2;
04073                         t(j+1) += -t3 - t4;
04074                 }
04075         }
04076 
04077         // straight
04078         fftr_d(q,ip);
04079 
04080         // mirrored
04081         fftr_d(t,ip);
04082 
04083         EMData* out = new EMData();
04084         out->set_size(maxrin,2,1);
04085         float *dout = out->get_data();
04086         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04087         //out->set_size(maxrin,1,1);
04088         //float *dout = out->get_data();
04089         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04090         free(t);
04091         free(q);
04092         return out;
04093 }
04094 
04095 
04096 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04097 
04098         int maxrin = numr[numr.size()-1];
04099 
04100         vector<float> r(2*maxrin);
04101 
04102         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04103 
04104         return r;
04105 }
04106 
04107 #define  dout(i,j)        dout[i+maxrin*j]
04108 #define  circ1b(i)        circ1b[i-1]
04109 #define  circ2b(i)        circ2b[i-1]
04110 
04111 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04112 
04113    // dimension         circ1(lcirc),circ2(lcirc)
04114 
04115         int   ip, jc, numr3i, numr2i, i, j;
04116         float t1, t2, t3, t4, c1, c2, d1, d2;
04117 
04118         int nring = numr.size()/3;
04119         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04120         int maxrin = numr[numr.size()-1];
04121 
04122         float* circ1b = circ1->get_data();
04123         float* circ2b = circ2->get_data();
04124 
04125 #ifdef _WIN32
04126         ip = -(int)(log((float)maxrin)/log(2.0f));
04127 #else
04128         ip = -(int)(log2(maxrin));
04129 #endif  //_WIN32
04130         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04131 
04132         //  q - straight  = circ1 * conjg(circ2)
04133 
04134         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04135 
04136         for (i=1; i<=nring; i++) {
04137 
04138                 numr3i = numr(3,i);
04139                 numr2i = numr(2,i);
04140 
04141                 t1   = circ1b(numr2i) * circ2b(numr2i);
04142                 q(1) += t1;
04143                 t(1) += t1;
04144 
04145                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04146                 if (numr3i == maxrin)  {
04147                         q(2) += t1;
04148                         t(2) += t1;
04149                 } else {
04150                         q(numr3i+1) += t1;
04151                         t(numr3i+1) += t1;
04152                 }
04153 
04154                 for (j=3; j<=numr3i; j=j+2) {
04155                         jc     = j+numr2i-1;
04156 
04157                         c1     = circ1b(jc);
04158                         c2     = circ1b(jc+1);
04159                         d1     = circ2b(jc);
04160                         d2     = circ2b(jc+1);
04161 
04162                         t1     = c1 * d1;
04163                         t3     = c1 * d2;
04164                         t2     = c2 * d2;
04165                         t4     = c2 * d1;
04166 
04167                         q(j)   +=  t1 + t2;
04168                         q(j+1) += -t3 + t4;
04169                         t(j)   +=  t1 - t2;
04170                         t(j+1) += -t3 - t4;
04171                 }
04172         }
04173         // straight
04174         fftr_q(q,ip);
04175         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04176 
04177         // mirrored
04178         fftr_q(t,ip);
04179 }
04180 
04181 
04182 
04183 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04184 {
04185 
04186         int   ip, jc, numr3i, numr2i, i, j;
04187         float c1, c2, d1, d2;
04188 
04189         int nring = numr.size()/3;
04190         int maxrin = numr[numr.size()-1];
04191 
04192         float* circ1b = circ1->get_data();
04193         float* circ2b = circ2->get_data();
04194 
04195         double *q;
04196 
04197         q = (double*)calloc(maxrin,sizeof(double));
04198 
04199 #ifdef _WIN32
04200         ip = -(int)(log((float)maxrin)/log(2.0f));
04201 #else
04202         ip = -(int)(log2(maxrin));
04203 #endif  //_WIN32
04204 
04205          //  q - straight  = circ1 * conjg(circ2)
04206 
04207         for (i=1;i<=nring;i++) {
04208 
04209                 numr3i = numr(3,i);
04210                 numr2i = numr(2,i);
04211 
04212                 q(1) += circ1b(numr2i) * circ2b(numr2i);
04213 
04214                 if (numr3i == maxrin)   q(2) += circ1b(numr2i+1) * circ2b(numr2i+1);
04215                 else             q(numr3i+1) += circ1b(numr2i+1) * circ2b(numr2i+1);
04216 
04217                 for (j=3;j<=numr3i;j=j+2) {
04218                         jc     = j+numr2i-1;
04219 
04220                         c1     = circ1b(jc);
04221                         c2     = circ1b(jc+1);
04222                         d1     = circ2b(jc);
04223                         d2     = circ2b(jc+1);
04224 
04225                         q(j)   +=  c1 * d1 + c2 * d2;
04226                         q(j+1) += -c1 * d2 + c2 * d1;
04227                 }
04228         }
04229 
04230         // straight
04231         fftr_d(q,ip);
04232 
04233         EMData* out = new EMData();
04234         out->set_size(maxrin,1,1);
04235         float *dout = out->get_data();
04236         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04237         free(q);
04238         return out;
04239 
04240 }
04241 
04242 
04243 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04244 {
04245 
04246         int   ip, jc, numr3i, numr2i, i, j;
04247         float c1, c2, d1, d2;
04248 
04249         int nring = numr.size()/3;
04250         int maxrin = numr[numr.size()-1];
04251 
04252         float* circ1b = circ1->get_data();
04253         float* circ2b = circ2->get_data();
04254 
04255         double *t;
04256 
04257         t = (double*)calloc(maxrin,sizeof(double));
04258 
04259 #ifdef _WIN32
04260         ip = -(int)(log((float)maxrin)/log(2.0f));
04261 #else
04262         ip = -(int)(log2(maxrin));
04263 #endif  //_WIN32
04264 
04265          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04266 
04267         for (i=1;i<=nring;i++) {
04268 
04269                 numr3i = numr(3,i);
04270                 numr2i = numr(2,i);
04271                 t(1) += circ1b(numr2i) * circ2b(numr2i);
04272 
04273                 if (numr3i == maxrin)  t(2) += circ1b(numr2i+1) * circ2b(numr2i+1);
04274                 else          t(numr3i+1) += circ1b(numr2i+1) * circ2b(numr2i+1);
04275 
04276                 for (j=3;j<=numr3i;j=j+2) {
04277                         jc     = j+numr2i-1;
04278 
04279                         c1     = circ1b(jc);
04280                         c2     = circ1b(jc+1);
04281                         d1     = circ2b(jc);
04282                         d2     = circ2b(jc+1);
04283 
04284                         t(j)   +=  c1 * d1 - c2 * d2;
04285                         t(j+1) += -c1 * d2 - c2 * d1;
04286                 }
04287         }
04288 
04289         // mirrored
04290         fftr_d(t,ip);
04291 
04292         EMData* out = new EMData();
04293         out->set_size(maxrin,1,1);
04294         float *dout = out->get_data();
04295         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04296         free(t);
04297         return out;
04298 
04299 }
04300 
04301 #undef circ1b
04302 #undef circ2b
04303 #undef dout
04304 
04305 #undef  circ1
04306 #undef  circ2
04307 #undef  t
04308 #undef  q
04309 #undef  b
04310 #undef  t7
04311 
04312 
04313 #define    QUADPI                   3.141592653589793238462643383279502884197
04314 #define    PI2                      2*QUADPI
04315 
04316 float Util::ener(EMData* ave, vector<int> numr) {
04317         ENTERFUNC;
04318         long double ener,en;
04319 
04320         int nring = numr.size()/3;
04321         float *aveptr = ave->get_data();
04322 
04323         ener = 0.0;
04324         for (int i=1; i<=nring; i++) {
04325                 int numr3i = numr(3,i);
04326                 int np     = numr(2,i)-1;
04327                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04328                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04329                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04330                 ener += en/numr3i;
04331         }
04332         EXITFUNC;
04333         return static_cast<float>(ener);
04334 }
04335 
04336 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04337         ENTERFUNC;
04338         long double ener, en;
04339         float arg, cs, si;
04340 
04341         int nima = data.size();
04342         int nring = numr.size()/3;
04343         int maxrin = numr(3,nring);
04344 
04345         ener = 0.0;
04346         for (int i=1; i<=nring; i++) {
04347                 int numr3i = numr(3,i);
04348                 int np     = numr(2,i)-1;
04349                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04350                 float temp1 = 0.0, temp2 = 0.0;
04351                 for (int kk=0; kk<nima; kk++) {
04352                         float *ptr = data[kk]->get_data();
04353                         temp1 += ptr[np];
04354                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04355                 }
04356                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04357                 for (int j=2; j<numr3i; j+=2) {
04358                         float tempr = 0.0, tempi = 0.0;
04359                         for (int kk=0; kk<nima; kk++) {
04360                                 float *ptr = data[kk]->get_data();
04361                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04362                                 cs = cos(arg);
04363                                 si = sin(arg);
04364                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04365                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04366                         }
04367                         en += tq*(tempr*tempr+tempi*tempi);
04368                 }
04369                 ener += en/numr3i;
04370         }
04371         EXITFUNC;
04372         return static_cast<float>(ener);
04373 }
04374 
04375 void Util::update_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04376         int nring = numr.size()/3;
04377         float *ave = avep->get_data();
04378         float *dat = datp->get_data();
04379         int i, j, numr3i, np;
04380         float  arg, cs, si;
04381         int maxrin = numr(3,nring);
04382         if(mirror == 1) { //for mirrored data has to be conjugated
04383                 for (i=1; i<=nring; i++) {
04384                         numr3i = numr(3,i);
04385                         np     = numr(2,i)-1;
04386                         ave[np]   += dat[np];
04387                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04388                         for (j=2; j<numr3i; j=j+2) {
04389                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04390                                 cs = cos(arg);
04391                                 si = sin(arg);
04392                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04393                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04394                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04395                         }
04396                 }
04397         } else {
04398                 for (i=1; i<=nring; i++) {
04399                         numr3i = numr(3,i);
04400                         np     = numr(2,i)-1;
04401                         ave[np]   += dat[np];
04402                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04403                         for (j=2; j<numr3i; j=j+2) {
04404                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04405                                 cs = cos(arg);
04406                                 si = sin(arg);
04407                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04408                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04409                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04410                         }
04411                 }
04412         }
04413         avep->update();
04414         EXITFUNC;
04415 }
04416 
04417 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04418         int nring = numr.size()/3;
04419         float *ave = avep->get_data();
04420         float *dat = datp->get_data();
04421         int i, j, numr3i, np;
04422         float  arg, cs, si;
04423         int maxrin = numr(3,nring);
04424         if(mirror == 1) { //for mirrored data has to be conjugated
04425                 for (i=1; i<=nring; i++) {
04426                         numr3i = numr(3,i);
04427                         np     = numr(2,i)-1;
04428                         ave[np]   -= dat[np];
04429                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04430                         for (j=2; j<numr3i; j=j+2) {
04431                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04432                                 cs = cos(arg);
04433                                 si = sin(arg);
04434                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04435                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04436                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04437                         }
04438                 }
04439         } else {
04440                 for (i=1; i<=nring; i++) {
04441                         numr3i = numr(3,i);
04442                         np     = numr(2,i)-1;
04443                         ave[np]   -= dat[np];
04444                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04445                         for (j=2; j<numr3i; j=j+2) {
04446                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04447                                 cs = cos(arg);
04448                                 si = sin(arg);
04449                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04450                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04451                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04452                         }
04453                 }
04454         }
04455         avep->update();
04456         EXITFUNC;
04457 }
04458 
04459 
04460 #undef    QUADPI
04461 #undef    PI2
04462 
04463 #undef  numr
04464 #undef  circ
04465 
04466 
04467 #define QUADPI   3.141592653589793238462643383279502884197
04468 #define PI2      QUADPI*2
04469 #define deg_rad  QUADPI/180.0
04470 #define rad_deg  180.0/QUADPI
04471 
04472 struct ori_t
04473 {
04474     int iphi;
04475     int itht;
04476     int id;
04477 };
04478 
04479 
04480 struct cmpang
04481 {
04482     bool operator()( const ori_t& a, const ori_t& b )
04483     {
04484         if( a.itht != b.itht )
04485         {
04486             return a.itht < b.itht;
04487         }
04488 
04489         return a.iphi < b.iphi;
04490     }
04491 };
04492 
04493 
04494 vector<double> Util::cml_weights(const vector<float>& cml){
04495         static const int NBIN = 100;
04496         int nline=cml.size()/2;
04497         vector<double> weights(nline);
04498 
04499         vector<ori_t> angs(nline);
04500         for( int i=0; i < nline; ++i ) {
04501                 angs[i].iphi = int( NBIN*cml[2*i] );
04502                 angs[i].itht = int( NBIN*cml[2*i+1] );
04503                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04504                 angs[i].id = i;
04505         }
04506 
04507         //std::cout << "# of angs: " << angs.size() << std::endl;
04508 
04509         std::sort( angs.begin(), angs.end(), cmpang() );
04510 
04511         vector<float> newphi;
04512         vector<float> newtht;
04513         vector< vector<int> > indices;
04514 
04515         int curt_iphi = -1;
04516         int curt_itht = -1;
04517         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04518                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04519                         Assert( indices.size() > 0 );
04520                         indices.back().push_back(angs[i].id);
04521                 } else {
04522                         curt_iphi = angs[i].iphi;
04523                         curt_itht = angs[i].itht;
04524 
04525                         newphi.push_back( float(curt_iphi)/NBIN );
04526                         newtht.push_back( float(curt_itht)/NBIN );
04527                         indices.push_back( vector<int>(1,angs[i].id) );
04528                 }
04529         }
04530 
04531         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04532 
04533 
04534         int num_agl = newphi.size();
04535 
04536         if(num_agl>2) {
04537                 vector<double> w=Util::vrdg(newphi, newtht);
04538 
04539                 Assert( w.size()==newphi.size() );
04540                 Assert( indices.size()==newphi.size() );
04541 
04542                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04543                     /*
04544                     std::cout << "phi,tht,w,n: ";
04545                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04546                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04547                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04548                     std::cout << indices[i].size() << "(";
04549                     */
04550 
04551                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04552                             int id = indices[i][j];
04553                             weights[id] = w[i]/indices[i].size();
04554                             //std::cout << id << " ";
04555                     }
04556 
04557                     //std::cout << ")" << std::endl;
04558 
04559                 }
04560         } else {
04561                 cout<<"warning in Util.cml_weights"<<endl;
04562                 double val = PI2/float(nline);
04563                 for(int i=0; i<nline; i++)  weights[i]=val;
04564         }
04565 
04566         return weights;
04567 
04568 }
04569 
04570 /****************************************************
04571  * New code for common-lines
04572  ****************************************************/
04573 
04574 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04575 {
04576         int i;
04577         int nx=img->get_xsize();
04578         float *img_ptr  = img->get_data();
04579         float *line_ptr = line->get_data();
04580         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04581         img->update();
04582 }
04583 
04584 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04585         int j;
04586         int nx = sino->get_xsize();
04587         int i = nx * pos_line;
04588         float r1, r2;
04589         float *line_ptr = line->get_data();
04590         float *sino_ptr = sino->get_data();
04591         for (j=ilf;j<=ihf; j += 2) {
04592                 r1 = line_ptr[j];
04593                 r2 = line_ptr[j + 1];
04594                 sino_ptr[i + j - ilf] = r1;
04595                 sino_ptr[i + j - ilf + 1] = r2;
04596                 sino_ptr[i + nx * nblines + j - ilf] = r1;
04597                 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04598         }
04599         sino->update();
04600 }
04601 
04602 vector<double> Util::cml_init_rot(vector<float> Ori){
04603         int nb_ori = Ori.size() / 4;
04604         int i, ind;
04605         float ph, th, ps;
04606         double cph, cth, cps, sph, sth, sps;
04607         vector<double> Rot(nb_ori*9);
04608         for (i=0; i<nb_ori; ++i){
04609                 ind = i*4;
04610                 // spider convention phi=psi-90, psi=phi+90
04611                 ph = Ori[ind+2]-90;
04612                 th = Ori[ind+1];
04613                 ps = Ori[ind]+90;
04614                 ph *= deg_rad;
04615                 th *= deg_rad;
04616                 ps *= deg_rad;
04617                 // pre-calculate some trigo stuffs
04618                 cph = cos(ph);
04619                 cth = cos(th);
04620                 cps = cos(ps);
04621                 sph = sin(ph);
04622                 sth = sin(th);
04623                 sps = sin(ps);
04624                 // fill rotation matrix
04625                 ind = i*9;
04626                 Rot[ind] = cph*cps-cth*sps*sph;
04627                 Rot[ind+1] = cph*sps+cth*cps*sph;
04628                 Rot[ind+2] = sth*sph;
04629                 Rot[ind+3] = -sph*cps-cth*sps*cph;
04630                 Rot[ind+4] = -sph*sps+cth*cps*cph;
04631                 Rot[ind+5] = sth*cph;
04632                 Rot[ind+6] = sth*sps;
04633                 Rot[ind+7] = -sth*cps;
04634                 Rot[ind+8] = cth;
04635         }
04636 
04637         return Rot;
04638 }
04639 
04640 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04641         float ph, ps;
04642         double cph, cth, cps, sph, sth, sps;
04643         int ind = iprj*9;
04644         // spider convention phi=psi-90, psi=phi+90
04645         ph = nps-90;
04646         ps = nph+90;
04647         ph *= deg_rad;
04648         th *= deg_rad;
04649         ps *= deg_rad;
04650         // pre-calculate some trigo stuffs
04651         cph = cos(ph);
04652         cth = cos(th);
04653         cps = cos(ps);
04654         sph = sin(ph);
04655         sth = sin(th);
04656         sps = sin(ps);
04657         // fill rotation matrix
04658         Rot[ind] = (float)(cph*cps-cth*sps*sph);
04659         Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04660         Rot[ind+2] = (float)(sth*sph);
04661         Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04662         Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04663         Rot[ind+5] = (float)(sth*cph);
04664         Rot[ind+6] = (float)(sth*sps);
04665         Rot[ind+7] = (float)(-sth*cps);
04666         Rot[ind+8] = (float)(cth);
04667 
04668         return Rot;
04669 }
04670 
04671 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04672         vector<int> com(2*(n_prj - 1));
04673         int a = i_prj*9;
04674         int i, b, c;
04675         int n1=0, n2=0;
04676         float vmax = 1 - 1.0e-6f;
04677         double r11, r12, r13, r23, r31, r32, r33;
04678 
04679         c = 0;
04680         for (i=0; i<n_prj; ++i){
04681                 if (i!=i_prj){
04682                         b = i*9;
04683                         // this is equivalent to R = A*B'
04684                         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04685                         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04686                         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04687                         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04688                         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04689                         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04690                         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04691                         if (r33 > vmax) {
04692                             n2 = 270;
04693                             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04694                         }
04695                         else if (r33 < -vmax) {
04696                             n2 = 270;
04697                             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04698                         } else {
04699                             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04700                             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04701                             if (n1 < 0) {n1 += 360;}
04702                             if (n2 <= 0) {n2 = abs(n2);}
04703                             else {n2 = 360 - n2;}
04704                         }
04705 
04706                         if (n1 >= 360){n1 = n1 % 360;}
04707                         if (n2 >= 360){n2 = n2 % 360;}
04708 
04709                         // store common-lines
04710                         b = c*2;
04711                         com[b] = n1;
04712                         com[b+1] = n2;
04713                         ++c;
04714                 }
04715         }
04716 
04717     return com;
04718 
04719 }
04720 
04721 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04722         vector<int> com(2*n_lines);
04723         int a=0, b, c, l;
04724         int n1=0, n2=0, mem=-1;
04725         float vmax = 1 - 1.0e-6f;
04726         double r11, r12, r13, r23, r31, r32, r33;
04727         c = 0;
04728         for (l=0; l<n_lines; ++l){
04729                 c = 2*l;
04730                 if (seq[c]!=mem){
04731                     mem = seq[c];
04732                     a = seq[c]*9;
04733                 }
04734                 b = seq[c+1]*9;
04735 
04736                 // this is equivalent to R = A*B'
04737                 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04738                 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04739                 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04740                 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04741                 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04742                 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04743                 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04744                 if (r33 > vmax) {
04745                     n2 = 270;
04746                     n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04747                 }
04748                 else if (r33 < -vmax) {
04749                     n2 = 270;
04750                     n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04751                 } else {
04752                     n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04753                     n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04754                     if (n1 < 0) {n1 += 360;}
04755                     if (n2 <= 0) {n2 = abs(n2);}
04756                     else {n2 = 360 - n2;}
04757                 }
04758                 if (n1 >= 360){n1 = n1 % 360;}
04759                 if (n2 >= 360){n2 = n2 % 360;}
04760 
04761                 // store common-lines
04762                 com[c] = n1;
04763                 com[c+1] = n2;
04764         }
04765 
04766         return com;
04767 
04768 }
04769 
04770 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04771         // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04772         vector<double> cml(2*nlines); // [phi, theta] / line
04773         float ph1, th1;
04774         float ph2, th2;
04775         double nx, ny, nz;
04776         double norm;
04777         double sth1=0, sph1=0, cth1=0, cph1=0;
04778         double sth2, sph2, cth2, cph2;
04779         int l, ind, c;
04780         int mem = -1;
04781         for (l=0; l<nlines; ++l){
04782                 c = 2*l;
04783                 if (seq[c]!=mem){
04784                         mem = seq[c];
04785                         ind = 4*seq[c];
04786                         ph1 = Ori[ind]*deg_rad;
04787                         th1 = Ori[ind+1]*deg_rad;
04788                         sth1 = sin(th1);
04789                         sph1 = sin(ph1);
04790                         cth1 = cos(th1);
04791                         cph1 = cos(ph1);
04792                 }
04793                 ind = 4*seq[c+1];
04794                 ph2 = Ori[ind]*deg_rad;
04795                 th2 = Ori[ind+1]*deg_rad;
04796                 sth2 = sin(th2);
04797                 cth2 = cos(th2);
04798                 sph2 = sin(ph2);
04799                 cph2 = cos(ph2);
04800                 // cross product
04801                 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04802                 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04803                 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04804                 norm = sqrt(nx*nx+ny*ny+nz*nz);
04805                 nx /= norm;
04806                 ny /= norm;
04807                 nz /= norm;
04808                 // apply mirror if need
04809                 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04810                 // compute theta and phi
04811                 cml[c+1] = acos(nz);
04812                 if (cml[c+1] == 0) {cml[c] = 0;}
04813                 else {
04814                         cml[c+1] *= rad_deg;
04815                         if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04816                         cml[c] = rad_deg * atan2(nx, ny);
04817                         cml[c] = fmod(360 + cml[c], 360);
04818 
04819                 }
04820         }
04821 
04822         return cml;
04823 }
04824 
04825 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04826         double res = 0;
04827         double buf = 0;
04828         float* line_1;
04829         float* line_2;
04830         int i, n, ind;
04831         int lnlen = data[0]->get_xsize();
04832         for (n=0; n<n_lines; ++n) {
04833                 ind = n*2;
04834                 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04835                 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04836                 buf = 0;
04837                 for (i=0; i<lnlen; ++i) {
04838                     buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04839                 }
04840                 res += buf * weights[n];
04841         }
04842 
04843         return res;
04844 
04845 }
04846 
04847 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04848                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04849         // res: [best_disc, best_ipsi]
04850         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04851         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04852         vector<double> res(2);
04853         int lnlen = data[0]->get_xsize();
04854         int end = 2*(n_prj-1);
04855         double disc, buf, bdisc, tmp;
04856         int n, i, ipsi, ind, bipsi, c;
04857         float* line_1;
04858         float* line_2;
04859         bdisc = 1.0e6;
04860         bipsi = -1;
04861         // loop psi
04862         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04863                 // discrepancy
04864                 disc = 0;
04865                 c = 0;
04866                 for (n=0; n<n_prj; ++n) {
04867                         if(n!=iprj) {
04868                                 ind = 2*c;
04869                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04870                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04871                                 buf = 0;
04872                                 for (i=0; i<lnlen; ++i) {
04873                                         tmp = line_1[i]-line_2[i];
04874                                         buf += tmp*tmp;
04875                                 }
04876                                 disc += buf * weights[iw[c]];
04877                                 ++c;
04878                         }
04879                 }
04880                 // select the best value
04881                 if (disc <= bdisc) {
04882                         bdisc = disc;
04883                         bipsi = ipsi;
04884                 }
04885                 // update common-lines
04886                 for (i=0; i<end; i+=2){
04887                         com[i] += d_psi;
04888                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
04889                 }
04890         }
04891         res[0] = bdisc;
04892         res[1] = float(bipsi);
04893 
04894         return res;
04895 }
04896 
04897 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
04898                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04899         // res: [best_disc, best_ipsi]
04900         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04901         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04902         vector<double> res(2);
04903         int lnlen = data[0]->get_xsize();
04904         int end = 2*(n_prj-1);
04905         double disc, buf, bdisc, tmp;
04906         int n, i, ipsi, ind, bipsi, c;
04907         float* line_1;
04908         float* line_2;
04909         bdisc = 1.0e6;
04910         bipsi = -1;
04911         // loop psi
04912         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04913                 // discrepancy
04914                 disc = 0;
04915                 c = 0;
04916                 for (n=0; n<n_prj; ++n) {
04917                         if(n!=iprj) {
04918                                 ind = 2*c;
04919                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04920                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04921                                 buf = 0;
04922                                 for (i=0; i<lnlen; ++i) {
04923                                         tmp = line_1[i]-line_2[i];
04924                                         buf += tmp*tmp;
04925                                 }
04926                                 disc += buf;
04927                                 ++c;
04928                         }
04929                 }
04930                 // select the best value
04931                 if (disc <= bdisc) {
04932                         bdisc = disc;
04933                         bipsi = ipsi;
04934                 }
04935                 // update common-lines
04936                 for (i=0; i<end; i+=2){
04937                         com[i] += d_psi;
04938                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
04939                 }
04940         }
04941         res[0] = bdisc;
04942         res[1] = float(bipsi);
04943 
04944         return res;
04945 }
04946 
04947 #undef  QUADPI
04948 #undef  PI2
04949 #undef  deg_rad
04950 #undef  rad_deg
04951 
04952 /****************************************************
04953  * END OF NEW CODE FOR COMMON-LINES
04954  ****************************************************/
04955 
04956 // helper function for k-means
04957 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04958         ENTERFUNC;
04959 
04960         int nima = data.size();
04961         vector<float> res(nima);
04962         double result = 0.;
04963         double valmin = 1.0e20;
04964         int valpos = -1;
04965 
04966         for (int kk=0; kk<nima; kk++){
04967         result = 0;
04968 
04969         float *y_data = data[kk]->get_data();
04970         float *x_data = image->get_data();
04971         long totsize = image->get_xsize()*image->get_ysize();
04972         for (long i = 0; i < totsize; i++) {
04973             double temp = x_data[i]- y_data[i];
04974             result += temp*temp;
04975         }
04976         result /= totsize;
04977         res[kk] = (float)result;
04978 
04979         if(result<valmin) {valmin = result; valpos = kk;}
04980 
04981         }
04982 
04983         Dict retvals;
04984         retvals["dist"] = res;
04985         retvals["pos"]  = valpos;
04986 
04987         EXITFUNC;
04988         return retvals;
04989 
04990 }
04991 
04992 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
04993         ENTERFUNC;
04994 
04995         int nima = data.size();
04996         vector<float> res(nima);
04997         double result = 0.;
04998         double valmin = 1.0e20;
04999         int valpos = -1;
05000 
05001         for (int kk=0; kk<nima; kk++){
05002         result = 0;
05003         //validate_input_args(image, data[kk]);
05004 
05005         float *y_data = data[kk]->get_data();
05006         float *x_data = image->get_data();
05007 
05008         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05009         int nx  = data[kk]->get_xsize();
05010         int ny  = data[kk]->get_ysize();
05011         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05012         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05013 
05014         int ixb = 2*((nx+1)%2);
05015         int iyb = ny%2;
05016         int iz = 0;
05017 
05018         for ( int iy = 0; iy <= ny-1; iy++) {
05019             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05020                 int ii = ix + (iy  + iz * ny)* lsd2;
05021                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05022             }
05023         }
05024         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05025             int ii = (iy  + iz * ny)* lsd2;
05026             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05027             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05028         }
05029         if(nx%2 == 0) {
05030             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05031                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05032                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05033                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05034             }
05035 
05036         }
05037         result *= 2;
05038         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05039         if(ny%2 == 0) {
05040             int ii = (ny/2  + iz * ny)* lsd2;
05041             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05042         }
05043         if(nx%2 == 0) {
05044             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05045             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05046             if(ny%2 == 0) {
05047                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05048                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05049             }
05050         }
05051 
05052         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05053         res[kk] = (float)result;
05054 
05055         if(result<valmin) {valmin = result; valpos = kk;}
05056 
05057         }
05058 
05059         Dict retvals;
05060         retvals["dist"] = res;
05061         retvals["pos"]  = valpos;
05062 
05063         EXITFUNC;
05064         return retvals;
05065 }
05066 
05067 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05068     long int d2 = group2[s2 - 1] - group2[0];
05069     long int p2 = 0;
05070     long int i1 = 0;
05071     long int i2 = 0;
05072     long int max = 0;
05073     long int cont = 0;
05074     long int i = 0;
05075     int stop1 = 0;
05076     int stop2 = 0;
05077 
05078     for (i=0; i<s1; i++) {
05079         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05080         if (p2 >= s2) {p2 = s2 - 1;}
05081         i1 = p2;
05082         i2 = p2;
05083         max = s2;
05084         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05085 
05086         stop1 = 0;
05087         stop2 = 0;
05088         while (max--) {
05089             if (group1[i] == group2[i1]) {
05090                 if (flag) {stb[cont] = group1[i];}
05091                 cont++;
05092                 break;
05093             }
05094             if (group2[i1] < group1[i]) {stop1=1;}
05095             if (group1[i] == group2[i2]) {
05096                 if (flag) {stb[cont] = group1[i];}
05097                 cont++;
05098                 break;
05099             }
05100             if (group2[i2] > group1[i]) {stop2=1;}
05101             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05102 
05103             if (stop1 & stop2) {break;}
05104             i1--;
05105             i2++;
05106             if (i1 < 0) {i1 = 0;}
05107             if (i2 >= s2) {i2 = s2 - 1;}
05108         }
05109         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05110     }
05111 
05112     return cont;
05113 }
05114 
05115 
05116 
05117 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*(size_t)nx]
05118 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05119 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05120 {
05121         /* Exception Handle */
05122         if (!img) {
05123                 throw NullPointerException("NULL input image");
05124         }
05125         /* ============================== */
05126 
05127         // Get the size of the input image
05128         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05129         /* ============================== */
05130 
05131 
05132         /* Exception Handle */
05133         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)
05134         {
05135                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05136                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05137         }
05138         /* ============================== */
05139 
05140 
05141         /*    Calculation of the start point */
05142         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05143         /* ============================*/
05144 
05145 
05146         /* Calculation of the size of the decimated image */
05147         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05148         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05149         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05150         if(r1>1){r1=1;}
05151         if(r2>1){r2=1;}
05152         if(r3>1){r3=1;}
05153         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05154         /* ===========================================*/
05155 
05156 
05157         EMData* img2 = new EMData();
05158         img2->set_size(new_nx,new_ny,new_nz);
05159         float *new_ptr = img2->get_data();
05160         float *old_ptr = img->get_data();
05161         int iptr, jptr, kptr = 0;
05162         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05163                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05164                         for (int i=new_st_x; i<nx; i+=x_step) {
05165                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05166                         iptr++;}
05167                 jptr++;}
05168         kptr++;}
05169         img2->update();
05170         return img2;
05171 }
05172 #undef old_ptr
05173 #undef new_ptr
05174 
05175 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05176 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05177 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05178 {
05179         /* Exception Handle */
05180         if (!img) throw NullPointerException("NULL input image");
05181         /* ============================== */
05182 
05183         // Get the size of the input image
05184         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05185         /* ============================== */
05186 
05187         /* Exception Handle */
05188         if(new_nx>nx || new_ny>ny || new_nz>nz)
05189                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05190         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)
05191                 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05192         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))))
05193                 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05194         /* ============================== */
05195 
05196         /*    Calculation of the start point */
05197         int  new_st_x = nx/2-new_nx/2 + x_offset,
05198              new_st_y = ny/2-new_ny/2 + y_offset,
05199              new_st_z = nz/2-new_nz/2 + z_offset;
05200         /* ============================== */
05201 
05202         /* Exception Handle */
05203         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05204                 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05205         /* ============================== */
05206 
05207         EMData* wind = img->copy_empty_head();
05208         wind->set_size(new_nx, new_ny, new_nz);
05209         float *outp=wind->get_data();
05210         float *inp=img->get_data();
05211 
05212         for (int k=0; k<new_nz; k++)
05213                 for(int j=0; j<new_ny; j++)
05214                         for(int i=0; i<new_nx; i++)
05215                                 outp(i,j,k) = inp(i,j,k);
05216         wind->update();
05217         return wind;
05218 }
05219 #undef inp
05220 #undef outp
05221 
05222 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05223 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05224 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset, const char *params)
05225 {
05226         /* Exception Handle */
05227         if (!img)  throw NullPointerException("NULL input image");
05228         /* ============================== */
05229 
05230         // Get the size of the input image
05231         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05232         /* ============================== */
05233 
05234         /* Exception Handle */
05235         if(new_nx<nx || new_ny<ny || new_nz<nz)
05236                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05237         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)
05238                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05239         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))))
05240                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05241         /* ============================== */
05242 
05243         EMData* pading = img->copy_head();
05244         pading->set_size(new_nx, new_ny, new_nz);
05245         float *inp  = img->get_data();
05246         float *outp = pading->get_data();
05247 
05248 
05249         /* Calculation of the average and the circumference values for background substitution
05250         =======================================================================================*/
05251         float background;
05252 
05253         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05254         else if (strcmp(params,"circumference")==0) {
05255                 float sum1=0.0f;
05256                 size_t cnt=0;
05257                 for(int i=0;i<nx;i++) {
05258                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05259                         cnt+=2;
05260                 }
05261                 if(nz-1 == 0) {
05262                         for (int j=1;j<ny-1;j++) {
05263                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05264                                 cnt+=2;
05265                         }
05266                 } else {
05267                         for (int k=1;k<nz-1;k++) {
05268                                 for (int j=1;j<ny-1;j++) {
05269                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05270                                         cnt+=2;
05271                                 }
05272                         }
05273                 }
05274                 background = sum1/cnt;
05275         } else {
05276                 background = static_cast<float>( atof( params ) );
05277         }
05278         /*=====================================================================================*/
05279 
05280          /*Initial Padding */
05281         int new_st_x=0,new_st_y=0,new_st_z=0;
05282         for (int k=0;k<new_nz;k++)
05283                 for(int j=0;j<new_ny;j++)
05284                         for (int i=0;i<new_nx;i++)
05285                                 outp(i,j,k)=background;
05286         /*============================== */
05287 
05288         /*    Calculation of the start point */
05289         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05290         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05291         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05292         /* ============================== */
05293 
05294         for (int k=0;k<nz;k++)
05295                 for(int j=0;j<ny;j++)
05296                         for(int i=0;i<nx;i++)
05297                                 outp(i,j,k)=inp(i,j,k);
05298         pading->update();
05299         return pading;
05300 }
05301 #undef inp
05302 #undef outp
05303 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05304 
05305 void Util::colreverse(float* beg, float* end, int nx) {
05306         float* tmp = new float[nx];
05307         int n = (end - beg)/nx;
05308         int nhalf = n/2;
05309         for (int i = 0; i < nhalf; i++) {
05310                 // swap col i and col n-1-i
05311                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05312                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05313                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05314         }
05315         delete[] tmp;
05316 }
05317 
05318 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05319 {
05320         int nxy = nx*ny;
05321         colreverse(beg, end, nxy);
05322 }
05323 
05324 
05325 void Util::cyclicshift(EMData *image, Dict params) {
05326 
05327         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05328 
05329         int dx = params["dx"];
05330         int dy = params["dy"];
05331         int dz = params["dz"];
05332 
05333         // The reverse trick we're using shifts to the left (a negative shift)
05334         int nx = image->get_xsize();
05335         dx %= nx;
05336         if (dx < 0) dx += nx;
05337         int ny = image->get_ysize();
05338         dy %= ny;
05339         if (dy < 0) dy += ny;
05340         int nz = image->get_zsize();
05341         dz %= nz;
05342         if (dz < 0) dz += nz;
05343 
05344         int mx = -(dx - nx);
05345         int my = -(dy - ny);
05346         int mz = -(dz - nz);
05347 
05348         float* data = image->get_data();
05349         // x-reverses
05350         if (mx != 0) {
05351                 for (int iz = 0; iz < nz; iz++)
05352                        for (int iy = 0; iy < ny; iy++) {
05353                                 // reverses for column iy
05354                                 size_t offset = nx*iy + (size_t)nx*ny*iz; // starting location for column iy in slice iz
05355                                 reverse(&data[offset],&data[offset+mx]);
05356                                 reverse(&data[offset+mx],&data[offset+nx]);
05357                                 reverse(&data[offset],&data[offset+nx]);
05358                         }
05359         }
05360         // y-reverses
05361         if (my != 0) {
05362                 for (int iz = 0; iz < nz; iz++) {
05363                         size_t offset = (size_t)nx*ny*iz;
05364                         colreverse(&data[offset], &data[offset + my*nx], nx);
05365                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05366                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05367                 }
05368         }
05369         if (mz != 0) {
05370                 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05371                 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05372                 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05373         }
05374         image->update();
05375 }
05376 
05377 //-----------------------------------------------------------------------------------------------------------------------
05378 
05379 
05380 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05381 {
05382         if (image->is_complex())
05383                 throw ImageFormatException("Cannot do histogram on Fourier image");
05384         //float hmax, hmin;
05385         float *imageptr=0, *maskptr=0;
05386         int nx=image->get_xsize();
05387         int ny=image->get_ysize();
05388         int nz=image->get_zsize();
05389 
05390         if(mask != NULL){
05391                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05392                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05393                 maskptr =mask->get_data();
05394         }
05395         if( nbins == 0) nbins = nx;
05396         vector <float> freq(2*nbins, 0.0);
05397 
05398         imageptr=image->get_data();
05399         if( hmin == hmax ) {
05400                 if(mask == NULL) {
05401                         hmax = image->get_attr("maximum");
05402                         hmin = image->get_attr("minimum");
05403                 } else {
05404                         bool  First = true;
05405                         for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05406                         if (maskptr[i]>=0.5f) {
05407                                         if(First) {
05408                                                 hmax = imageptr[i];
05409                                                 hmin = imageptr[i];
05410                                                 First = false;
05411                                         } else {
05412                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05413                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05414                                         }
05415                                 }
05416                         }
05417                 }
05418         }
05419         float hdiff = hmax - hmin;
05420         float ff = (nbins-1)/hdiff;
05421         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05422         if(mask == NULL) {
05423                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05424                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05425                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05426                 }
05427         } else {
05428                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05429                         if(maskptr[i] >= 0.5) {
05430                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05431                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05432                         }
05433                 }
05434         }
05435         return freq;
05436 }
05437 
05438 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05439 {
05440         /* Exception Handle */
05441         if (img->is_complex() || ref->is_complex())
05442                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05443 
05444         if(mask != NULL){
05445                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05446                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05447         /* ===================================================== */
05448 
05449         /* Image size calculation */
05450         size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05451         size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05452         /* ===================================================== */
05453 
05454         /* The reference image attributes */
05455         float *ref_ptr = ref->get_data();
05456         float ref_h_min = ref->get_attr("minimum");
05457         float ref_h_max = ref->get_attr("maximum");
05458         float ref_h_avg = ref->get_attr("mean");
05459         float ref_h_sig = ref->get_attr("sigma");
05460         /* ===================================================== */
05461 
05462         /* Input image under mask attributes */
05463         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05464 
05465         vector<float> img_data = Util::infomask(img, mask);
05466         float img_avg = img_data[0];
05467         float img_sig = img_data[1];
05468 
05469         /* The image under mask -- size calculation */
05470         int cnt=0;
05471         for(size_t i=0;i<size_img;++i)
05472                 if (mask_ptr[i]>0.5f)
05473                                 cnt++;
05474         /* ===================================================== */
05475 
05476         /* Histogram of reference image calculation */
05477         float ref_h_diff = ref_h_max - ref_h_min;
05478 
05479         #ifdef _WIN32
05480                 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05481         #else
05482                 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05483         #endif  //_WIN32
05484 
05485         float *ref_freq_bin = new float[3*hist_len];
05486 
05487         //initialize value in each bin to zero
05488         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05489 
05490         for (size_t i = 0;i < size_ref;++i) {
05491                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05492                 ref_freq_bin[L]++;
05493         }
05494         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05495 
05496         //Parameters Calculation (i.e) 'A' x + 'B'
05497         float A = ref_h_sig/img_sig;
05498         float B = ref_h_avg - (A*img_avg);
05499 
05500         vector<float> args;
05501         args.push_back(A);
05502         args.push_back(B);
05503 
05504         vector<float> scale;
05505         scale.push_back(1.e-7f*A);
05506         scale.push_back(-1.e-7f*B);
05507 
05508         vector<float> ref_freq_hist;
05509         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05510 
05511         vector<float> data;
05512         data.push_back(ref_h_diff);
05513         data.push_back(ref_h_min);
05514 
05515         Dict parameter;
05516 
05517         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05518         parameter["args"] = args;
05519         parameter["scale"]= scale;
05520         parameter["data"] = data;
05521         parameter["ref_freq_bin"] = ref_freq_hist;
05522         parameter["size_img"]=(double)size_img;
05523         parameter["hist_len"]=hist_len;
05524         /* ===================================================== */
05525 
05526         return parameter;
05527 }
05528 
05529 
05530 float Util::hist_comp_freq(float PA,float PB,size_t size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05531 {
05532         float *img_ptr = img->get_data();
05533         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05534 
05535         int *img_freq_bin = new int[3*hist_len];
05536         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05537         for(size_t i = 0;i < size_img;++i) {
05538                 if(mask_ptr[i] > 0.5f) {
05539                         float img_xn = img_ptr[i]*PA + PB;
05540                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05541                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05542                 }
05543         }
05544         int freq_hist = 0;
05545 
05546         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);
05547         freq_hist = (-freq_hist);
05548         return static_cast<float>(freq_hist);
05549 }
05550 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05551 #define    QUADPI                       3.141592653589793238462643383279502884197
05552 #define    DGR_TO_RAD                   QUADPI/180
05553 #define    DM(I)                        DM          [I-1]
05554 #define    SS(I)                        SS          [I-1]
05555 Dict Util::CANG(float PHI,float THETA,float PSI)
05556 {
05557         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05558         vector<float>   DM,SS;
05559 
05560         for(int i =0;i<9;i++) DM.push_back(0);
05561 
05562         for(int i =0;i<6;i++) SS.push_back(0);
05563 
05564         CPHI = cos(double(PHI)*DGR_TO_RAD);
05565         SPHI = sin(double(PHI)*DGR_TO_RAD);
05566         CTHE = cos(double(THETA)*DGR_TO_RAD);
05567         STHE = sin(double(THETA)*DGR_TO_RAD);
05568         CPSI = cos(double(PSI)*DGR_TO_RAD);
05569         SPSI = sin(double(PSI)*DGR_TO_RAD);
05570 
05571         SS(1) = float(CPHI);
05572         SS(2) = float(SPHI);
05573         SS(3) = float(CTHE);
05574         SS(4) = float(STHE);
05575         SS(5) = float(CPSI);
05576         SS(6) = float(SPSI);
05577 
05578         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05579         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05580         DM(3) = float(-STHE*CPSI);
05581         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05582         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05583         DM(6) = float(STHE*SPSI);
05584         DM(7) = float(STHE*CPHI);
05585         DM(8) = float(STHE*SPHI);
05586         DM(9) = float(CTHE);
05587 
05588         Dict DMnSS;
05589         DMnSS["DM"] = DM;
05590         DMnSS["SS"] = SS;
05591 
05592         return(DMnSS);
05593 }
05594 #undef SS
05595 #undef DM
05596 #undef QUADPI
05597 #undef DGR_TO_RAD
05598 //-----------------------------------------------------------------------------------------------------------------------
05599 struct t_BPCQ_line{
05600         int rX;     // radius along X axe
05601         int offset; // offset of voxel in volume - beginning of the line
05602         float xbb;  // XBB coefficient
05603         float ybb;  // YBB coefficient
05604 };
05605 
05606 void Util::BPCQ( EMData *B, EMData *CUBE, const int radius )
05607 {
05608         if (B->is_complex()) {
05609                 B->do_ift_inplace();
05610                 B->depad();
05611         }
05612 
05613         const Transform * transform = B->get_attr("xform.projection");
05614         Dict transform_params = transform->get_params("spider");
05615 
05616         // ---- build DM matrix (transform matrix) - convert from 3x4 matrix to 2x3 matrix (only 2 first rows are nedeed)
05617         std::vector<float> DM = transform->get_matrix();
05618         DM[3+0] = DM[4+0];
05619         DM[3+1] = DM[4+1];
05620         DM[3+2] = DM[4+2];
05621 
05622         delete transform;
05623 
05624         const int NSAM = B->get_xsize();
05625         const int NROW = B->get_ysize();
05626 
05627         // buffer "lines_to_process" should be aligned to size of cache line (usually 64 or 128 bytes)
05628         t_BPCQ_line * lines_to_process;
05629 #ifdef _WIN32
05630         if ( (lines_to_process = (t_BPCQ_line *)_aligned_malloc( 4*radius*radius*sizeof(t_BPCQ_line), 256 )) == NULL )
05631 #else
05632         if ( posix_memalign( reinterpret_cast<void**>(&lines_to_process), 256, 4*radius*radius*sizeof(t_BPCQ_line) ) != 0 )
05633 #endif  //_WIN32
05634         {
05635                 throw std::bad_alloc();
05636         }
05637         t_BPCQ_line * first_free_line = lines_to_process;
05638 
05639         // calculate lines parameters
05640         {
05641                 //  Unsure about sign of shifts, check later PAP 06/28/09
05642                 const float x_shift_plus_center = float(NSAM/2 +1) + float(transform_params[ "tx" ]);
05643                 const float y_shift_plus_center = float(NROW/2 +1) + float(transform_params[ "ty" ]);
05644 
05645                 const int sizeX = CUBE->get_xsize();
05646                 const int sizeY = CUBE->get_ysize();
05647                 const int sizeZ = CUBE->get_zsize();
05648 
05649                 const int centerX = sizeX / 2;
05650                 const int centerY = sizeY / 2;
05651                 const int centerZ = sizeZ / 2;
05652 
05653                 const int minRZ = (centerZ >= radius) ? (-radius) : (-centerZ);
05654                 const int maxRZ = (sizeZ > centerZ+radius) ? (radius) : (sizeZ-centerZ-1);
05655 
05656                 for ( int rZ=minRZ; rZ<=maxRZ; ++rZ ) {
05657                         for ( int rY=-radius; rY<=radius; ++rY ) {
05658                                 const int sqRX = radius*radius - rZ*rZ - rY*rY;
05659                                 if (sqRX >= 0) {
05660 #ifdef  _WIN32
05661                                         first_free_line->rX     = static_cast<int>( floor(sqrtf(sqRX)+0.5) );
05662 #else
05663                                         first_free_line->rX     = static_cast<int>( roundf(sqrtf(sqRX)) );
05664 #endif  //_WIN32
05665                                         first_free_line->offset = sizeX*( centerY+rY + sizeY*(centerZ+rZ) ) + centerX - first_free_line->rX;
05666                                         first_free_line->xbb    = rZ*DM[2] + rY*DM[1] + x_shift_plus_center;
05667                                         first_free_line->ybb    = rZ*DM[5] + rY*DM[4] + y_shift_plus_center;
05668                                         ++first_free_line;
05669                                 }
05670                         }
05671                 }
05672         }
05673 
05674         const float * const Bptr = B->get_data();
05675         float * const CUBE_begin = CUBE->get_data();
05676 
05677         // update voxels in volume
05678         // this loop takes more than 95% of calculations time spent in Util::BPCQ function
05679         for ( t_BPCQ_line * iLine = lines_to_process; iLine < first_free_line; ++iLine ) {
05680                 const int rX_first = -(iLine->rX);
05681                 const int rX_last  =   iLine->rX;
05682                 float  *CUBE_ptr = CUBE_begin + iLine->offset;
05683                 for (int rX=rX_first; rX<=rX_last; ++rX, ++CUBE_ptr) {
05684                         const float XB  = rX * DM[0] + iLine->xbb;
05685                         const float YB  = rX * DM[3] + iLine->ybb;
05686                         const int IQX = int(XB);
05687                         const int IQY = int(YB);
05688                         if ( IQX < 1 || IQX >= NSAM || IQY < 1 || IQY >= NROW )  continue;
05689                         const float DIPX = XB-IQX;
05690                         const float DIPY = YB-IQY;
05691                         const float b00 = Bptr[IQX-1+((IQY-1)*NSAM)];
05692                         const float b01 = Bptr[IQX-1+((IQY-0)*NSAM)];
05693                         const float b10 = Bptr[IQX-0+((IQY-1)*NSAM)];
05694                         const float b11 = Bptr[IQX-0+((IQY-0)*NSAM)];
05695                         *(CUBE_ptr) = *(CUBE_ptr) + b00 + DIPY*(b01-b00) + DIPX*(b10-b00+DIPY*(b11-b10-b01+b00));
05696                 }
05697         }
05698 
05699         free(lines_to_process);
05700 }
05701 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05702 #define    W(i,j)                       Wptr    [i+(j)*Wnx]
05703 #define    PROJ(i,j)            PROJptr [i+(j)*NNNN]
05704 #define    SS(i,j)              SS          [i+(j)*6]
05705 
05706 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05707 {
05708 
05709         --K; // now indexes are started from 0
05710 
05711         int NSAM = PROJ->get_xsize();
05712         int NROW = PROJ->get_ysize();
05713 
05714         if (PROJ->is_fftpadded()) {
05715                 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);  // correction for DFT image
05716         }
05717 
05718     const int ntotal = NSAM*NROW;
05719         const float q = 2.0f;
05720         const float qt = 8.0f/q;
05721         //  Fix for padding 2x
05722         const int ipad = 1;
05723         NSAM *= ipad;
05724         NROW *= ipad;
05725         const int NNNN = NSAM+2-(NSAM%2);
05726         const int NX2 = NSAM/2;
05727         const int NR2  = NROW/2;
05728 
05729         const int NANG = int(SS.size())/6;
05730 
05731         EMData* W = new EMData();
05732         const int Wnx = NNNN/2;
05733         W->set_size(Wnx,NROW,1);
05734         W->to_zero();
05735         float *Wptr = W->get_data();
05736 
05737         for (int L=0; L<NANG; L++) {
05738                 const float tmp1 = SS(2,K)*SS(3,L)*(SS(0,K)*SS(0,L) + SS(1,K)*SS(1,L)) - SS(2,L)*SS(3,K);
05739                 const float tmp2 = SS(3,L)*( SS(0,K)*SS(1,L) - SS(0,L)*SS(1,K) );
05740                 float OX = SS(5,K)*tmp2 + SS(4,K)*tmp1;
05741                 float OY = SS(4,K)*tmp2 - SS(5,K)*tmp1;
05742                 if(OX < 0.0f) {
05743                         OX = -OX;
05744                         OY = -OY;
05745                 }
05746 
05747                 if( OX > 1.0e-6f || fabs(OY) > 1.0e-6f ) {
05748                         for (int J=0; J<NROW; ++J) {
05749                                 const float JY_OY = (J > NR2) ? ((J-NROW)*OY) : (J*OY);
05750                                 int xma = NX2;
05751                                 int xmi = 0;
05752                                 const float fxma = ( q-JY_OY) / OX;
05753                                 const float fxmi = (-q-JY_OY) / OX;
05754                                 if (fxma < xmi || fxmi > xma ) {
05755                                         continue;
05756                                 }
05757                                 if (fxma < xma) {
05758                                         xma = static_cast<int>(fxma+0.5f);
05759                                 }
05760                                 if (fxmi > xmi) {
05761                                         xmi = static_cast<int>(fxmi+0.5f);
05762                                 }
05763                                 for( int I=xmi; I<=xma; ++I ) {
05764                                         const float Y = I*OX + JY_OY;
05765                                         W(I,J) += exp(-qt*Y*Y);
05766                                 }
05767                         }
05768                 } else {
05769                         for (int J=0; J<NROW; ++J) {
05770                                 for (int I=0; I<NNNN/2; ++I) {
05771                                         W(I,J) += 1.0f;
05772                                 }
05773                         }
05774                 }
05775         }
05776 
05777     EMData* proj_in = PROJ;
05778 
05779         const bool realOnInput = PROJ->is_real();
05780     if (realOnInput) {
05781                 // copy input image and run DFT on it
05782                 PROJ = PROJ->norm_pad( false, ipad);
05783                 PROJ->do_fft_inplace();
05784                 PROJ->update();
05785     }
05786     float * PROJptr = PROJ->get_data();
05787 
05788         const float osnr = 1.0f/SNR;
05789         const float WNRMinv = 1.0f/W(0,0);
05790         for (int J=0; J<NROW; ++J)  {
05791                 float sy = (J > NR2) ? (J - NROW) : (J);
05792                 sy /= NROW;
05793                 sy *= sy;
05794                 for (int I=0; I<NNNN; I+=2) {
05795                         const int KX = I/2;
05796                         const float temp = W(KX,J)*WNRMinv;
05797                         float WW = temp/(temp*temp + osnr);
05798                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
05799                         const float sx = float(KX) / NSAM;
05800                         WW *= exp(qt*(sy + sx*sx));
05801                         PROJ(I,J)   *= WW;
05802                         PROJ(I+1,J) *= WW;
05803                 }
05804         }
05805         delete W; W = 0;
05806 
05807         PROJ->do_ift_inplace();
05808         PROJ->depad();
05809 
05810         if (realOnInput) {
05811                 // copy data back to input image
05812                 float* data_src = PROJ->get_data();
05813                 float* data_dst = proj_in->get_data();
05814                 memcpy( data_dst, data_src, ntotal * sizeof(float) );
05815                 delete PROJ;
05816         }
05817 
05818         proj_in->update();
05819 }
05820 /*
05821 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05822 {
05823         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05824         float WW,OX,OY,Y;
05825 
05826         NSAM = PROJ->get_xsize();
05827         NROW = PROJ->get_ysize();
05828         //  Fix for padding 2x
05829         int ntotal = NSAM*NROW;
05830         int ipad = 1;
05831         NSAM *= ipad;
05832         NROW *= ipad;
05833         NNNN = NSAM+2-(NSAM%2);
05834         NR2  = NROW/2;
05835 
05836         NANG = int(SS.size())/6;
05837 
05838         EMData* W = new EMData();
05839         int Wnx = NNNN/2;
05840         W->set_size(Wnx,NROW,1);
05841         W->to_zero();
05842         float *Wptr = W->get_data();
05843         float *PROJptr = PROJ->get_data();
05844         for (L=1; L<=NANG; L++) {
05845                 float  tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05846                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05847                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05848                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05849         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
05850 
05851                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
05852                         for(int J=1;J<=NROW;J++) {
05853                                 JY = (J-1);
05854                                 if(JY > NR2) JY=JY-NROW;
05855                                 for(int I=1;I<=NNNN/2;I++) {
05856                                         Y =  fabs(OX * (I-1) + OY * JY);
05857                                         if(Y < 2.0f) {
05858                                         W(I,J) += exp(-4*Y*Y);
05859         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
05860                                 }
05861                         }
05862                 } else {
05863                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05864                 }
05865         }
05866         EMData* proj_in = PROJ;
05867 
05868         PROJ = PROJ->norm_pad( false, ipad);
05869         PROJ->do_fft_inplace();
05870         PROJ->update();
05871         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05872         PROJptr = PROJ->get_data();
05873 
05874         float WNRMinv,temp;
05875         float osnr = 1.0f/SNR;
05876         WNRMinv = 1.0f/W(1,1);
05877         for(int J=1;J<=NROW;J++)
05878                 for(int I=1;I<=NNNN;I+=2) {
05879                         KX           = (I+1)/2;
05880                         temp         = W(KX,J)*WNRMinv;
05881                         WW           = temp/(temp*temp + osnr);
05882                         PROJ(I,J)   *= WW;
05883                         PROJ(I+1,J) *= WW;
05884                 }
05885         delete W; W = 0;
05886         PROJ->do_ift_inplace();
05887         PROJ->depad();
05888 
05889         float* data_src = PROJ->get_data();
05890         float* data_dst = proj_in->get_data();
05891 
05892         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05893 
05894         proj_in->update();
05895 
05896         delete PROJ;
05897 }
05898 */
05899 #undef PROJ
05900 #undef W
05901 #undef SS
05902 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05903 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05904 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05905 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05906 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
05907 #define    CC(i)                        CC          [i-1]
05908 #define    CP(i)                        CP          [i-1]
05909 #define    VP(i)                        VP          [i-1]
05910 #define    VV(i)                        VV          [i-1]
05911 #define    AMAX1(i,j)                   i>j?i:j
05912 #define    AMIN1(i,j)                   i<j?i:j
05913 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05914 {
05915         float rad2deg =(180.0f/3.1415926f);
05916         float deg2rad = (3.1415926f/180.0f);
05917 
05918         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05919 
05920         NSAM = PROJ->get_xsize();
05921         NROW = PROJ->get_ysize();
05922 
05923         if (PROJ->is_fftpadded()) {
05924                 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);  // correction for DFT image
05925         }
05926 
05927         NNNN = NSAM+2-(NSAM%2);
05928         NR2  = NROW/2;
05929         NANG = int(SS.size())/6;
05930 
05931         float RI[9];
05932         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05933         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05934         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05935         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05936         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05937         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05938         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05939         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05940         RI(3,3)=SS(3,NUMP);
05941 
05942         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05943 
05944         EMData* W = new EMData();
05945         int Wnx = NNNN/2;
05946         W->set_size(NNNN/2,NROW,1);
05947         W->to_one();
05948         float *Wptr = W->get_data();
05949 
05950         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05951 
05952         for (L=1; L<=NANG; L++) {
05953                 if (L != NUMP) {
05954                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05955                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05956                         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);
05957 
05958                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
05959                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05960                         ALPHA=rad2deg*float(asin(CCN));
05961                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05962                         if (ALPHA>90.0f)  ALPHA=180.0f-ALPHA;
05963                         if(ALPHA<1.0E-6) {
05964                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05965                         } else {
05966                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05967                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
05968                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05969                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05970                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05971                                 CP(1)   = 0.0;CP(2) = 0.0;
05972                                 VP(1)   = 0.0;VP(2) = 0.0;
05973 
05974                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05975                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05976                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05977                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05978 
05979                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05980 
05981                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
05982                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
05983                                 float tmpinv = 1.0f/TMP;
05984                                 for(int J=1;J<=NROW;J++) {
05985                                         JY = (J-1);
05986                                         if (JY>NR2)  JY=JY-NROW;
05987                                         for(int I=1;I<=NNNN/2;I++) {
05988                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05989                                                 RT     = 1.0f-FV/FM;
05990                                                 W(I,J) += ((RT>0.0f)*RT);
05991                                         }
05992                                 }
05993                         }
05994                 }
05995         }
05996 
05997     EMData* proj_in = PROJ;
05998     const bool realOnInput = PROJ->is_real();
05999 
06000         if (realOnInput) {
06001                 // copy input image and run DFT on it
06002                 PROJ = PROJ->norm_pad( false, 1 );
06003                 PROJ->do_fft_inplace();
06004                 PROJ->update();
06005         }
06006         float *PROJptr = PROJ->get_data();
06007 
06008         int KX;
06009         float WW;
06010         for(int J=1; J<=NROW; J++)
06011                 for(int I=1; I<=NNNN; I+=2) {
06012                         KX          =  (I+1)/2;
06013                         WW          =  1.0f/W(KX,J);
06014                         PROJ(I,J)   = PROJ(I,J)*WW;
06015                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
06016                 }
06017         delete W; W = 0;
06018         PROJ->do_ift_inplace();
06019         PROJ->depad();
06020 
06021         if (realOnInput) {
06022                 // copy data back to input image
06023                 float* data_src = PROJ->get_data();
06024                 float* data_dst = proj_in->get_data();
06025                 int ntotal = NSAM*NROW;
06026                 memcpy( data_dst, data_src, ntotal * sizeof(float) );
06027                 delete PROJ;
06028         }
06029 
06030         proj_in->update();
06031 }
06032 #undef   AMAX1
06033 #undef   AMIN1
06034 #undef   RI
06035 #undef   CC
06036 #undef   CP
06037 #undef   VV
06038 #undef   VP
06039 #undef   W
06040 #undef   SS
06041 #undef   PROJ
06042 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
06043 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06044 {
06045         float cst  = cs*1.0e7f;
06046 
06047         wgh /= 100.0;
06048         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06049         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06050         float ak2 = ak*ak;
06051         float g1 = dzz*1.0e4f*lambda*ak2;
06052         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06053 
06054         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06055         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06056 
06057         return ctfv;
06058 }
06059 
06060 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06061 {
06062         /***********
06063         ***get the size of the image for validation purpose
06064         **************/
06065         int nx = image->get_xsize();
06066         int ny = image->get_ysize();
06067         int nz = image->get_zsize();
06068         /********
06069         ***Exception Handle
06070         *************/
06071         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06072                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06073 
06074         size_t i, size = (size_t)nx*ny*nz;
06075 
06076         float* img_ptr = image->get_data();
06077         float* mask_ptr = mask->get_data();
06078 
06079         int ln=0;  //length of the output image = number of points under the mask.
06080         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06081 
06082         EMData* new_image = new EMData();
06083         new_image->set_size(ln,1,1); /* set size of the new image */
06084         float *new_ptr    = new_image->get_data();
06085 
06086         ln=-1;
06087         for(i = 0;i < size;i++){
06088                 if(mask_ptr[i] > 0.5f) {
06089                         ln++;
06090                         new_ptr[ln]=img_ptr[i];
06091                 }
06092         }
06093 
06094         return new_image;
06095 }
06096 
06097 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06098 {
06099         /********
06100         ***Exception Handle
06101         *************/
06102         if(mask == NULL)
06103                 throw ImageDimensionException("The mask cannot be an null image");
06104 
06105         /***********
06106         ***get the size of the mask
06107         **************/
06108         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06109 
06110         size_t i,size = (size_t)nx*ny*nz;                        /* loop counters */
06111         /* new image declaration */
06112         EMData *new_image = new EMData();
06113         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06114         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06115         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06116         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06117         int count = 0;
06118         float sum_under_mask = 0.0 ;
06119         for(i = 0;i < size;i++){
06120                         if(mask_ptr[i] > 0.5f){
06121                                 new_ptr[i] = img_ptr[count];
06122                                 sum_under_mask += img_ptr[count];
06123                                 count++;
06124                                 if( count > image->get_xsize() ) {
06125                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06126                                 }
06127                         }
06128         }
06129 
06130         if( count > image->get_xsize() ) {
06131             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06132         }
06133 
06134         float avg_under_mask = sum_under_mask / count;
06135         for(i = 0;i < size;i++) {
06136                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06137         }
06138         new_image->update();
06139         return new_image;
06140 }
06141 
06142 
06143 
06144 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06145 {
06146         vector<float>new_peak;
06147         int n1=peak1.size()/3;
06148         float p_size2=p_size*p_size;
06149         for (int i=0;i<n1;++i) {
06150                 vector<float>::iterator it2= peak1.begin()+3*i;
06151                 bool push_back1=true;
06152                 int n2=peak2.size()/3;
06153                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06154                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06155                 if(n2 ==0) {
06156                         new_peak.push_back(*it2);
06157                         new_peak.push_back(*(it2+1));
06158                         new_peak.push_back(*(it2+2));
06159                 } else  {
06160                         int j=0;
06161                         while (j< n2-1 ) {
06162                                 vector<float>::iterator it3= peak2.begin()+3*j;
06163                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06164                                 if(d2< p_size2 ) {
06165                                         if( (*it2)<(*it3) ) {
06166                                                 new_peak.push_back(*it3);
06167                                                 new_peak.push_back(*(it3+1));
06168                                                 new_peak.push_back(*(it3+2));
06169                                                 peak2.erase(it3);
06170                                                 peak2.erase(it3);
06171                                                 peak2.erase(it3);
06172                                                 push_back1=false;
06173                                         } else {
06174                                                 peak2.erase(it3);
06175                                                 peak2.erase(it3);
06176                                                 peak2.erase(it3);
06177                                         }
06178                                 } else  j=j+1;
06179                                 n2=peak2.size()/3;
06180                         }
06181                         if(push_back1) {
06182                                 new_peak.push_back(*it2);
06183                                 new_peak.push_back(*(it2+1));
06184                                 new_peak.push_back(*(it2+2));
06185                         }
06186                 }
06187         }
06188         return new_peak;
06189 }
06190 
06191 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06192 {
06193         // n size of the covariance/correlation matrix
06194         // covmat --- covariance/correlation matrix (n by n)
06195         // eigval --- returns eigenvalues
06196         // eigvec --- returns eigenvectors
06197 
06198         ENTERFUNC;
06199 
06200         int i;
06201 
06202         // make a copy of covmat so that it will not be overwritten
06203         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06204 
06205         char NEEDV = 'V';
06206         char UPLO = 'U';
06207         int lwork = -1;
06208         int info = 0;
06209         float *work, wsize;
06210 
06211         //  query to get optimal workspace
06212         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06213         lwork = (int)wsize;
06214 
06215         work = (float *)calloc(lwork, sizeof(float));
06216         //  calculate eigs
06217         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06218         free(work);
06219         EXITFUNC;
06220         return info;
06221 }
06222 
06223 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06224 {
06225 
06226         ENTERFUNC;
06227         int len = covmatpy.size();
06228         float *eigvec;
06229         float *eigval;
06230         float *covmat;
06231         int status = 0;
06232         eigval = (float*)calloc(ncov,sizeof(float));
06233         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06234         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06235 
06236         const float *covmat_ptr;
06237         covmat_ptr = &covmatpy[0];
06238         for(int i=0;i<len;i++){
06239             covmat[i] = covmat_ptr[i];
06240         }
06241 
06242         status = Util::coveig(ncov, covmat, eigval, eigvec);
06243 
06244         vector<float> eigval_py(ncov);
06245         const float *eigval_ptr;
06246         eigval_ptr = &eigval[0];
06247         for(int i=0;i<ncov;i++){
06248             eigval_py[i] = eigval_ptr[i];
06249         }
06250 
06251         vector<float> eigvec_py(ncov*ncov);
06252         const float *eigvec_ptr;
06253         eigvec_ptr = &eigvec[0];
06254         for(int i=0;i<ncov*ncov;i++){
06255             eigvec_py[i] = eigvec_ptr[i];
06256         }
06257 
06258         Dict res;
06259         res["eigval"] = eigval_py;
06260         res["eigvec"] = eigvec_py;
06261 
06262         EXITFUNC;
06263         return res;
06264 }
06265 
06266 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06267 {
06268         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06269 
06270         k=(int)pw.size();
06271         l=0;
06272         m=k;
06273         n2=n+2;
06274         n1=n+1;
06275         klmd=k+l+m;
06276         klm2d= k+l+m+2;
06277         nklmd=k+l+m+n;
06278         n2d=n+2;
06279         /*size has to be increased when N is large*/
06280         n_larg=klmd*2;
06281         klm2d=n_larg+klm2d;
06282         klmd=n_larg+klmd;
06283         nklmd=n_larg+nklmd;
06284         int size_q=klm2d*n2d;
06285         int size_cu=nklmd*2;
06286         static int i__;
06287 
06288         double *q ;
06289         double *x ;
06290         double *res;
06291         double *cu;
06292         float  *q2;
06293         float  *pw_;
06294         long int *iu;
06295         double *s;
06296         q   = (double*)calloc(size_q,sizeof(double));
06297         x   = (double*)calloc(n2d,sizeof(double));
06298         res = (double*)calloc(klmd,sizeof(double));
06299         cu  = (double*)calloc(size_cu,sizeof(double));
06300         s   = (double*)calloc(klmd,sizeof(double));
06301         q2  = (float*)calloc(size_q,sizeof(float));
06302         iu  = (long int*)calloc(size_cu,sizeof(long int));
06303         pw_ = (float*)calloc(k,sizeof(float));
06304 
06305         for( i__ =0; i__<k; ++i__) pw_[i__] = log(pw[i__]);
06306         long int l_k=k;
06307         long int l_n=n;
06308         long int l_iswi=iswi;
06309         vector<float> cl1_res;
06310         cl1_res = Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06311         free(q);
06312         free(x);
06313         free(res);
06314         free(s);
06315         free(cu);
06316         free(q2);
06317         free(iu);
06318         free(pw_);
06319         return cl1_res;
06320 }
06321 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)
06322 {
06323     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06324     float r__1;
06325     int tmp__i;
06326     long int i__, j;
06327     --s;
06328     --res;
06329     iu -= 3;
06330     cu -= 3;
06331     --x;
06332     long int klm2d;
06333     klm2d= *k+*k+2;
06334     klm2d=klm2d+klm2d;
06335     q_dim1 = klm2d;
06336     q_offset = 1 + q_dim1;
06337     q -= q_offset;
06338     q2_dim1 = klm2d;
06339     q2_offset = 1 + q2_dim1;
06340     q2 -= q2_offset;
06341     i__2=0;
06342     i__1 = *n - 1;
06343     tmp__i=0;
06344     for (j = 1; j <= i__1; ++j) {
06345         i__2 = *k;
06346         tmp__i+=1;
06347         for (i__ = 1; i__ <= i__2; ++i__) {
06348             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06349             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06350             }
06351     }
06352     for  (i__ = 1; i__ <= i__2; ++i__)
06353       { q2[i__ + *n * q2_dim1] = 1.f;
06354             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06355         }
06356    vector<float> fit_res;
06357    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06358    return fit_res;
06359 }
06360 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)
06361 {
06362     /* System generated locals */
06363     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06364 
06365     /* Local variables */
06366     long int i__, j, m, n1, ii, jj;
06367     double tmp;
06368     vector<float> p;
06369     --x;
06370     q_dim1 = *klm2d;
06371     q_offset = 1 + q_dim1;
06372     q -= q_offset;
06373     q1_dim1 = *klm2d;
06374     q1_offset = 1 + q1_dim1;
06375     q1 -= q1_offset;
06376     --s;
06377     --res;
06378     iu -= 3;
06379     cu -= 3;
06380 
06381     /* Function Body */
06382     long int l = 0;
06383 
06384 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06385     m = *ks;
06386     n1 = *n + 1;
06387     if (*iswi == 1) {
06388         i__1 = n1;
06389         for (jj = 1; jj <= i__1; ++jj) {
06390             i__2 = *ks;
06391             for (ii = 1; ii <= i__2; ++ii) {
06392         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06393 
06394                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06395                         ;
06396             }
06397         }
06398     } else if (*iswi == 2) {
06399         i__1 = *ks;
06400         for (ii = 1; ii <= i__1; ++ii) {
06401             i__2 = n1;
06402             for (jj = 1; jj <= i__2; ++jj) {
06403                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06404                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06405             }
06406         }
06407     } else if (*iswi == 3) {
06408         l = 2;
06409         i__1 = n1;
06410         for (jj = 1; jj <= i__1; ++jj) {
06411             i__2 = *ks + 2;
06412             for (ii = 1; ii <= i__2; ++ii) {
06413                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06414             }
06415             i__2 = *ks;
06416             for (ii = 1; ii <= i__2; ++ii) {
06417                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06418             }
06419         }
06420     } else if (*iswi == 4) {
06421         l = 2;
06422         i__1 = n1;
06423         for (jj = 1; jj <= i__1; ++jj) {
06424             i__2 = *ks + 2;
06425             for (ii = 1; ii <= i__2; ++ii) {
06426                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06427             }
06428             i__2 = *ks;
06429             for (ii = 1; ii <= i__2; ++ii) {
06430                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06431             }
06432         }
06433     } else if (*iswi == 5) {
06434         l = 1;
06435         i__1 = n1;
06436         for (jj = 1; jj <= i__1; ++jj) {
06437             i__2 = *ks + 1;
06438             for (ii = 1; ii <= i__2; ++ii) {
06439                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06440             }
06441             i__2 = *ks;
06442             for (ii = 1; ii <= i__2; ++ii) {
06443                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06444             }
06445         }
06446     } else if (*iswi == 6) {
06447         l = 1;
06448         i__1 = n1;
06449         for (jj = 1; jj <= i__1; ++jj) {
06450             i__2 = *ks + 1;
06451             for (ii = 1; ii <= i__2; ++ii) {
06452                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06453             }
06454             i__2 = *ks;
06455             for (ii = 1; ii <= i__2; ++ii) {
06456                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06457             }
06458         }
06459     } else if (*iswi == 7) {
06460         l = 3;
06461         i__1 = n1;
06462         for (jj = 1; jj <= i__1; ++jj) {
06463             i__2 = *ks + 3;
06464             for (ii = 1; ii <= i__2; ++ii) {
06465                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06466             }
06467             i__2 = *ks;
06468             for (ii = 1; ii <= i__2; ++ii) {
06469                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06470             }
06471         }
06472     } else if (*iswi == 8) {
06473         l = 4;
06474         i__1 = n1;
06475         for (jj = 1; jj <= i__1; ++jj) {
06476             i__2 = *ks + 4;
06477             for (ii = 1; ii <= i__2; ++ii) {
06478                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06479             }
06480             i__2 = *ks;
06481             for (ii = 1; ii <= i__2; ++ii) {
06482                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06483             }
06484         }
06485     }
06486 
06487     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06488     i__1 = *ks;
06489     int tmp__j=0;
06490     for (i__ = 1; i__ <= i__1; ++i__) {
06491         tmp = 0.f;
06492         i__2 = *n - 1;
06493         for (j = 1; j <= i__2; ++j) {
06494             tmp__j=j;
06495             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06496             }
06497         tmp += x[*n];
06498         p.push_back(static_cast<float>(exp(tmp)));
06499         p.push_back(q1[i__ + q1_dim1]);
06500     }
06501     i__2=*n;
06502     for (i__=1;i__<=i__2;++i__)
06503         { p.push_back(static_cast<float>(x[i__]));}
06504     return p;
06505 }
06506 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06507         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06508 {
06509 
06510     long int q_dim1, q_offset, i__1, i__2;
06511     double d__1;
06512 
06513     static long int i__, j;
06514     static double z__;
06515     static long int n1, n2, ia, ii, kk, in, nk, js;
06516     static double sn, zu, zv;
06517     static long int nk1, klm, nkl, jmn, jpn;
06518     static double cuv;
06519     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06520     static float xmin;
06521     static double xmax;
06522     static long int iout;
06523     static double xsum;
06524     static long int iineg, maxit;
06525     static double toler;
06526     static float error;
06527     static double pivot;
06528     static long int kforce, iphase;
06529     static double tpivot;
06530 
06531     --s;
06532     --res;
06533     iu -= 3;
06534     cu -= 3;
06535     --x;
06536     q_dim1 = *klm2d;
06537     q_offset = 1 + q_dim1;
06538     q -= q_offset;
06539 
06540     /* Function Body */
06541     maxit = 500;
06542     kode = 0;
06543     toler = 1e-4f;
06544     iter = 0;
06545     n1 = *n + 1;
06546     n2 = *n + 2;
06547     nk = *n + *k;
06548     nk1 = nk + 1;
06549     nkl = nk + *l;
06550     nkl1 = nkl + 1;
06551     klm = *k + *l + *m;
06552     klm1 = klm + 1;
06553     klm2 = klm + 2;
06554     nklm = *n + klm;
06555     kforce = 1;
06556     iter = 0;
06557     js = 1;
06558     ia = 0;
06559 /* SET UP LABELS IN Q. */
06560     i__1 = *n;
06561     for (j = 1; j <= i__1; ++j) {
06562         q[klm2 + j * q_dim1] = (double) j;
06563 /* L10: */
06564     }
06565     i__1 = klm;
06566     for (i__ = 1; i__ <= i__1; ++i__) {
06567         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06568         if (q[i__ + n1 * q_dim1] >= 0.f) {
06569             goto L30;
06570         }
06571         i__2 = n2;
06572         for (j = 1; j <= i__2; ++j) {
06573             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06574 /* L20: */
06575         }
06576 L30:
06577         ;
06578     }
06579 /* SET UP PHASE 1 COSTS. */
06580     iphase = 2;
06581     i__1 = nklm;
06582     for (j = 1; j <= i__1; ++j) {
06583         cu[(j << 1) + 1] = 0.f;
06584         cu[(j << 1) + 2] = 0.f;
06585         iu[(j << 1) + 1] = 0;
06586         iu[(j << 1) + 2] = 0;
06587 /* L40: */
06588     }
06589     if (*l == 0) {
06590         goto L60;
06591     }
06592     i__1 = nkl;
06593     for (j = nk1; j <= i__1; ++j) {
06594         cu[(j << 1) + 1] = 1.f;
06595         cu[(j << 1) + 2] = 1.f;
06596         iu[(j << 1) + 1] = 1;
06597         iu[(j << 1) + 2] = 1;
06598 /* L50: */
06599     }
06600     iphase = 1;
06601 L60:
06602     if (*m == 0) {
06603         goto L80;
06604     }
06605     i__1 = nklm;
06606     for (j = nkl1; j <= i__1; ++j) {
06607         cu[(j << 1) + 2] = 1.f;
06608         iu[(j << 1) + 2] = 1;
06609         jmn = j - *n;
06610         if (q[jmn + n2 * q_dim1] < 0.f) {
06611             iphase = 1;
06612         }
06613 /* L70: */
06614     }
06615 L80:
06616     if (kode == 0) {
06617         goto L150;
06618     }
06619     i__1 = *n;
06620     for (j = 1; j <= i__1; ++j) {
06621         if ((d__1 = x[j]) < 0.) {
06622             goto L90;
06623         } else if (d__1 == 0) {
06624             goto L110;
06625         } else {
06626             goto L100;
06627         }
06628 L90:
06629         cu[(j << 1) + 1] = 1.f;
06630         iu[(j << 1) + 1] = 1;
06631         goto L110;
06632 L100:
06633         cu[(j << 1) + 2] = 1.f;
06634         iu[(j << 1) + 2] = 1;
06635 L110:
06636         ;
06637     }
06638     i__1 = *k;
06639     for (j = 1; j <= i__1; ++j) {
06640         jpn = j + *n;
06641         if ((d__1 = res[j]) < 0.) {
06642             goto L120;
06643         } else if (d__1 == 0) {
06644             goto L140;
06645         } else {
06646             goto L130;
06647         }
06648 L120:
06649         cu[(jpn << 1) + 1] = 1.f;
06650         iu[(jpn << 1) + 1] = 1;
06651         if (q[j + n2 * q_dim1] > 0.f) {
06652             iphase = 1;
06653         }
06654         goto L140;
06655 L130:
06656         cu[(jpn << 1) + 2] = 1.f;
06657         iu[(jpn << 1) + 2] = 1;
06658         if (q[j + n2 * q_dim1] < 0.f) {
06659             iphase = 1;
06660         }
06661 L140:
06662         ;
06663     }
06664 L150:
06665     if (iphase == 2) {
06666         goto L500;
06667     }
06668 /* COMPUTE THE MARGINAL COSTS. */
06669 L160:
06670     i__1 = n1;
06671     for (j = js; j <= i__1; ++j) {
06672         xsum = 0.;
06673         i__2 = klm;
06674         for (i__ = 1; i__ <= i__2; ++i__) {
06675             ii = (long int) q[i__ + n2 * q_dim1];
06676             if (ii < 0) {
06677                 goto L170;
06678             }
06679             z__ = cu[(ii << 1) + 1];
06680             goto L180;
06681 L170:
06682             iineg = -ii;
06683             z__ = cu[(iineg << 1) + 2];
06684 L180:
06685             xsum += q[i__ + j * q_dim1] * z__;
06686 /*  180       XSUM = XSUM + Q(I,J)*Z */
06687 /* L190: */
06688         }
06689         q[klm1 + j * q_dim1] = xsum;
06690 /* L200: */
06691     }
06692     i__1 = *n;
06693     for (j = js; j <= i__1; ++j) {
06694         ii = (long int) q[klm2 + j * q_dim1];
06695         if (ii < 0) {
06696             goto L210;
06697         }
06698         z__ = cu[(ii << 1) + 1];
06699         goto L220;
06700 L210:
06701         iineg = -ii;
06702         z__ = cu[(iineg << 1) + 2];
06703 L220:
06704         q[klm1 + j * q_dim1] -= z__;
06705 /* L230: */
06706     }
06707 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06708 L240:
06709     xmax = 0.f;
06710     if (js > *n) {
06711         goto L490;
06712     }
06713     i__1 = *n;
06714     for (j = js; j <= i__1; ++j) {
06715         zu = q[klm1 + j * q_dim1];
06716         ii = (long int) q[klm2 + j * q_dim1];
06717         if (ii > 0) {
06718             goto L250;
06719         }
06720         ii = -ii;
06721         zv = zu;
06722         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06723         goto L260;
06724 L250:
06725         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06726 L260:
06727         if (kforce == 1 && ii > *n) {
06728             goto L280;
06729         }
06730         if (iu[(ii << 1) + 1] == 1) {
06731             goto L270;
06732         }
06733         if (zu <= xmax) {
06734             goto L270;
06735         }
06736         xmax = zu;
06737         in = j;
06738 L270:
06739         if (iu[(ii << 1) + 2] == 1) {
06740             goto L280;
06741         }
06742         if (zv <= xmax) {
06743             goto L280;
06744         }
06745         xmax = zv;
06746         in = j;
06747 L280:
06748         ;
06749     }
06750     if (xmax <= toler) {
06751         goto L490;
06752     }
06753     if (q[klm1 + in * q_dim1] == xmax) {
06754         goto L300;
06755     }
06756     i__1 = klm2;
06757     for (i__ = 1; i__ <= i__1; ++i__) {
06758         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06759 /* L290: */
06760     }
06761     q[klm1 + in * q_dim1] = xmax;
06762 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06763 L300:
06764     if (iphase == 1 || ia == 0) {
06765         goto L330;
06766     }
06767     xmax = 0.f;
06768     i__1 = ia;
06769     for (i__ = 1; i__ <= i__1; ++i__) {
06770         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06771         if (z__ <= xmax) {
06772             goto L310;
06773         }
06774         xmax = z__;
06775         iout = i__;
06776 L310:
06777         ;
06778     }
06779     if (xmax <= toler) {
06780         goto L330;
06781     }
06782     i__1 = n2;
06783     for (j = 1; j <= i__1; ++j) {
06784         z__ = q[ia + j * q_dim1];
06785         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06786         q[iout + j * q_dim1] = z__;
06787 /* L320: */
06788     }
06789     iout = ia;
06790     --ia;
06791     pivot = q[iout + in * q_dim1];
06792     goto L420;
06793 L330:
06794     kk = 0;
06795     i__1 = klm;
06796     for (i__ = 1; i__ <= i__1; ++i__) {
06797         z__ = q[i__ + in * q_dim1];
06798         if (z__ <= toler) {
06799             goto L340;
06800         }
06801         ++kk;
06802         res[kk] = q[i__ + n1 * q_dim1] / z__;
06803         s[kk] = (double) i__;
06804 L340:
06805         ;
06806     }
06807 L350:
06808     if (kk > 0) {
06809         goto L360;
06810     }
06811     kode = 2;
06812     goto L590;
06813 L360:
06814     xmin = static_cast<float>( res[1] );
06815     iout = (long int) s[1];
06816     j = 1;
06817     if (kk == 1) {
06818         goto L380;
06819     }
06820     i__1 = kk;
06821     for (i__ = 2; i__ <= i__1; ++i__) {
06822         if (res[i__] >= xmin) {
06823             goto L370;
06824         }
06825         j = i__;
06826         xmin = static_cast<float>( res[i__] );
06827         iout = (long int) s[i__];
06828 L370:
06829         ;
06830     }
06831     res[j] = res[kk];
06832     s[j] = s[kk];
06833 L380:
06834     --kk;
06835     pivot = q[iout + in * q_dim1];
06836     ii = (long int) q[iout + n2 * q_dim1];
06837     if (iphase == 1) {
06838         goto L400;
06839     }
06840     if (ii < 0) {
06841         goto L390;
06842     }
06843     if (iu[(ii << 1) + 2] == 1) {
06844         goto L420;
06845     }
06846     goto L400;
06847 L390:
06848     iineg = -ii;
06849     if (iu[(iineg << 1) + 1] == 1) {
06850         goto L420;
06851     }
06852 /* 400 II = IABS(II) */
06853 L400:
06854     ii = abs(ii);
06855     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06856     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06857         goto L420;
06858     }
06859 /* BYPASS INTERMEDIATE VERTICES. */
06860     i__1 = n1;
06861     for (j = js; j <= i__1; ++j) {
06862         z__ = q[iout + j * q_dim1];
06863         q[klm1 + j * q_dim1] -= z__ * cuv;
06864         q[iout + j * q_dim1] = -z__;
06865 /* L410: */
06866     }
06867     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06868     goto L350;
06869 /* GAUSS-JORDAN ELIMINATION. */
06870 L420:
06871     if (iter < maxit) {
06872         goto L430;
06873     }
06874     kode = 3;
06875     goto L590;
06876 L430:
06877     ++iter;
06878     i__1 = n1;
06879     for (j = js; j <= i__1; ++j) {
06880         if (j != in) {
06881             q[iout + j * q_dim1] /= pivot;
06882         }
06883 /* L440: */
06884     }
06885 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
06886 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
06887 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
06888 /*     DO 460 J=JS,N1 */
06889 /*        IF(J .EQ. IN) GO TO 460 */
06890 /*        Z = -Q(IOUT,J) */
06891 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
06892 /* 460 CONTINUE */
06893     i__1 = n1;
06894     for (j = js; j <= i__1; ++j) {
06895         if (j == in) {
06896             goto L460;
06897         }
06898         z__ = -q[iout + j * q_dim1];
06899         i__2 = klm1;
06900         for (i__ = 1; i__ <= i__2; ++i__) {
06901             if (i__ != iout) {
06902                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06903             }
06904 /* L450: */
06905         }
06906 L460:
06907         ;
06908     }
06909     tpivot = -pivot;
06910     i__1 = klm1;
06911     for (i__ = 1; i__ <= i__1; ++i__) {
06912         if (i__ != iout) {
06913             q[i__ + in * q_dim1] /= tpivot;
06914         }
06915 /* L470: */
06916     }
06917     q[iout + in * q_dim1] = 1.f / pivot;
06918     z__ = q[iout + n2 * q_dim1];
06919     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06920     q[klm2 + in * q_dim1] = z__;
06921     ii = (long int) abs(z__);
06922     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06923         goto L240;
06924     }
06925     i__1 = klm2;
06926     for (i__ = 1; i__ <= i__1; ++i__) {
06927         z__ = q[i__ + in * q_dim1];
06928         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06929         q[i__ + js * q_dim1] = z__;
06930 /* L480: */
06931     }
06932     ++js;
06933     goto L240;
06934 /* TEST FOR OPTIMALITY. */
06935 L490:
06936     if (kforce == 0) {
06937         goto L580;
06938     }
06939     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06940         goto L500;
06941     }
06942     kforce = 0;
06943     goto L240;
06944 /* SET UP PHASE 2 COSTS. */
06945 L500:
06946     iphase = 2;
06947     i__1 = nklm;
06948     for (j = 1; j <= i__1; ++j) {
06949         cu[(j << 1) + 1] = 0.f;
06950         cu[(j << 1) + 2] = 0.f;
06951 /* L510: */
06952     }
06953     i__1 = nk;
06954     for (j = n1; j <= i__1; ++j) {
06955         cu[(j << 1) + 1] = 1.f;
06956         cu[(j << 1) + 2] = 1.f;
06957 /* L520: */
06958     }
06959     i__1 = klm;
06960     for (i__ = 1; i__ <= i__1; ++i__) {
06961         ii = (long int) q[i__ + n2 * q_dim1];
06962         if (ii > 0) {
06963             goto L530;
06964         }
06965         ii = -ii;
06966         if (iu[(ii << 1) + 2] == 0) {
06967             goto L560;
06968         }
06969         cu[(ii << 1) + 2] = 0.f;
06970         goto L540;
06971 L530:
06972         if (iu[(ii << 1) + 1] == 0) {
06973             goto L560;
06974         }
06975         cu[(ii << 1) + 1] = 0.f;
06976 L540:
06977         ++ia;
06978         i__2 = n2;
06979         for (j = 1; j <= i__2; ++j) {
06980             z__ = q[ia + j * q_dim1];
06981             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06982             q[i__ + j * q_dim1] = z__;
06983 /* L550: */
06984         }
06985 L560:
06986         ;
06987     }
06988     goto L160;
06989 L570:
06990     if (q[klm1 + n1 * q_dim1] <= toler) {
06991         goto L500;
06992     }
06993     kode = 1;
06994     goto L590;
06995 L580:
06996     if (iphase == 1) {
06997         goto L570;
06998     }
06999 /* PREPARE OUTPUT. */
07000     kode = 0;
07001 L590:
07002     xsum = 0.;
07003     i__1 = *n;
07004     for (j = 1; j <= i__1; ++j) {
07005         x[j] = 0.f;
07006 /* L600: */
07007     }
07008     i__1 = klm;
07009     for (i__ = 1; i__ <= i__1; ++i__) {
07010         res[i__] = 0.f;
07011 /* L610: */
07012     }
07013     i__1 = klm;
07014     for (i__ = 1; i__ <= i__1; ++i__) {
07015         ii = (long int) q[i__ + n2 * q_dim1];
07016         sn = 1.f;
07017         if (ii > 0) {
07018             goto L620;
07019         }
07020         ii = -ii;
07021         sn = -1.f;
07022 L620:
07023         if (ii > *n) {
07024             goto L630;
07025         }
07026         x[ii] = sn * q[i__ + n1 * q_dim1];
07027         goto L640;
07028 L630:
07029         iimn = ii - *n;
07030         res[iimn] = sn * q[i__ + n1 * q_dim1];
07031         if (ii >= n1 && ii <= nk) {
07032             xsum += q[i__ + n1 * q_dim1];
07033         }
07034 L640:
07035         ;
07036     }
07037     error = (float)xsum;
07038     return;
07039 }
07040 
07041 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07042 {
07043         int j,d;
07044         EMData * e = new EMData();
07045         float *eptr, *imgptr;
07046         imgptr = img->get_data();
07047         float SSE = 0.f;
07048         for (j = 0 ; j < N ; j++) {
07049                 e->read_image(images,S[j]);
07050                 eptr = e->get_data();
07051                 for (d = 0; d < size; d++) {
07052                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07053                 }
07054         delete e;
07055         return SSE;
07056 }
07057 
07058 
07059 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07060 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07061 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07062 
07063 
07064 #define         quadpi                  3.141592653589793238462643383279502884197
07065 #define         dgr_to_rad              quadpi/180
07066 #define         deg_to_rad              quadpi/180
07067 #define         rad_to_deg              180/quadpi
07068 #define         rad_to_dgr              180/quadpi
07069 #define         TRUE                    1
07070 #define         FALSE                   0
07071 
07072 
07073 #define theta(i)                theta   [i-1]
07074 #define phi(i)                  phi     [i-1]
07075 #define weight(i)               weight  [i-1]
07076 #define lband(i)                lband   [i-1]
07077 #define ts(i)                   ts      [i-1]
07078 #define thetast(i)              thetast [i-1]
07079 #define key(i)                  key     [i-1]
07080 
07081 
07082 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07083 {
07084 
07085         ENTERFUNC;
07086 
07087         if ( th.size() != ph.size() ) {
07088                 LOGERR("images not same size");
07089                 throw ImageFormatException( "images not same size");
07090         }
07091 
07092         // rand_seed
07093         srand(10);
07094 
07095         int i,*key;
07096         int len = th.size();
07097         double *theta,*phi,*weight;
07098         theta   =       (double*) calloc(len,sizeof(double));
07099         phi     =       (double*) calloc(len,sizeof(double));
07100         weight  =       (double*) calloc(len,sizeof(double));
07101         key     =       (int*) calloc(len,sizeof(int));
07102         const float *thptr, *phptr;
07103 
07104         thptr = &th[0];
07105         phptr = &ph[0];
07106         for(i=1;i<=len;i++){
07107                 key(i) = i;
07108                 weight(i) = 0.0;
07109         }
07110 
07111         for(i = 0;i<len;i++){
07112                 theta[i] = thptr[i];
07113                 phi[i]   = phptr[i];
07114         }
07115 
07116         //  sort by theta
07117         Util::hsortd(theta, phi, key, len, 1);
07118 
07119         //Util::voronoidiag(theta,phi, weight, len);
07120         Util::voronoi(phi, theta, weight, len);
07121 
07122         //sort by key
07123         Util::hsortd(weight, weight, key, len, 2);
07124 
07125         free(theta);
07126         free(phi);
07127         free(key);
07128         vector<double> wt;
07129         double count = 0;
07130         for(i=1; i<= len; i++)
07131         {
07132                 wt.push_back(weight(i));
07133                 count += weight(i);
07134         }
07135 
07136         //if( abs(count-6.28) > 0.1 )
07137         //{
07138         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07139         //}
07140 
07141         free(weight);
07142 
07143         EXITFUNC;
07144         return wt;
07145 
07146 }
07147 
07148 struct  tmpstruct{
07149         double theta1,phi1;
07150         int key1;
07151         };
07152 
07153 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07154 {
07155         ENTERFUNC;
07156         vector<tmpstruct> tmp(len);
07157         int i;
07158         for(i = 1;i<=len;i++)
07159         {
07160                 tmp[i-1].theta1 = theta(i);
07161                 tmp[i-1].phi1 = phi(i);
07162                 tmp[i-1].key1 = key(i);
07163         }
07164 
07165         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07166         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07167 
07168         for(i = 1;i<=len;i++)
07169         {
07170                 theta(i) = tmp[i-1].theta1;
07171                 phi(i)   = tmp[i-1].phi1;
07172                 key(i)   = tmp[i-1].key1;
07173         }
07174         EXITFUNC;
07175 }
07176 
07177 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07178 {
07179         return(tmp1.theta1 < tmp2.theta1);
07180 }
07181 
07182 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07183 {
07184         return(tmp1.key1 < tmp2.key1);
07185 }
07186 
07187 /******************  VORONOI DIAGRAM **********************************/
07188 /*
07189 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07190 {
07191         ENTERFUNC;
07192 
07193         int     *lband;
07194         double  aat=0.0f,*ts;
07195         double  aa,acum,area;
07196         int     last;
07197         int numth       =       1;
07198         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07199 
07200         int i,it,l,k;
07201         int nband,lb,low,medium,lhigh,lbw,lenw;
07202 
07203 
07204         lband   =       (int*)calloc(nbt,sizeof(int));
07205         ts      =       (double*)calloc(nbt,sizeof(double));
07206 
07207         if(lband == NULL || ts == NULL ){
07208                 fprintf(stderr,"memory allocation failure!\n");
07209                 exit(1);
07210         }
07211 
07212         nband=nbt;
07213 
07214         while(nband>0){
07215                 Util::angstep(ts,nband);
07216 
07217                 l=1;
07218                 for(i=1;i<=n;i++){
07219                         if(theta(i)>ts(l)){
07220                                 lband(l)=i;
07221                                 l=l+1;
07222                                 if(l>nband)  exit(1);
07223                         }
07224                 }
07225 
07226                 l=1;
07227                 for(i=1;i<=n;i++){
07228                         if(theta(i)>ts(l)){
07229                                 lband(l)=i;
07230                                 l=l+1;
07231                                 if(l>nband)  exit(1);
07232                         }
07233                 }
07234 
07235                 lband(l)=n+1;
07236                 acum=0.0;
07237                 for(it=l;it>=1;it-=numth){
07238                         for(i=it;i>=mymax(1,it-numth+1);i--){
07239                         if(i==l) last   =        TRUE;
07240                         else     last   =        FALSE;
07241 
07242                         if(l==1){
07243                                 lb=1;
07244                                 low=1;
07245                                 medium=n+1;
07246                                 lhigh=n-lb+1;
07247                                 lbw=1;
07248                         }
07249                         else if(i==1){
07250                                 lb=1;
07251                                 low=1;
07252                                 medium=lband(1);
07253                                 lhigh=lband(2)-1;
07254                                 lbw=1;
07255                         }
07256                         else if(i==l){
07257                                 if(l==2)        lb=1;
07258                                 else            lb=lband(l-2);
07259                                 low=lband(l-1)-lb+1;
07260                                 medium=lband(l)-lb+1;
07261                                 lhigh=n-lb+1;
07262                                 lbw=lband(i-1);
07263                         }
07264                         else{
07265                                 if(i==2)        lb=1;
07266                                 else            lb=lband(i-2);
07267                                 low=lband(i-1)-lb+1;
07268                                 medium=lband(i)-lb+1;
07269                                 lhigh=lband(i+1)-1-lb+1;
07270                                 lbw=lband(i-1);
07271                         }
07272                         lenw=medium-low;
07273 
07274 
07275                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07276 
07277 
07278                         if(nband>1){
07279                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07280                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07281 
07282                                 aa = 0.0;
07283                                 for(k = lbw;k<=lbw+lenw-1;k++)
07284                                         aa = aa+weight(k);
07285 
07286                                 acum=acum+aa;
07287                                 aat=aa/area;
07288                                 }
07289 
07290                         }
07291                         for(i=it;mymax(1,it-numth+1);i--){
07292                         if(fabs(aat-1.0)>0.02){
07293                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07294                                 goto  label2;
07295                                 }
07296                         }
07297                 acum=acum/quadpi/2.0;
07298                 exit(1);
07299 label2:
07300 
07301                 continue;
07302                 }
07303 
07304         free(ts);
07305         free(lband);
07306 
07307         }
07308 
07309         EXITFUNC;
07310 }
07311 
07312 
07313 void Util::angstep(double* thetast,int len){
07314 
07315         ENTERFUNC;
07316 
07317         double t1,t2,tmp;
07318         int i;
07319         if(len>1){
07320                 t1=0;
07321                 for(i=1;i<=len-1;i++){
07322                         tmp=cos(t1)-1.0/((float)len);
07323                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07324                         thetast(i)=t2 * rad_to_deg;
07325                         t1=t2;
07326                 }
07327         }
07328         thetast(len)=90.0;
07329 
07330         EXITFUNC;
07331 }
07332 */
07333 /*
07334 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07335 {
07336 
07337         ENTERFUNC;
07338         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07339         int nt6, n, ier,nout,lnew,mdup,nd;
07340         int i,k,mt,status;
07341 
07342 
07343         double *ds, *x, *y, *z;
07344         double tol=1.0e-8;
07345         double a;
07346 
07347         if(last){
07348                 if(medium>nt)  n = nt+nt;
07349                 else           n = nt+nt-medium+1;
07350         }
07351         else{
07352                 n=nt;
07353         }
07354 
07355         nt6 = n*6;
07356 
07357         list = (int*)calloc(nt6,sizeof(int));
07358         lptr = (int*)calloc(nt6,sizeof(int));
07359         lend = (int*)calloc(n  ,sizeof(int));
07360         iwk  = (int*)calloc(n  ,sizeof(int));
07361         good = (int*)calloc(n  ,sizeof(int));
07362         key  = (int*)calloc(n  ,sizeof(int));
07363         indx = (int*)calloc(n  ,sizeof(int));
07364         lcnt = (int*)calloc(n  ,sizeof(int));
07365 
07366         ds      =       (double*) calloc(n,sizeof(double));
07367         x       =       (double*) calloc(n,sizeof(double));
07368         y       =       (double*) calloc(n,sizeof(double));
07369         z       =       (double*) calloc(n,sizeof(double));
07370 
07371         if (list == NULL ||
07372         lptr == NULL ||
07373         lend == NULL ||
07374         iwk  == NULL ||
07375         good == NULL ||
07376         key  == NULL ||
07377         indx == NULL ||
07378         lcnt == NULL ||
07379         x    == NULL ||
07380         y    == NULL ||
07381         z    == NULL ||
07382         ds   == NULL) {
07383                 printf("memory allocation failure!\n");
07384                 exit(1);
07385         }
07386 
07387 
07388 
07389         for(i = 1;i<=nt;i++){
07390                 x[i-1] = theta(i);
07391                 y[i-1] = phi(i);
07392         }
07393 
07394 
07395 
07396         if (last) {
07397                 for(i=nt+1;i<=n;i++){
07398                         x[i-1]=180.0-x[2*nt-i];
07399                         y[i-1]=180.0+y[2*nt-i];
07400                 }
07401         }
07402 
07403 
07404         Util::disorder2(x,y,key,n);
07405 
07406         Util::ang_to_xyz(x,y,z,n);
07407 
07408 
07409         //  Make sure that first three are no colinear
07410         label1:
07411         for(k=0; k<2; k++){
07412                 for(i=k+1; i<3; i++){
07413                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07414                                 Util::flip23(x, y, z, key, k, n);
07415                                 goto label1;
07416                         }
07417                 }
07418         }
07419 
07420 
07421         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07422 
07423 
07424         if (status != 0) {
07425                 printf(" error in trmsh3 \n");
07426                 exit(1);
07427         }
07428 
07429 
07430         mdup=n-nout;
07431         if (ier == -2) {
07432                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07433                 exit(1);
07434         }
07435         else if (ier > 0) {
07436                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07437                 exit(1);
07438         }
07439 
07440         nd=0;
07441         for (k=1;k<=n;k++){
07442                 if (indx[k-1]>0){
07443                         nd++;
07444                         good[nd-1]=k;
07445                 }
07446         }
07447 
07448 
07449         for(i = 1;i<=nout;i++) {
07450                 k=good[i-1];
07451                 if (key[k-1] >= low && key[k-1]<medium){
07452                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07453                         if (ier != 0){
07454                                 weight[key[k-1]-low] =-1.0;
07455                         }
07456                         else {
07457                                 weight[key[k-1]-low]=a/lcnt[i-1];
07458                         }
07459                 }
07460         }
07461 
07462 // Fill out the duplicated weights
07463         for(i = 1;i<=n;i++){
07464                 mt=-indx[i-1];
07465                 if (mt>0){
07466                         k=good[mt-1];
07467 //  This is a duplicated entry, get the already calculated
07468 //   weight and assign it.
07469                         if (key[i-1]>=low && key[i-1]<medium){
07470 //  Is it already calculated weight??
07471                                 if(key[k-1]>=low && key[k-1]<medium){
07472                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07473                                 }
07474                                 else{
07475 //  No, the weight is from the outside of valid region, calculate it anyway
07476                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07477                                         if (ier != 0){
07478                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07479                                                 weight[key[i-1]-low] =-1.0;
07480                                         }
07481                                         else {
07482                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07483                                         }
07484                                 }
07485                         }
07486                 }
07487         }
07488 
07489 
07490         free(list);
07491         free(lend);
07492         free(iwk);
07493         free(good);
07494         free(key);
07495 
07496         free(indx);
07497         free(lcnt);
07498         free(ds);
07499         free(x);
07500         free(y);
07501         free(z);
07502         EXITFUNC;
07503 }
07504 */
07505 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07506 {
07507 
07508         ENTERFUNC;
07509 
07510         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07511         int nt6, n, ier, nout, lnew, mdup, nd;
07512         int i,k,mt,status;
07513 
07514 
07515         double *ds, *x, *y, *z;
07516         double tol  = 1.0e-8;
07517         double dtol = 15;
07518         double a;
07519 
07520         /*if(last){
07521                 if(medium>nt)  n = nt+nt;
07522                 else           n = nt+nt-medium+1;
07523         }
07524         else{
07525                 n=nt;
07526         }*/
07527 
07528         n = nt + nt;
07529 
07530         nt6 = n*6;
07531 
07532         list = (int*)calloc(nt6,sizeof(int));
07533         lptr = (int*)calloc(nt6,sizeof(int));
07534         lend = (int*)calloc(n  ,sizeof(int));
07535         iwk  = (int*)calloc(n  ,sizeof(int));
07536         good = (int*)calloc(n  ,sizeof(int));
07537         key  = (int*)calloc(n  ,sizeof(int));
07538         indx = (int*)calloc(n  ,sizeof(int));
07539         lcnt = (int*)calloc(n  ,sizeof(int));
07540 
07541         ds      =       (double*) calloc(n,sizeof(double));
07542         x       =       (double*) calloc(n,sizeof(double));
07543         y       =       (double*) calloc(n,sizeof(double));
07544         z       =       (double*) calloc(n,sizeof(double));
07545 
07546         if (list == NULL ||
07547         lptr == NULL ||
07548         lend == NULL ||
07549         iwk  == NULL ||
07550         good == NULL ||
07551         key  == NULL ||
07552         indx == NULL ||
07553         lcnt == NULL ||
07554         x    == NULL ||
07555         y    == NULL ||
07556         z    == NULL ||
07557         ds   == NULL) {
07558                 printf("memory allocation failure!\n");
07559                 exit(1);
07560         }
07561 
07562         bool colinear=true;
07563         while(colinear)
07564         {
07565 
07566         L1:
07567             for(i = 0; i<nt; i++){
07568                 x[i] = theta[i];
07569                 y[i] = phi[i];
07570                 x[nt+i] = 180.0 - x[i];
07571                 y[nt+i] = 180.0 + y[i];
07572             }
07573 
07574             Util::disorder2(x, y, key, n);
07575 
07576             // check if the first three angles are not close, else shuffle
07577             double val;
07578             for(k=0; k<2; k++){
07579                 for(i=k+1; i<3; i++){
07580                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07581                     if( val  < dtol) {
07582                         goto L1;
07583                     }
07584                 }
07585             }
07586 
07587             Util::ang_to_xyz(x, y, z, n);
07588 
07589             //  Make sure that first three has no duplication
07590             bool dupnode=true;
07591             dupnode=true;
07592             while(dupnode)
07593             {
07594                 for(k=0; k<2; k++){
07595                     for(i=k+1; i<3; i++){
07596                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07597                                 Util::flip23(x, y, z, key, k, n);
07598                                 continue;
07599                         }
07600                     }
07601                 }
07602                 dupnode = false;
07603             }
07604 
07605 
07606             ier = 0;
07607 
07608             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07609 
07610             if (status != 0) {
07611                 printf(" error in trmsh3 \n");
07612                 exit(1);
07613             }
07614 
07615             if (ier > 0) {
07616                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07617                 exit(1);
07618             }
07619 
07620             mdup=n-nout;
07621             if (ier == -2) {
07622                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07623             }
07624             else
07625             {
07626                 colinear=false;
07627             }
07628         }
07629 
07630 
07631         Assert( ier != -2 );
07632 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07633 //  INDX contains node numbers from the squeezed list
07634         nd=0;
07635         for (k=1; k<=n; k++){
07636                 if (indx[k-1]>0) {
07637                         nd++;
07638                         good[nd-1]=k;
07639                 }
07640         }
07641 
07642 //
07643 // *** Compute the Voronoi region areas.
07644 //
07645         for(i = 1; i<=nout; i++) {
07646                 k=good[i-1];
07647                 //  We only need n weights from hemisphere
07648                 if (key[k-1] <= nt) {
07649 //  CALCULATE THE AREA
07650                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07651                         if (ier != 0){
07652 //  We set the weight to -1, this will signal the error in the calling
07653 //   program, as the area will turn out incorrect
07654                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07655                                 weight[key[k-1]-1] =-1.0;
07656                         } else {
07657 //  Assign the weight
07658                                 weight[key[k-1]-1]=a/lcnt[i-1];
07659                         }
07660                 }
07661         }
07662 
07663 
07664 // Fill out the duplicated weights
07665         for(i = 1; i<=n; i++){
07666                 mt =- indx[i-1];
07667                 if (mt>0){
07668                         k = good[mt-1];
07669 //  This is a duplicated entry, get the already calculated
07670 //   weight and assign it.
07671                 //  We only need n weights from hemisphere
07672                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07673                         }
07674         }
07675 
07676         free(list);
07677         free(lend);
07678         free(iwk);
07679         free(good);
07680         free(key);
07681         free(lptr);
07682         free(indx);
07683         free(lcnt);
07684         free(ds);
07685         free(x);
07686         free(y);
07687         free(z);
07688 
07689 
07690         EXITFUNC;
07691 }
07692 
07693 void Util::disorder2(double *x,double *y, int *key, int len)
07694 {
07695         ENTERFUNC;
07696         int k, i;
07697         for(i=0; i<len; i++) key[i]=i+1;
07698 
07699         for(i = 0; i<len;i++){
07700                 k = rand()%len;
07701                 std::swap(key[k], key[i]);
07702                 std::swap(x[k], x[i]);
07703                 std::swap(y[k], y[i]);
07704         }
07705         EXITFUNC;
07706 }
07707 
07708 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07709 {
07710         ENTERFUNC;
07711         double costheta,sintheta,cosphi,sinphi;
07712         for(int i = 0;  i<len;  i++) {
07713                 cosphi = cos(y[i]*dgr_to_rad);
07714                 sinphi = sin(y[i]*dgr_to_rad);
07715                 if(fabs(x[i]-90.0)< 1.0e-5){
07716                         x[i] = cosphi;
07717                         y[i] = sinphi;
07718                         z[i] = 0.0;
07719                 } else {
07720                         costheta = cos(x[i]*dgr_to_rad);
07721                         sintheta = sin(x[i]*dgr_to_rad);
07722                         x[i] = cosphi*sintheta;
07723                         y[i] = sinphi*sintheta;
07724                         z[i] = costheta;
07725                 }
07726         }
07727         EXITFUNC;
07728 }
07729 
07730 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07731 {
07732         ENTERFUNC;
07733         int i = k;
07734         while( i == k )  i = rand()%len;
07735         std::swap(key[i], key[k]);
07736         std::swap(x[i], x[k]);
07737         std::swap(y[i], y[k]);
07738         std::swap(z[i], z[k]);
07739         EXITFUNC;
07740 }
07741 
07742 
07743 #undef  mymax
07744 #undef  mymin
07745 #undef  sign
07746 #undef  quadpi
07747 #undef  dgr_to_rad
07748 #undef  deg_to_rad
07749 #undef  rad_to_deg
07750 #undef  rad_to_dgr
07751 #undef  TRUE
07752 #undef  FALSE
07753 #undef  theta
07754 #undef  phi
07755 #undef  weight
07756 #undef  lband
07757 #undef  ts
07758 #undef  thetast
07759 #undef  key
07760 
07761 
07762 /*################################################################################################
07763 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07764 ######   You must link the resulting object file with the libraries: #############################
07765 ####################    -lf2c -lm   (in that order)   ############################################
07766 ################################################################################################*/
07767 
07768 /* Common Block Declarations */
07769 
07770 
07771 #define TRUE_ (1)
07772 #define FALSE_ (0)
07773 #define abs(x) ((x) >= 0 ? (x) : -(x))
07774 
07775 struct stcom_{
07776     double y;
07777 };
07778 stcom_ stcom_1;
07779 #ifdef KR_headers
07780 double floor();
07781 int i_dnnt(x) double *x;
07782 #else
07783 int i_dnnt(double *x)
07784 #endif
07785 {
07786         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07787 }
07788 
07789 
07790 
07791 
07792 /* ____________________STRID______________________________________ */
07793 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07794         double *y, double *z__, int *n, int *list, int *
07795         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07796         int *near__, int *next, double *dist, int *ier)
07797 {
07798     /* System generated locals */
07799     int i__1, i__2;
07800 
07801     /* Local variables */
07802     static double d__;
07803     static int i__, j;
07804     static double d1, d2, d3;
07805     static int i0, lp, kt, ku, lpl, nku;
07806     static int nexti;
07807 
07808 
07809 /* *********************************************************** */
07810 
07811 /*                                              From STRIPACK */
07812 /*                                            Robert J. Renka */
07813 /*                                  Dept. of Computer Science */
07814 /*                                       Univ. of North Texas */
07815 /*                                           renka@cs.unt.edu */
07816 /*                                                   01/20/03 */
07817 
07818 /*   This is an alternative to TRMESH with the inclusion of */
07819 /* an efficient means of removing duplicate or nearly dupli- */
07820 /* cate nodes. */
07821 
07822 /*   This subroutine creates a Delaunay triangulation of a */
07823 /* set of N arbitrarily distributed points, referred to as */
07824 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07825 /* routine TRMESH for definitions and a list of additional */
07826 /* subroutines.  This routine is an alternative to TRMESH */
07827 /* with the inclusion of an efficient means of removing dup- */
07828 /* licate or nearly duplicate nodes. */
07829 
07830 /*   The algorithm has expected time complexity O(N*log(N)) */
07831 /* for random nodal distributions. */
07832 
07833 
07834 /* On input: */
07835 
07836 /*       N0 = Number of nodes, possibly including duplicates. */
07837 /*            N0 .GE. 3. */
07838 
07839 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07840 /*             bound on the deviation from 1 of the cosine of */
07841 /*             the angle between the nodes.  Note that */
07842 /*             |1-cos(A)| is approximately A*A/2. */
07843 
07844 /* The above parameters are not altered by this routine. */
07845 
07846 /*       X,Y,Z = Arrays of length at least N0 containing the */
07847 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07848 /*               Z(K)) is referred to as node K, and K is re- */
07849 /*               ferred to as a nodal index.  It is required */
07850 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
07851 /*               K.  The first three nodes must not be col- */
07852 /*               linear (lie on a common great circle). */
07853 
07854 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
07855 
07856 /*       LEND = Array of length at least N0. */
07857 
07858 /*       INDX = Array of length at least N0. */
07859 
07860 /*       LCNT = Array of length at least N0 (length N is */
07861 /*              sufficient). */
07862 
07863 /*       NEAR,NEXT,DIST = Work space arrays of length at */
07864 /*                        least N0.  The space is used to */
07865 /*                        efficiently determine the nearest */
07866 /*                        triangulation node to each un- */
07867 /*                        processed node for use by ADDNOD. */
07868 
07869 /* On output: */
07870 
07871 /*       N = Number of nodes in the triangulation.  3 .LE. N */
07872 /*           .LE. N0, or N = 0 if IER < 0. */
07873 
07874 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
07875 /*               of the triangulation nodes in the first N */
07876 /*               locations.  The original array elements are */
07877 /*               shifted down as necessary to eliminate dup- */
07878 /*               licate nodes. */
07879 
07880 /*       LIST = Set of nodal indexes which, along with LPTR, */
07881 /*              LEND, and LNEW, define the triangulation as a */
07882 /*              set of N adjacency lists -- counterclockwise- */
07883 /*              ordered sequences of neighboring nodes such */
07884 /*              that the first and last neighbors of a bound- */
07885 /*              ary node are boundary nodes (the first neigh- */
07886 /*              bor of an interior node is arbitrary).  In */
07887 /*              order to distinguish between interior and */
07888 /*              boundary nodes, the last neighbor of each */
07889 /*              boundary node is represented by the negative */
07890 /*              of its index. */
07891 
07892 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
07893 /*              correspondence with the elements of LIST. */
07894 /*              LIST(LPTR(I)) indexes the node which follows */
07895 /*              LIST(I) in cyclical counterclockwise order */
07896 /*              (the first neighbor follows the last neigh- */
07897 /*              bor). */
07898 
07899 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
07900 /*              points to the last neighbor of node K for */
07901 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
07902 /*              only if K is a boundary node. */
07903 
07904 /*       LNEW = Pointer to the first empty location in LIST */
07905 /*              and LPTR (list length plus one).  LIST, LPTR, */
07906 /*              LEND, and LNEW are not altered if IER < 0, */
07907 /*              and are incomplete if IER > 0. */
07908 
07909 /*       INDX = Array of output (triangulation) nodal indexes */
07910 /*              associated with input nodes.  For I = 1 to */
07911 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
07912 /*              the triangulation node with the same (or */
07913 /*              nearly the same) coordinates as input node I. */
07914 
07915 /*       LCNT = Array of int weights (counts) associated */
07916 /*              with the triangulation nodes.  For I = 1 to */
07917 /*              N, LCNT(I) is the number of occurrences of */
07918 /*              node I in the input node set, and thus the */
07919 /*              number of duplicates is LCNT(I)-1. */
07920 
07921 /*       NEAR,NEXT,DIST = Garbage. */
07922 
07923 /*       IER = Error indicator: */
07924 /*             IER =  0 if no errors were encountered. */
07925 /*             IER = -1 if N0 < 3 on input. */
07926 /*             IER = -2 if the first three nodes are */
07927 /*                      collinear. */
07928 /*             IER = -3 if Subroutine ADDNOD returns an error */
07929 /*                      flag.  This should not occur. */
07930 
07931 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
07932 /*                                INSERT, INTADD, JRAND, */
07933 /*                                LEFT, LSTPTR, STORE, SWAP, */
07934 /*                                SWPTST, TRFIND */
07935 
07936 /* Intrinsic function called by TRMSH3:  ABS */
07937 
07938 /* *********************************************************** */
07939 
07940 
07941 /* Local parameters: */
07942 
07943 /* D =        (Negative cosine of) distance from node KT to */
07944 /*              node I */
07945 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
07946 /*              respectively */
07947 /* I,J =      Nodal indexes */
07948 /* I0 =       Index of the node preceding I in a sequence of */
07949 /*              unprocessed nodes:  I = NEXT(I0) */
07950 /* KT =       Index of a triangulation node */
07951 /* KU =       Index of an unprocessed node and DO-loop index */
07952 /* LP =       LIST index (pointer) of a neighbor of KT */
07953 /* LPL =      Pointer to the last neighbor of KT */
07954 /* NEXTI =    NEXT(I) */
07955 /* NKU =      NEAR(KU) */
07956 
07957     /* Parameter adjustments */
07958     --dist;
07959     --next;
07960     --near__;
07961     --indx;
07962     --lend;
07963     --z__;
07964     --y;
07965     --x;
07966     --list;
07967     --lptr;
07968     --lcnt;
07969 
07970     /* Function Body */
07971     if (*n0 < 3) {
07972         *n = 0;
07973         *ier = -1;
07974         return 0;
07975     }
07976 
07977 /* Store the first triangle in the linked list. */
07978 
07979     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07980             z__[3])) {
07981 
07982 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
07983 
07984         list[1] = 3;
07985         lptr[1] = 2;
07986         list[2] = -2;
07987         lptr[2] = 1;
07988         lend[1] = 2;
07989 
07990         list[3] = 1;
07991         lptr[3] = 4;
07992         list[4] = -3;
07993         lptr[4] = 3;
07994         lend[2] = 4;
07995 
07996         list[5] = 2;
07997         lptr[5] = 6;
07998         list[6] = -1;
07999         lptr[6] = 5;
08000         lend[3] = 6;
08001 
08002     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08003             y[3], &z__[3])) {
08004 
08005 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
08006 /*     i.e., node 3 lies in the left hemisphere defined by */
08007 /*     arc 1->2. */
08008 
08009         list[1] = 2;
08010         lptr[1] = 2;
08011         list[2] = -3;
08012         lptr[2] = 1;
08013         lend[1] = 2;
08014 
08015         list[3] = 3;
08016         lptr[3] = 4;
08017         list[4] = -1;
08018         lptr[4] = 3;
08019         lend[2] = 4;
08020 
08021         list[5] = 1;
08022         lptr[5] = 6;
08023         list[6] = -2;
08024         lptr[6] = 5;
08025         lend[3] = 6;
08026 
08027 
08028     } else {
08029 
08030 /*   The first three nodes are collinear. */
08031 
08032         *n = 0;
08033         *ier = -2;
08034         return 0;
08035     }
08036 
08037     //printf("pass check colinear\n");
08038 
08039 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08040 
08041     *lnew = 7;
08042     indx[1] = 1;
08043     indx[2] = 2;
08044     indx[3] = 3;
08045     lcnt[1] = 1;
08046     lcnt[2] = 1;
08047     lcnt[3] = 1;
08048     if (*n0 == 3) {
08049         *n = 3;
08050         *ier = 0;
08051         return 0;
08052     }
08053 
08054 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08055 /*   used to obtain an expected-time (N*log(N)) incremental */
08056 /*   algorithm by enabling constant search time for locating */
08057 /*   each new node in the triangulation. */
08058 
08059 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08060 /*   triangulation node closest to KU (used as the starting */
08061 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08062 /*   is an increasing function of the arc length (angular */
08063 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08064 /*   arc length a. */
08065 
08066 /* Since it is necessary to efficiently find the subset of */
08067 /*   unprocessed nodes associated with each triangulation */
08068 /*   node J (those that have J as their NEAR entries), the */
08069 /*   subsets are stored in NEAR and NEXT as follows:  for */
08070 /*   each node J in the triangulation, I = NEAR(J) is the */
08071 /*   first unprocessed node in J's set (with I = 0 if the */
08072 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08073 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08074 /*   set are initially ordered by increasing indexes (which */
08075 /*   maximizes efficiency) but that ordering is not main- */
08076 /*   tained as the data structure is updated. */
08077 
08078 /* Initialize the data structure for the single triangle. */
08079 
08080     near__[1] = 0;
08081     near__[2] = 0;
08082     near__[3] = 0;
08083     for (ku = *n0; ku >= 4; --ku) {
08084         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08085         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08086         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08087         if (d1 <= d2 && d1 <= d3) {
08088             near__[ku] = 1;
08089             dist[ku] = d1;
08090             next[ku] = near__[1];
08091             near__[1] = ku;
08092         } else if (d2 <= d1 && d2 <= d3) {
08093             near__[ku] = 2;
08094             dist[ku] = d2;
08095             next[ku] = near__[2];
08096             near__[2] = ku;
08097         } else {
08098             near__[ku] = 3;
08099             dist[ku] = d3;
08100             next[ku] = near__[3];
08101             near__[3] = ku;
08102         }
08103 /* L1: */
08104     }
08105 
08106 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08107 /*   in the triangulation, and NKU = NEAR(KU). */
08108 
08109     kt = 3;
08110     i__1 = *n0;
08111     for (ku = 4; ku <= i__1; ++ku) {
08112         nku = near__[ku];
08113 
08114 /* Remove KU from the set of unprocessed nodes associated */
08115 /*   with NEAR(KU). */
08116         i__ = nku;
08117         if (near__[i__] == ku) {
08118             near__[i__] = next[ku];
08119         } else {
08120             i__ = near__[i__];
08121 L2:
08122             i0 = i__;
08123             i__ = next[i0];
08124             if (i__ != ku) {
08125                 goto L2;
08126             }
08127             next[i0] = next[ku];
08128         }
08129         near__[ku] = 0;
08130 
08131 /* Bypass duplicate nodes. */
08132 
08133         if (dist[ku] <= *tol - 1.) {
08134             indx[ku] = -nku;
08135             ++lcnt[nku];
08136             goto L6;
08137         }
08138 
08139 
08140 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08141         ++kt;
08142         x[kt] = x[ku];
08143         y[kt] = y[ku];
08144         z__[kt] = z__[ku];
08145         indx[ku] = kt;
08146         lcnt[kt] = 1;
08147         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08148                 , lnew, ier);
08149         if (*ier != 0) {
08150             *n = 0;
08151             *ier = -3;
08152             return 0;
08153         }
08154 
08155 /* Loop on neighbors J of node KT. */
08156 
08157         lpl = lend[kt];
08158         lp = lpl;
08159 L3:
08160         lp = lptr[lp];
08161         j = (i__2 = list[lp], abs(i__2));
08162 
08163 /* Loop on elements I in the sequence of unprocessed nodes */
08164 /*   associated with J:  KT is a candidate for replacing J */
08165 /*   as the nearest triangulation node to I.  The next value */
08166 /*   of I in the sequence, NEXT(I), must be saved before I */
08167 /*   is moved because it is altered by adding I to KT's set. */
08168 
08169         i__ = near__[j];
08170 L4:
08171         if (i__ == 0) {
08172             goto L5;
08173         }
08174         nexti = next[i__];
08175 
08176 /* Test for the distance from I to KT less than the distance */
08177 /*   from I to J. */
08178 
08179         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08180         if (d__ < dist[i__]) {
08181 
08182 /* Replace J by KT as the nearest triangulation node to I: */
08183 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08184 /*   of unprocessed nodes and add it to KT's set. */
08185 
08186             near__[i__] = kt;
08187             dist[i__] = d__;
08188             if (i__ == near__[j]) {
08189                 near__[j] = nexti;
08190             } else {
08191                 next[i0] = nexti;
08192             }
08193             next[i__] = near__[kt];
08194             near__[kt] = i__;
08195         } else {
08196             i0 = i__;
08197         }
08198 
08199 /* Bottom of loop on I. */
08200 
08201         i__ = nexti;
08202         goto L4;
08203 
08204 /* Bottom of loop on neighbors J. */
08205 
08206 L5:
08207         if (lp != lpl) {
08208             goto L3;
08209         }
08210 L6:
08211         ;
08212     }
08213     *n = kt;
08214     *ier = 0;
08215     return 0;
08216 } /* trmsh3_ */
08217 
08218 /* stripack.dbl sent by Robert on 06/03/03 */
08219 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08220         double *y, double *z__, int *list, int *lptr, int
08221         *lend, int *lnew, int *ier)
08222 {
08223     /* Initialized data */
08224 
08225     static double tol = 0.;
08226 
08227     /* System generated locals */
08228     int i__1;
08229 
08230     /* Local variables */
08231     static int l;
08232     static double p[3], b1, b2, b3;
08233     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08234     /* Subroutine */ int swap_(int *, int *, int *,
08235             int *, int *, int *, int *, int *);
08236     static int lpo1s;
08237     /* Subroutine */ int bdyadd_(int *, int *, int *,
08238             int *, int *, int *, int *), intadd_(int *,
08239             int *, int *, int *, int *, int *, int *,
08240             int *), trfind_(int *, double *, int *,
08241             double *, double *, double *, int *, int *,
08242             int *, double *, double *, double *, int *,
08243             int *, int *), covsph_(int *, int *, int *,
08244             int *, int *, int *);
08245     int lstptr_(int *, int *, int *, int *);
08246     long int swptst_(int *, int *, int *, int *,
08247             double *, double *, double *);
08248 
08249 
08250 /* *********************************************************** */
08251 
08252 /*                                              From STRIPACK */
08253 /*                                            Robert J. Renka */
08254 /*                                  Dept. of Computer Science */
08255 /*                                       Univ. of North Texas */
08256 /*                                           renka@cs.unt.edu */
08257 /*                                                   01/08/03 */
08258 
08259 /*   This subroutine adds node K to a triangulation of the */
08260 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08261 /* of the convex hull of nodes 1,...,K. */
08262 
08263 /*   The algorithm consists of the following steps:  node K */
08264 /* is located relative to the triangulation (TRFIND), its */
08265 /* index is added to the data structure (INTADD or BDYADD), */
08266 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08267 /* the arcs opposite K so that all arcs incident on node K */
08268 /* and opposite node K are locally optimal (satisfy the cir- */
08269 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08270 /* input, a Delaunay triangulation will result. */
08271 
08272 
08273 /* On input: */
08274 
08275 /*       NST = Index of a node at which TRFIND begins its */
08276 /*             search.  Search time depends on the proximity */
08277 /*             of this node to K.  If NST < 1, the search is */
08278 /*             begun at node K-1. */
08279 
08280 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08281 /*           new node to be added.  K .GE. 4. */
08282 
08283 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08284 /*               tesian coordinates of the nodes. */
08285 /*               (X(I),Y(I),Z(I)) defines node I for */
08286 /*               I = 1,...,K. */
08287 
08288 /* The above parameters are not altered by this routine. */
08289 
08290 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08291 /*                             the triangulation of nodes 1 */
08292 /*                             to K-1.  The array lengths are */
08293 /*                             assumed to be large enough to */
08294 /*                             add node K.  Refer to Subrou- */
08295 /*                             tine TRMESH. */
08296 
08297 /* On output: */
08298 
08299 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08300 /*                             the addition of node K as the */
08301 /*                             last entry unless IER .NE. 0 */
08302 /*                             and IER .NE. -3, in which case */
08303 /*                             the arrays are not altered. */
08304 
08305 /*       IER = Error indicator: */
08306 /*             IER =  0 if no errors were encountered. */
08307 /*             IER = -1 if K is outside its valid range */
08308 /*                      on input. */
08309 /*             IER = -2 if all nodes (including K) are col- */
08310 /*                      linear (lie on a common geodesic). */
08311 /*             IER =  L if nodes L and K coincide for some */
08312 /*                      L < K.  Refer to TOL below. */
08313 
08314 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08315 /*                                INTADD, JRAND, LSTPTR, */
08316 /*                                STORE, SWAP, SWPTST, */
08317 /*                                TRFIND */
08318 
08319 /* Intrinsic function called by ADDNOD:  ABS */
08320 
08321 /* *********************************************************** */
08322 
08323 
08324 /* Local parameters: */
08325 
08326 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08327 /*              by TRFIND. */
08328 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08329 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08330 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08331 /*              counterclockwise order. */
08332 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08333 /*              be tested for a swap */
08334 /* IST =      Index of node at which TRFIND begins its search */
08335 /* KK =       Local copy of K */
08336 /* KM1 =      K-1 */
08337 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08338 /*              if node K coincides with a vertex */
08339 /* LP =       LIST pointer */
08340 /* LPF =      LIST pointer to the first neighbor of K */
08341 /* LPO1 =     LIST pointer to IO1 */
08342 /* LPO1S =    Saved value of LPO1 */
08343 /* P =        Cartesian coordinates of node K */
08344 /* TOL =      Tolerance defining coincident nodes:  bound on */
08345 /*              the deviation from 1 of the cosine of the */
08346 /*              angle between the nodes. */
08347 /*              Note that |1-cos(A)| is approximately A*A/2. */
08348 
08349     /* Parameter adjustments */
08350     --lend;
08351     --z__;
08352     --y;
08353     --x;
08354     --list;
08355     --lptr;
08356 
08357     /* Function Body */
08358 
08359     kk = *k;
08360     if (kk < 4) {
08361         goto L3;
08362     }
08363 
08364 /* Initialization: */
08365     km1 = kk - 1;
08366     ist = *nst;
08367     if (ist < 1) {
08368         ist = km1;
08369     }
08370     p[0] = x[kk];
08371     p[1] = y[kk];
08372     p[2] = z__[kk];
08373 
08374 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08375 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08376 /*   from node K. */
08377     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08378             , &b1, &b2, &b3, &i1, &i2, &i3);
08379 
08380 /*   Test for collinear or (nearly) duplicate nodes. */
08381 
08382     if (i1 == 0) {
08383         goto L4;
08384     }
08385     l = i1;
08386     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08387         goto L5;
08388     }
08389     l = i2;
08390     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08391         goto L5;
08392     }
08393     if (i3 != 0) {
08394         l = i3;
08395         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08396             goto L5;
08397         }
08398         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08399     } else {
08400         if (i1 != i2) {
08401             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08402         } else {
08403             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08404         }
08405     }
08406     *ier = 0;
08407 
08408 /* Initialize variables for optimization of the */
08409 /*   triangulation. */
08410     lp = lend[kk];
08411     lpf = lptr[lp];
08412     io2 = list[lpf];
08413     lpo1 = lptr[lpf];
08414     io1 = (i__1 = list[lpo1], abs(i__1));
08415 
08416 /* Begin loop:  find the node opposite K. */
08417 
08418 L1:
08419     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08420     if (list[lp] < 0) {
08421         goto L2;
08422     }
08423     lp = lptr[lp];
08424     in1 = (i__1 = list[lp], abs(i__1));
08425 
08426 /* Swap test:  if a swap occurs, two new arcs are */
08427 /*             opposite K and must be tested. */
08428 
08429     lpo1s = lpo1;
08430     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08431         goto L2;
08432     }
08433     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08434     if (lpo1 == 0) {
08435 
08436 /*   A swap is not possible because KK and IN1 are already */
08437 /*     adjacent.  This error in SWPTST only occurs in the */
08438 /*     neutral case and when there are nearly duplicate */
08439 /*     nodes. */
08440 
08441         lpo1 = lpo1s;
08442         goto L2;
08443     }
08444     io1 = in1;
08445     goto L1;
08446 
08447 /* No swap occurred.  Test for termination and reset */
08448 /*   IO2 and IO1. */
08449 
08450 L2:
08451     if (lpo1 == lpf || list[lpo1] < 0) {
08452         return 0;
08453     }
08454     io2 = io1;
08455     lpo1 = lptr[lpo1];
08456     io1 = (i__1 = list[lpo1], abs(i__1));
08457     goto L1;
08458 
08459 /* KK < 4. */
08460 
08461 L3:
08462     *ier = -1;
08463     return 0;
08464 
08465 /* All nodes are collinear. */
08466 
08467 L4:
08468     *ier = -2;
08469     return 0;
08470 
08471 /* Nodes L and K coincide. */
08472 
08473 L5:
08474     *ier = l;
08475     return 0;
08476 } /* addnod_ */
08477 
08478 double angle_(double *v1, double *v2, double *v3)
08479 {
08480     /* System generated locals */
08481     double ret_val;
08482 
08483     /* Builtin functions */
08484     //double sqrt(double), acos(double);
08485 
08486     /* Local variables */
08487     static double a;
08488     static int i__;
08489     static double ca, s21, s23, u21[3], u23[3];
08490 
08491 
08492 /* *********************************************************** */
08493 
08494 /*                                              From STRIPACK */
08495 /*                                            Robert J. Renka */
08496 /*                                  Dept. of Computer Science */
08497 /*                                       Univ. of North Texas */
08498 /*                                           renka@cs.unt.edu */
08499 /*                                                   06/03/03 */
08500 
08501 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08502 /* face of the unit sphere, this function returns the */
08503 /* interior angle at V2 -- the dihedral angle between the */
08504 /* plane defined by V2 and V3 (and the origin) and the plane */
08505 /* defined by V2 and V1 or, equivalently, the angle between */
08506 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08507 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08508 /* wise.  The surface area of a spherical polygon with CCW- */
08509 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08510 /* Asum is the sum of the m interior angles computed from the */
08511 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08512 /* (Vm-1,Vm,V1). */
08513 
08514 
08515 /* On input: */
08516 
08517 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08518 /*                  sian coordinates of unit vectors.  These */
08519 /*                  vectors, if nonzero, are implicitly */
08520 /*                  scaled to have length 1. */
08521 
08522 /* Input parameters are not altered by this function. */
08523 
08524 /* On output: */
08525 
08526 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08527 /*               V2 X V3 = 0. */
08528 
08529 /* Module required by ANGLE:  LEFT */
08530 
08531 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08532 
08533 /* *********************************************************** */
08534 
08535 
08536 /* Local parameters: */
08537 
08538 /* A =       Interior angle at V2 */
08539 /* CA =      cos(A) */
08540 /* I =       DO-loop index and index for U21 and U23 */
08541 /* S21,S23 = Sum of squared components of U21 and U23 */
08542 /* U21,U23 = Unit normal vectors to the planes defined by */
08543 /*             pairs of triangle vertices */
08544 
08545 
08546 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08547 
08548     /* Parameter adjustments */
08549     --v3;
08550     --v2;
08551     --v1;
08552 
08553     /* Function Body */
08554     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08555     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08556     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08557 
08558     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08559     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08560     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08561 
08562 /* Normalize U21 and U23 to unit vectors. */
08563 
08564     s21 = 0.;
08565     s23 = 0.;
08566     for (i__ = 1; i__ <= 3; ++i__) {
08567         s21 += u21[i__ - 1] * u21[i__ - 1];
08568         s23 += u23[i__ - 1] * u23[i__ - 1];
08569 /* L1: */
08570     }
08571 
08572 /* Test for a degenerate triangle associated with collinear */
08573 /*   vertices. */
08574 
08575     if (s21 == 0. || s23 == 0.) {
08576         ret_val = 0.;
08577         return ret_val;
08578     }
08579     s21 = sqrt(s21);
08580     s23 = sqrt(s23);
08581     for (i__ = 1; i__ <= 3; ++i__) {
08582         u21[i__ - 1] /= s21;
08583         u23[i__ - 1] /= s23;
08584 /* L2: */
08585     }
08586 
08587 /* Compute the angle A between normals: */
08588 
08589 /*   CA = cos(A) = <U21,U23> */
08590 
08591     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08592     if (ca < -1.) {
08593         ca = -1.;
08594     }
08595     if (ca > 1.) {
08596         ca = 1.;
08597     }
08598     a = acos(ca);
08599 
08600 /* Adjust A to the interior angle:  A > Pi iff */
08601 /*   V3 Right V1->V2. */
08602 
08603     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08604             , &v3[3])) {
08605         a = acos(-1.) * 2. - a;
08606     }
08607     ret_val = a;
08608     return ret_val;
08609 } /* angle_ */
08610 
08611 double areas_(double *v1, double *v2, double *v3)
08612 {
08613     /* System generated locals */
08614     double ret_val;
08615 
08616     /* Builtin functions */
08617     //double sqrt(double), acos(double);
08618 
08619     /* Local variables */
08620     static int i__;
08621     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08622             ca2, ca3;
08623 
08624 
08625 /* *********************************************************** */
08626 
08627 /*                                              From STRIPACK */
08628 /*                                            Robert J. Renka */
08629 /*                                  Dept. of Computer Science */
08630 /*                                       Univ. of North Texas */
08631 /*                                           renka@cs.unt.edu */
08632 /*                                                   06/22/98 */
08633 
08634 /*   This function returns the area of a spherical triangle */
08635 /* on the unit sphere. */
08636 
08637 
08638 /* On input: */
08639 
08640 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08641 /*                  sian coordinates of unit vectors (the */
08642 /*                  three triangle vertices in any order). */
08643 /*                  These vectors, if nonzero, are implicitly */
08644 /*                  scaled to have length 1. */
08645 
08646 /* Input parameters are not altered by this function. */
08647 
08648 /* On output: */
08649 
08650 /*       AREAS = Area of the spherical triangle defined by */
08651 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08652 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08653 /*               if and only if V1, V2, and V3 lie in (or */
08654 /*               close to) a plane containing the origin. */
08655 
08656 /* Modules required by AREAS:  None */
08657 
08658 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08659 
08660 /* *********************************************************** */
08661 
08662 
08663 /* Local parameters: */
08664 
08665 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08666 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08667 /* I =           DO-loop index and index for Uij */
08668 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08669 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08670 /*                 pairs of triangle vertices */
08671 
08672 
08673 /* Compute cross products Uij = Vi X Vj. */
08674 
08675     /* Parameter adjustments */
08676     --v3;
08677     --v2;
08678     --v1;
08679 
08680     /* Function Body */
08681     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08682     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08683     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08684 
08685     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08686     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08687     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08688 
08689     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08690     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08691     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08692 
08693 /* Normalize Uij to unit vectors. */
08694 
08695     s12 = 0.;
08696     s23 = 0.;
08697     s31 = 0.;
08698     for (i__ = 1; i__ <= 3; ++i__) {
08699         s12 += u12[i__ - 1] * u12[i__ - 1];
08700         s23 += u23[i__ - 1] * u23[i__ - 1];
08701         s31 += u31[i__ - 1] * u31[i__ - 1];
08702 /* L2: */
08703     }
08704 
08705 /* Test for a degenerate triangle associated with collinear */
08706 /*   vertices. */
08707 
08708     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08709         ret_val = 0.;
08710         return ret_val;
08711     }
08712     s12 = sqrt(s12);
08713     s23 = sqrt(s23);
08714     s31 = sqrt(s31);
08715     for (i__ = 1; i__ <= 3; ++i__) {
08716         u12[i__ - 1] /= s12;
08717         u23[i__ - 1] /= s23;
08718         u31[i__ - 1] /= s31;
08719 /* L3: */
08720     }
08721 
08722 /* Compute interior angles Ai as the dihedral angles between */
08723 /*   planes: */
08724 /*           CA1 = cos(A1) = -<U12,U31> */
08725 /*           CA2 = cos(A2) = -<U23,U12> */
08726 /*           CA3 = cos(A3) = -<U31,U23> */
08727 
08728     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08729     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08730     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08731     if (ca1 < -1.) {
08732         ca1 = -1.;
08733     }
08734     if (ca1 > 1.) {
08735         ca1 = 1.;
08736     }
08737     if (ca2 < -1.) {
08738         ca2 = -1.;
08739     }
08740     if (ca2 > 1.) {
08741         ca2 = 1.;
08742     }
08743     if (ca3 < -1.) {
08744         ca3 = -1.;
08745     }
08746     if (ca3 > 1.) {
08747         ca3 = 1.;
08748     }
08749     a1 = acos(ca1);
08750     a2 = acos(ca2);
08751     a3 = acos(ca3);
08752 
08753 /* Compute AREAS = A1 + A2 + A3 - PI. */
08754 
08755     ret_val = a1 + a2 + a3 - acos(-1.);
08756     if (ret_val < 0.) {
08757         ret_val = 0.;
08758     }
08759     return ret_val;
08760 } /* areas_ */
08761 
08762 //double areas_(double *, double *, double *);
08763 
08764 double Util::areav_(int *k, int *n, double *x, double *y,
08765         double *z__, int *list, int *lptr, int *lend, int
08766         *ier)
08767 {
08768     /* Initialized data */
08769 
08770     static double amax = 6.28;
08771 
08772     /* System generated locals */
08773     double ret_val;
08774 
08775     /* Local variables */
08776     static double a, c0[3], c2[3], c3[3];
08777     static int n1, n2, n3;
08778     static double v1[3], v2[3], v3[3];
08779     static int lp, lpl, ierr;
08780     static double asum;
08781     static long int first;
08782 
08783 
08784 /* *********************************************************** */
08785 
08786 /*                                            Robert J. Renka */
08787 /*                                  Dept. of Computer Science */
08788 /*                                       Univ. of North Texas */
08789 /*                                           renka@cs.unt.edu */
08790 /*                                                   10/25/02 */
08791 
08792 /*   Given a Delaunay triangulation and the index K of an */
08793 /* interior node, this subroutine returns the (surface) area */
08794 /* of the Voronoi region associated with node K.  The Voronoi */
08795 /* region is the polygon whose vertices are the circumcenters */
08796 /* of the triangles that contain node K, where a triangle */
08797 /* circumcenter is the point (unit vector) lying at the same */
08798 /* angular distance from the three vertices and contained in */
08799 /* the same hemisphere as the vertices. */
08800 
08801 
08802 /* On input: */
08803 
08804 /*       K = Nodal index in the range 1 to N. */
08805 
08806 /*       N = Number of nodes in the triangulation.  N > 3. */
08807 
08808 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08809 /*               coordinates of the nodes (unit vectors). */
08810 
08811 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08812 /*                        gulation.  Refer to Subroutine */
08813 /*                        TRMESH. */
08814 
08815 /* Input parameters are not altered by this function. */
08816 
08817 /* On output: */
08818 
08819 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08820 /*               in which case AREAV = 0. */
08821 
08822 /*       IER = Error indicator: */
08823 /*             IER = 0 if no errors were encountered. */
08824 /*             IER = 1 if K or N is outside its valid range */
08825 /*                     on input. */
08826 /*             IER = 2 if K indexes a boundary node. */
08827 /*             IER = 3 if an error flag is returned by CIRCUM */
08828 /*                     (null triangle). */
08829 /*             IER = 4 if AREAS returns a value greater than */
08830 /*                     AMAX (defined below). */
08831 
08832 /* Modules required by AREAV:  AREAS, CIRCUM */
08833 
08834 /* *********************************************************** */
08835 
08836 
08837 /* Maximum valid triangle area is less than 2*Pi: */
08838 
08839     /* Parameter adjustments */
08840     --lend;
08841     --z__;
08842     --y;
08843     --x;
08844     --list;
08845     --lptr;
08846 
08847     /* Function Body */
08848 
08849 /* Test for invalid input. */
08850 
08851     if (*k < 1 || *k > *n || *n <= 3) {
08852         goto L11;
08853     }
08854 
08855 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08856 /*   FIRST = TRUE only for the first triangle. */
08857 /*   The Voronoi region area is accumulated in ASUM. */
08858 
08859     n1 = *k;
08860     v1[0] = x[n1];
08861     v1[1] = y[n1];
08862     v1[2] = z__[n1];
08863     lpl = lend[n1];
08864     n3 = list[lpl];
08865     if (n3 < 0) {
08866         goto L12;
08867     }
08868     lp = lpl;
08869     first = TRUE_;
08870     asum = 0.;
08871 
08872 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08873 
08874 L1:
08875     n2 = n3;
08876     lp = lptr[lp];
08877     n3 = list[lp];
08878     v2[0] = x[n2];
08879     v2[1] = y[n2];
08880     v2[2] = z__[n2];
08881     v3[0] = x[n3];
08882     v3[1] = y[n3];
08883     v3[2] = z__[n3];
08884     if (first) {
08885 
08886 /* First triangle:  compute the circumcenter C3 and save a */
08887 /*   copy in C0. */
08888 
08889         circum_(v1, v2, v3, c3, &ierr);
08890         if (ierr != 0) {
08891             goto L13;
08892         }
08893         c0[0] = c3[0];
08894         c0[1] = c3[1];
08895         c0[2] = c3[2];
08896         first = FALSE_;
08897     } else {
08898 
08899 /* Set C2 to C3, compute the new circumcenter C3, and compute */
08900 /*   the area A of triangle (V1,C2,C3). */
08901 
08902         c2[0] = c3[0];
08903         c2[1] = c3[1];
08904         c2[2] = c3[2];
08905         circum_(v1, v2, v3, c3, &ierr);
08906         if (ierr != 0) {
08907             goto L13;
08908         }
08909         a = areas_(v1, c2, c3);
08910         if (a > amax) {
08911             goto L14;
08912         }
08913         asum += a;
08914     }
08915 
08916 /* Bottom on loop on neighbors of K. */
08917 
08918     if (lp != lpl) {
08919         goto L1;
08920     }
08921 
08922 /* Compute the area of triangle (V1,C3,C0). */
08923 
08924     a = areas_(v1, c3, c0);
08925     if (a > amax) {
08926         goto L14;
08927     }
08928     asum += a;
08929 
08930 /* No error encountered. */
08931 
08932     *ier = 0;
08933     ret_val = asum;
08934     return ret_val;
08935 
08936 /* Invalid input. */
08937 
08938 L11:
08939     *ier = 1;
08940     ret_val = 0.;
08941     return ret_val;
08942 
08943 /* K indexes a boundary node. */
08944 
08945 L12:
08946     *ier = 2;
08947     ret_val = 0.;
08948     return ret_val;
08949 
08950 /* Error in CIRCUM. */
08951 
08952 L13:
08953     *ier = 3;
08954     ret_val = 0.;
08955     return ret_val;
08956 
08957 /* AREAS value larger than AMAX. */
08958 
08959 L14:
08960     *ier = 4;
08961     ret_val = 0.;
08962     return ret_val;
08963 } /* areav_ */
08964 
08965 double areav_new__(int *k, int *n, double *x, double *y,
08966         double *z__, int *list, int *lptr, int *lend, int
08967         *ier)
08968 {
08969     /* System generated locals */
08970     double ret_val = 0;
08971 
08972     /* Builtin functions */
08973     //double acos(double);
08974 
08975     /* Local variables */
08976     static int m;
08977     static double c1[3], c2[3], c3[3];
08978     static int n1, n2, n3;
08979     static double v1[3], v2[3], v3[3];
08980     static int lp;
08981     static double c1s[3], c2s[3];
08982     static int lpl, ierr;
08983     static double asum;
08984     double angle_(double *, double *, double *);
08985     static float areav;
08986 
08987 
08988 /* *********************************************************** */
08989 
08990 /*                                            Robert J. Renka */
08991 /*                                  Dept. of Computer Science */
08992 /*                                       Univ. of North Texas */
08993 /*                                           renka@cs.unt.edu */
08994 /*                                                   06/03/03 */
08995 
08996 /*   Given a Delaunay triangulation and the index K of an */
08997 /* interior node, this subroutine returns the (surface) area */
08998 /* of the Voronoi region associated with node K.  The Voronoi */
08999 /* region is the polygon whose vertices are the circumcenters */
09000 /* of the triangles that contain node K, where a triangle */
09001 /* circumcenter is the point (unit vector) lying at the same */
09002 /* angular distance from the three vertices and contained in */
09003 /* the same hemisphere as the vertices.  The Voronoi region */
09004 /* area is computed as Asum-(m-2)*Pi, where m is the number */
09005 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
09006 /* of interior angles at the vertices. */
09007 
09008 
09009 /* On input: */
09010 
09011 /*       K = Nodal index in the range 1 to N. */
09012 
09013 /*       N = Number of nodes in the triangulation.  N > 3. */
09014 
09015 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09016 /*               coordinates of the nodes (unit vectors). */
09017 
09018 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09019 /*                        gulation.  Refer to Subroutine */
09020 /*                        TRMESH. */
09021 
09022 /* Input parameters are not altered by this function. */
09023 
09024 /* On output: */
09025 
09026 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09027 /*               in which case AREAV = 0. */
09028 
09029 /*       IER = Error indicator: */
09030 /*             IER = 0 if no errors were encountered. */
09031 /*             IER = 1 if K or N is outside its valid range */
09032 /*                     on input. */
09033 /*             IER = 2 if K indexes a boundary node. */
09034 /*             IER = 3 if an error flag is returned by CIRCUM */
09035 /*                     (null triangle). */
09036 
09037 /* Modules required by AREAV:  ANGLE, CIRCUM */
09038 
09039 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09040 
09041 /* *********************************************************** */
09042 
09043 
09044 /* Test for invalid input. */
09045 
09046     /* Parameter adjustments */
09047     --lend;
09048     --z__;
09049     --y;
09050     --x;
09051     --list;
09052     --lptr;
09053 
09054     /* Function Body */
09055     if (*k < 1 || *k > *n || *n <= 3) {
09056         goto L11;
09057     }
09058 
09059 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09060 /*   The number of neighbors and the sum of interior angles */
09061 /*   are accumulated in M and ASUM, respectively. */
09062 
09063     n1 = *k;
09064     v1[0] = x[n1];
09065     v1[1] = y[n1];
09066     v1[2] = z__[n1];
09067     lpl = lend[n1];
09068     n3 = list[lpl];
09069     if (n3 < 0) {
09070         goto L12;
09071     }
09072     lp = lpl;
09073     m = 0;
09074     asum = 0.;
09075 
09076 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09077 
09078 L1:
09079     ++m;
09080     n2 = n3;
09081     lp = lptr[lp];
09082     n3 = list[lp];
09083     v2[0] = x[n2];
09084     v2[1] = y[n2];
09085     v2[2] = z__[n2];
09086     v3[0] = x[n3];
09087     v3[1] = y[n3];
09088     v3[2] = z__[n3];
09089     if (m == 1) {
09090 
09091 /* First triangle:  compute the circumcenter C2 and save a */
09092 /*   copy in C1S. */
09093 
09094         circum_(v1, v2, v3, c2, &ierr);
09095         if (ierr != 0) {
09096             goto L13;
09097         }
09098         c1s[0] = c2[0];
09099         c1s[1] = c2[1];
09100         c1s[2] = c2[2];
09101     } else if (m == 2) {
09102 
09103 /* Second triangle:  compute the circumcenter C3 and save a */
09104 /*   copy in C2S. */
09105 
09106         circum_(v1, v2, v3, c3, &ierr);
09107         if (ierr != 0) {
09108             goto L13;
09109         }
09110         c2s[0] = c3[0];
09111         c2s[1] = c3[1];
09112         c2s[2] = c3[2];
09113     } else {
09114 
09115 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09116 /*   C3, and compute the interior angle at C2 from the */
09117 /*   sequence of vertices (C1,C2,C3). */
09118 
09119         c1[0] = c2[0];
09120         c1[1] = c2[1];
09121         c1[2] = c2[2];
09122         c2[0] = c3[0];
09123         c2[1] = c3[1];
09124         c2[2] = c3[2];
09125         circum_(v1, v2, v3, c3, &ierr);
09126         if (ierr != 0) {
09127             goto L13;
09128         }
09129         asum += angle_(c1, c2, c3);
09130     }
09131 
09132 /* Bottom on loop on neighbors of K. */
09133 
09134     if (lp != lpl) {
09135         goto L1;
09136     }
09137 
09138 /* C3 is the last vertex.  Compute its interior angle from */
09139 /*   the sequence (C2,C3,C1S). */
09140 
09141     asum += angle_(c2, c3, c1s);
09142 
09143 /* Compute the interior angle at C1S from */
09144 /*   the sequence (C3,C1S,C2S). */
09145 
09146     asum += angle_(c3, c1s, c2s);
09147 
09148 /* No error encountered. */
09149 
09150     *ier = 0;
09151     ret_val = asum - (double) (m - 2) * acos(-1.);
09152     return ret_val;
09153 
09154 /* Invalid input. */
09155 
09156 L11:
09157     *ier = 1;
09158     areav = 0.f;
09159     return ret_val;
09160 
09161 /* K indexes a boundary node. */
09162 
09163 L12:
09164     *ier = 2;
09165     areav = 0.f;
09166     return ret_val;
09167 
09168 /* Error in CIRCUM. */
09169 
09170 L13:
09171     *ier = 3;
09172     areav = 0.f;
09173     return ret_val;
09174 } /* areav_new__ */
09175 
09176 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09177         list, int *lptr, int *lend, int *lnew)
09178 {
09179     static int k, n1, n2, lp, lsav, nsav, next;
09180     /* Subroutine */ int insert_(int *, int *, int *,
09181             int *, int *);
09182 
09183 
09184 /* *********************************************************** */
09185 
09186 /*                                              From STRIPACK */
09187 /*                                            Robert J. Renka */
09188 /*                                  Dept. of Computer Science */
09189 /*                                       Univ. of North Texas */
09190 /*                                           renka@cs.unt.edu */
09191 /*                                                   07/11/96 */
09192 
09193 /*   This subroutine adds a boundary node to a triangulation */
09194 /* of a set of KK-1 points on the unit sphere.  The data */
09195 /* structure is updated with the insertion of node KK, but no */
09196 /* optimization is performed. */
09197 
09198 /*   This routine is identical to the similarly named routine */
09199 /* in TRIPACK. */
09200 
09201 
09202 /* On input: */
09203 
09204 /*       KK = Index of a node to be connected to the sequence */
09205 /*            of all visible boundary nodes.  KK .GE. 1 and */
09206 /*            KK must not be equal to I1 or I2. */
09207 
09208 /*       I1 = First (rightmost as viewed from KK) boundary */
09209 /*            node in the triangulation that is visible from */
09210 /*            node KK (the line segment KK-I1 intersects no */
09211 /*            arcs. */
09212 
09213 /*       I2 = Last (leftmost) boundary node that is visible */
09214 /*            from node KK.  I1 and I2 may be determined by */
09215 /*            Subroutine TRFIND. */
09216 
09217 /* The above parameters are not altered by this routine. */
09218 
09219 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09220 /*                             created by Subroutine TRMESH. */
09221 /*                             Nodes I1 and I2 must be in- */
09222 /*                             cluded in the triangulation. */
09223 
09224 /* On output: */
09225 
09226 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09227 /*                             the addition of node KK.  Node */
09228 /*                             KK is connected to I1, I2, and */
09229 /*                             all boundary nodes in between. */
09230 
09231 /* Module required by BDYADD:  INSERT */
09232 
09233 /* *********************************************************** */
09234 
09235 
09236 /* Local parameters: */
09237 
09238 /* K =     Local copy of KK */
09239 /* LP =    LIST pointer */
09240 /* LSAV =  LIST pointer */
09241 /* N1,N2 = Local copies of I1 and I2, respectively */
09242 /* NEXT =  Boundary node visible from K */
09243 /* NSAV =  Boundary node visible from K */
09244 
09245     /* Parameter adjustments */
09246     --lend;
09247     --lptr;
09248     --list;
09249 
09250     /* Function Body */
09251     k = *kk;
09252     n1 = *i1;
09253     n2 = *i2;
09254 
09255 /* Add K as the last neighbor of N1. */
09256 
09257     lp = lend[n1];
09258     lsav = lptr[lp];
09259     lptr[lp] = *lnew;
09260     list[*lnew] = -k;
09261     lptr[*lnew] = lsav;
09262     lend[n1] = *lnew;
09263     ++(*lnew);
09264     next = -list[lp];
09265     list[lp] = next;
09266     nsav = next;
09267 
09268 /* Loop on the remaining boundary nodes between N1 and N2, */
09269 /*   adding K as the first neighbor. */
09270 
09271 L1:
09272     lp = lend[next];
09273     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09274     if (next == n2) {
09275         goto L2;
09276     }
09277     next = -list[lp];
09278     list[lp] = next;
09279     goto L1;
09280 
09281 /* Add the boundary nodes between N1 and N2 as neighbors */
09282 /*   of node K. */
09283 
09284 L2:
09285     lsav = *lnew;
09286     list[*lnew] = n1;
09287     lptr[*lnew] = *lnew + 1;
09288     ++(*lnew);
09289     next = nsav;
09290 
09291 L3:
09292     if (next == n2) {
09293         goto L4;
09294     }
09295     list[*lnew] = next;
09296     lptr[*lnew] = *lnew + 1;
09297     ++(*lnew);
09298     lp = lend[next];
09299     next = list[lp];
09300     goto L3;
09301 
09302 L4:
09303     list[*lnew] = -n2;
09304     lptr[*lnew] = lsav;
09305     lend[k] = *lnew;
09306     ++(*lnew);
09307     return 0;
09308 } /* bdyadd_ */
09309 
09310 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09311         int *lend, int *nodes, int *nb, int *na, int *nt)
09312 {
09313     /* System generated locals */
09314     int i__1;
09315 
09316     /* Local variables */
09317     static int k, n0, lp, nn, nst;
09318 
09319 
09320 /* *********************************************************** */
09321 
09322 /*                                              From STRIPACK */
09323 /*                                            Robert J. Renka */
09324 /*                                  Dept. of Computer Science */
09325 /*                                       Univ. of North Texas */
09326 /*                                           renka@cs.unt.edu */
09327 /*                                                   06/26/96 */
09328 
09329 /*   Given a triangulation of N nodes on the unit sphere */
09330 /* created by Subroutine TRMESH, this subroutine returns an */
09331 /* array containing the indexes (if any) of the counterclock- */
09332 /* wise-ordered sequence of boundary nodes -- the nodes on */
09333 /* the boundary of the convex hull of the set of nodes.  (The */
09334 /* boundary is empty if the nodes do not lie in a single */
09335 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09336 /* triangles are also returned. */
09337 
09338 
09339 /* On input: */
09340 
09341 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09342 
09343 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09344 /*                        gulation.  Refer to Subroutine */
09345 /*                        TRMESH. */
09346 
09347 /* The above parameters are not altered by this routine. */
09348 
09349 /*       NODES = int array of length at least NB */
09350 /*               (NB .LE. N). */
09351 
09352 /* On output: */
09353 
09354 /*       NODES = Ordered sequence of boundary node indexes */
09355 /*               in the range 1 to N (in the first NB loca- */
09356 /*               tions). */
09357 
09358 /*       NB = Number of boundary nodes. */
09359 
09360 /*       NA,NT = Number of arcs and triangles, respectively, */
09361 /*               in the triangulation. */
09362 
09363 /* Modules required by BNODES:  None */
09364 
09365 /* *********************************************************** */
09366 
09367 
09368 /* Local parameters: */
09369 
09370 /* K =   NODES index */
09371 /* LP =  LIST pointer */
09372 /* N0 =  Boundary node to be added to NODES */
09373 /* NN =  Local copy of N */
09374 /* NST = First element of nodes (arbitrarily chosen to be */
09375 /*         the one with smallest index) */
09376 
09377     /* Parameter adjustments */
09378     --lend;
09379     --list;
09380     --lptr;
09381     --nodes;
09382 
09383     /* Function Body */
09384     nn = *n;
09385 
09386 /* Search for a boundary node. */
09387 
09388     i__1 = nn;
09389     for (nst = 1; nst <= i__1; ++nst) {
09390         lp = lend[nst];
09391         if (list[lp] < 0) {
09392             goto L2;
09393         }
09394 /* L1: */
09395     }
09396 
09397 /* The triangulation contains no boundary nodes. */
09398 
09399     *nb = 0;
09400     *na = (nn - 2) * 3;
09401     *nt = nn - (2<<1);
09402     return 0;
09403 
09404 /* NST is the first boundary node encountered.  Initialize */
09405 /*   for traversal of the boundary. */
09406 
09407 L2:
09408     nodes[1] = nst;
09409     k = 1;
09410     n0 = nst;
09411 
09412 /* Traverse the boundary in counterclockwise order. */
09413 
09414 L3:
09415     lp = lend[n0];
09416     lp = lptr[lp];
09417     n0 = list[lp];
09418     if (n0 == nst) {
09419         goto L4;
09420     }
09421     ++k;
09422     nodes[k] = n0;
09423     goto L3;
09424 
09425 /* Store the counts. */
09426 
09427 L4:
09428     *nb = k;
09429     *nt = (*n << 1) - *nb - 2;
09430     *na = *nt + *n - 1;
09431     return 0;
09432 } /* bnodes_ */
09433 
09434 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09435         int *ier)
09436 {
09437     /* System generated locals */
09438     int i__1;
09439 
09440     /* Builtin functions */
09441     //double atan(double), cos(double), sin(double);
09442 
09443     /* Local variables */
09444     static double a, c__;
09445     static int i__;
09446     static double s;
09447     static int k2, k3;
09448     static double x0, y0;
09449     static int kk, np1;
09450 
09451 
09452 /* *********************************************************** */
09453 
09454 /*                                              From STRIPACK */
09455 /*                                            Robert J. Renka */
09456 /*                                  Dept. of Computer Science */
09457 /*                                       Univ. of North Texas */
09458 /*                                           renka@cs.unt.edu */
09459 /*                                                   04/06/90 */
09460 
09461 /*   This subroutine computes the coordinates of a sequence */
09462 /* of N equally spaced points on the unit circle centered at */
09463 /* (0,0).  An N-sided polygonal approximation to the circle */
09464 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09465 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09466 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09467 /* is 2*PI*R, where R is the radius of the circle in device */
09468 /* coordinates. */
09469 
09470 
09471 /* On input: */
09472 
09473 /*       K = Number of points in each quadrant, defining N as */
09474 /*           4K.  K .GE. 1. */
09475 
09476 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09477 
09478 /* K is not altered by this routine. */
09479 
09480 /* On output: */
09481 
09482 /*       XC,YC = Cartesian coordinates of the points on the */
09483 /*               unit circle in the first N+1 locations. */
09484 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09485 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09486 /*               and YC(N+1) = YC(1). */
09487 
09488 /*       IER = Error indicator: */
09489 /*             IER = 0 if no errors were encountered. */
09490 /*             IER = 1 if K < 1 on input. */
09491 
09492 /* Modules required by CIRCLE:  None */
09493 
09494 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09495 /*                                          SIN */
09496 
09497 /* *********************************************************** */
09498 
09499 
09500 /* Local parameters: */
09501 
09502 /* I =     DO-loop index and index for XC and YC */
09503 /* KK =    Local copy of K */
09504 /* K2 =    K*2 */
09505 /* K3 =    K*3 */
09506 /* NP1 =   N+1 = 4*K + 1 */
09507 /* A =     Angular separation between adjacent points */
09508 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09509 /*           rotation through angle A */
09510 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09511 /*           circle in the first quadrant */
09512 
09513     /* Parameter adjustments */
09514     --yc;
09515     --xc;
09516 
09517     /* Function Body */
09518     kk = *k;
09519     k2 = kk << 1;
09520     k3 = kk * 3;
09521     np1 = (kk << 2) + 1;
09522 
09523 /* Test for invalid input, compute A, C, and S, and */
09524 /*   initialize (X0,Y0) to (1,0). */
09525 
09526     if (kk < 1) {
09527         goto L2;
09528     }
09529     a = atan(1.) * 2. / (double) kk;
09530     c__ = cos(a);
09531     s = sin(a);
09532     x0 = 1.;
09533     y0 = 0.;
09534 
09535 /* Loop on points (X0,Y0) in the first quadrant, storing */
09536 /*   the point and its reflections about the x axis, the */
09537 /*   y axis, and the line y = -x. */
09538 
09539     i__1 = kk;
09540     for (i__ = 1; i__ <= i__1; ++i__) {
09541         xc[i__] = x0;
09542         yc[i__] = y0;
09543         xc[i__ + kk] = -y0;
09544         yc[i__ + kk] = x0;
09545         xc[i__ + k2] = -x0;
09546         yc[i__ + k2] = -y0;
09547         xc[i__ + k3] = y0;
09548         yc[i__ + k3] = -x0;
09549 
09550 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09551 
09552         x0 = c__ * x0 - s * y0;
09553         y0 = s * x0 + c__ * y0;
09554 /* L1: */
09555     }
09556 
09557 /* Store the coordinates of the first point as the last */
09558 /*   point. */
09559 
09560     xc[np1] = xc[1];
09561     yc[np1] = yc[1];
09562     *ier = 0;
09563     return 0;
09564 
09565 /* K < 1. */
09566 
09567 L2:
09568     *ier = 1;
09569     return 0;
09570 } /* circle_ */
09571 
09572 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09573         double *c__, int *ier)
09574 {
09575     /* Builtin functions */
09576     //double sqrt(double);
09577 
09578     /* Local variables */
09579     static int i__;
09580     static double e1[3], e2[3], cu[3], cnorm;
09581 
09582 
09583 /* *********************************************************** */
09584 
09585 /*                                              From STRIPACK */
09586 /*                                            Robert J. Renka */
09587 /*                                  Dept. of Computer Science */
09588 /*                                       Univ. of North Texas */
09589 /*                                           renka@cs.unt.edu */
09590 /*                                                   10/27/02 */
09591 
09592 /*   This subroutine returns the circumcenter of a spherical */
09593 /* triangle on the unit sphere:  the point on the sphere sur- */
09594 /* face that is equally distant from the three triangle */
09595 /* vertices and lies in the same hemisphere, where distance */
09596 /* is taken to be arc-length on the sphere surface. */
09597 
09598 
09599 /* On input: */
09600 
09601 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09602 /*                  sian coordinates of the three triangle */
09603 /*                  vertices (unit vectors) in CCW order. */
09604 
09605 /* The above parameters are not altered by this routine. */
09606 
09607 /*       C = Array of length 3. */
09608 
09609 /* On output: */
09610 
09611 /*       C = Cartesian coordinates of the circumcenter unless */
09612 /*           IER > 0, in which case C is not defined.  C = */
09613 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09614 
09615 /*       IER = Error indicator: */
09616 /*             IER = 0 if no errors were encountered. */
09617 /*             IER = 1 if V1, V2, and V3 lie on a common */
09618 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09619 /*             (The vertices are not tested for validity.) */
09620 
09621 /* Modules required by CIRCUM:  None */
09622 
09623 /* Intrinsic function called by CIRCUM:  SQRT */
09624 
09625 /* *********************************************************** */
09626 
09627 
09628 /* Local parameters: */
09629 
09630 /* CNORM = Norm of CU:  used to compute C */
09631 /* CU =    Scalar multiple of C:  E1 X E2 */
09632 /* E1,E2 = Edges of the underlying planar triangle: */
09633 /*           V2-V1 and V3-V1, respectively */
09634 /* I =     DO-loop index */
09635 
09636     /* Parameter adjustments */
09637     --c__;
09638     --v3;
09639     --v2;
09640     --v1;
09641 
09642     /* Function Body */
09643     for (i__ = 1; i__ <= 3; ++i__) {
09644         e1[i__ - 1] = v2[i__] - v1[i__];
09645         e2[i__ - 1] = v3[i__] - v1[i__];
09646 /* L1: */
09647     }
09648 
09649 /* Compute CU = E1 X E2 and CNORM**2. */
09650 
09651     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09652     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09653     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09654     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09655 
09656 /* The vertices lie on a common line if and only if CU is */
09657 /*   the zero vector. */
09658 
09659     if (cnorm != 0.) {
09660 
09661 /*   No error:  compute C. */
09662 
09663         cnorm = sqrt(cnorm);
09664         for (i__ = 1; i__ <= 3; ++i__) {
09665             c__[i__] = cu[i__ - 1] / cnorm;
09666 /* L2: */
09667         }
09668 
09669 /* If the vertices are nearly identical, the problem is */
09670 /*   ill-conditioned and it is possible for the computed */
09671 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09672 /*   when it should be positive. */
09673 
09674         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09675             c__[1] = -c__[1];
09676             c__[2] = -c__[2];
09677             c__[3] = -c__[3];
09678         }
09679         *ier = 0;
09680     } else {
09681 
09682 /*   CU = 0. */
09683 
09684         *ier = 1;
09685     }
09686     return 0;
09687 } /* circum_ */
09688 
09689 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09690         *lptr, int *lend, int *lnew)
09691 {
09692     static int k, lp, nst, lsav, next;
09693     /* Subroutine */ int insert_(int *, int *, int *,
09694             int *, int *);
09695 
09696 
09697 /* *********************************************************** */
09698 
09699 /*                                              From STRIPACK */
09700 /*                                            Robert J. Renka */
09701 /*                                  Dept. of Computer Science */
09702 /*                                       Univ. of North Texas */
09703 /*                                           renka@cs.unt.edu */
09704 /*                                                   07/17/96 */
09705 
09706 /*   This subroutine connects an exterior node KK to all */
09707 /* boundary nodes of a triangulation of KK-1 points on the */
09708 /* unit sphere, producing a triangulation that covers the */
09709 /* sphere.  The data structure is updated with the addition */
09710 /* of node KK, but no optimization is performed.  All boun- */
09711 /* dary nodes must be visible from node KK. */
09712 
09713 
09714 /* On input: */
09715 
09716 /*       KK = Index of the node to be connected to the set of */
09717 /*            all boundary nodes.  KK .GE. 4. */
09718 
09719 /*       N0 = Index of a boundary node (in the range 1 to */
09720 /*            KK-1).  N0 may be determined by Subroutine */
09721 /*            TRFIND. */
09722 
09723 /* The above parameters are not altered by this routine. */
09724 
09725 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09726 /*                             created by Subroutine TRMESH. */
09727 /*                             Node N0 must be included in */
09728 /*                             the triangulation. */
09729 
09730 /* On output: */
09731 
09732 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09733 /*                             the addition of node KK as the */
09734 /*                             last entry.  The updated */
09735 /*                             triangulation contains no */
09736 /*                             boundary nodes. */
09737 
09738 /* Module required by COVSPH:  INSERT */
09739 
09740 /* *********************************************************** */
09741 
09742 
09743 /* Local parameters: */
09744 
09745 /* K =     Local copy of KK */
09746 /* LP =    LIST pointer */
09747 /* LSAV =  LIST pointer */
09748 /* NEXT =  Boundary node visible from K */
09749 /* NST =   Local copy of N0 */
09750 
09751     /* Parameter adjustments */
09752     --lend;
09753     --lptr;
09754     --list;
09755 
09756     /* Function Body */
09757     k = *kk;
09758     nst = *n0;
09759 
09760 /* Traverse the boundary in clockwise order, inserting K as */
09761 /*   the first neighbor of each boundary node, and converting */
09762 /*   the boundary node to an interior node. */
09763 
09764     next = nst;
09765 L1:
09766     lp = lend[next];
09767     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09768     next = -list[lp];
09769     list[lp] = next;
09770     if (next != nst) {
09771         goto L1;
09772     }
09773 
09774 /* Traverse the boundary again, adding each node to K's */
09775 /*   adjacency list. */
09776 
09777     lsav = *lnew;
09778 L2:
09779     lp = lend[next];
09780     list[*lnew] = next;
09781     lptr[*lnew] = *lnew + 1;
09782     ++(*lnew);
09783     next = list[lp];
09784     if (next != nst) {
09785         goto L2;
09786     }
09787 
09788     lptr[*lnew - 1] = lsav;
09789     lend[k] = *lnew - 1;
09790     return 0;
09791 } /* covsph_ */
09792 
09793 
09794 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09795         double *y, double *z__, int *list, int *lend, int
09796         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09797         double *xc, double *yc, double *zc, double *rc,
09798         int *ier)
09799 {
09800     /* System generated locals */
09801     int i__1, i__2;
09802 
09803     /* Builtin functions */
09804     //double acos(double);
09805 
09806     /* Local variables */
09807     static double c__[3], t;
09808     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09809     static double v1[3], v2[3], v3[3];
09810     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09811              lpn;
09812     static long int swp;
09813     static int ierr;
09814     int lstptr_(int *, int *, int *, int *);
09815     long int swptst_(int *, int *, int *, int *,
09816             double *, double *, double *);
09817 
09818 
09819 /* *********************************************************** */
09820 
09821 /*                                              From STRIPACK */
09822 /*                                            Robert J. Renka */
09823 /*                                  Dept. of Computer Science */
09824 /*                                       Univ. of North Texas */
09825 /*                                           renka@cs.unt.edu */
09826 /*                                                   03/05/03 */
09827 
09828 /*   Given a Delaunay triangulation of nodes on the surface */
09829 /* of the unit sphere, this subroutine returns the set of */
09830 /* triangle circumcenters corresponding to Voronoi vertices, */
09831 /* along with the circumradii and a list of triangle indexes */
09832 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09833 /* entries. */
09834 
09835 /*   A triangle circumcenter is the point (unit vector) lying */
09836 /* at the same angular distance from the three vertices and */
09837 /* contained in the same hemisphere as the vertices.  (Note */
09838 /* that the negative of a circumcenter is also equidistant */
09839 /* from the vertices.)  If the triangulation covers the sur- */
09840 /* face, the Voronoi vertices are the circumcenters of the */
09841 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09842 /* LNEW are not altered in this case. */
09843 
09844 /*   On the other hand, if the nodes are contained in a sin- */
09845 /* gle hemisphere, the triangulation is implicitly extended */
09846 /* to the entire surface by adding pseudo-arcs (of length */
09847 /* greater than 180 degrees) between boundary nodes forming */
09848 /* pseudo-triangles whose 'circumcenters' are included in the */
09849 /* list.  This extension to the triangulation actually con- */
09850 /* sists of a triangulation of the set of boundary nodes in */
09851 /* which the swap test is reversed (a non-empty circumcircle */
09852 /* test).  The negative circumcenters are stored as the */
09853 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
09854 /* LNEW contain a data structure corresponding to the ex- */
09855 /* tended triangulation (Voronoi diagram), but LIST is not */
09856 /* altered in this case.  Thus, if it is necessary to retain */
09857 /* the original (unextended) triangulation data structure, */
09858 /* copies of LPTR and LNEW must be saved before calling this */
09859 /* routine. */
09860 
09861 
09862 /* On input: */
09863 
09864 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09865 /*           Note that, if N = 3, there are only two Voronoi */
09866 /*           vertices separated by 180 degrees, and the */
09867 /*           Voronoi regions are not well defined. */
09868 
09869 /*       NCOL = Number of columns reserved for LTRI.  This */
09870 /*              must be at least NB-2, where NB is the number */
09871 /*              of boundary nodes. */
09872 
09873 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09874 /*               coordinates of the nodes (unit vectors). */
09875 
09876 /*       LIST = int array containing the set of adjacency */
09877 /*              lists.  Refer to Subroutine TRMESH. */
09878 
09879 /*       LEND = Set of pointers to ends of adjacency lists. */
09880 /*              Refer to Subroutine TRMESH. */
09881 
09882 /* The above parameters are not altered by this routine. */
09883 
09884 /*       LPTR = Array of pointers associated with LIST.  Re- */
09885 /*              fer to Subroutine TRMESH. */
09886 
09887 /*       LNEW = Pointer to the first empty location in LIST */
09888 /*              and LPTR (list length plus one). */
09889 
09890 /*       LTRI = int work space array dimensioned 6 by */
09891 /*              NCOL, or unused dummy parameter if NB = 0. */
09892 
09893 /*       LISTC = int array of length at least 3*NT, where */
09894 /*               NT = 2*N-4 is the number of triangles in the */
09895 /*               triangulation (after extending it to cover */
09896 /*               the entire surface if necessary). */
09897 
09898 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
09899 
09900 /* On output: */
09901 
09902 /*       LPTR = Array of pointers associated with LISTC: */
09903 /*              updated for the addition of pseudo-triangles */
09904 /*              if the original triangulation contains */
09905 /*              boundary nodes (NB > 0). */
09906 
09907 /*       LNEW = Pointer to the first empty location in LISTC */
09908 /*              and LPTR (list length plus one).  LNEW is not */
09909 /*              altered if NB = 0. */
09910 
09911 /*       LTRI = Triangle list whose first NB-2 columns con- */
09912 /*              tain the indexes of a clockwise-ordered */
09913 /*              sequence of vertices (first three rows) */
09914 /*              followed by the LTRI column indexes of the */
09915 /*              triangles opposite the vertices (or 0 */
09916 /*              denoting the exterior region) in the last */
09917 /*              three rows.  This array is not generally of */
09918 /*              any use. */
09919 
09920 /*       LISTC = Array containing triangle indexes (indexes */
09921 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
09922 /*               pondence with LIST/LPTR entries (or entries */
09923 /*               that would be stored in LIST for the */
09924 /*               extended triangulation):  the index of tri- */
09925 /*               angle (N1,N2,N3) is stored in LISTC(K), */
09926 /*               LISTC(L), and LISTC(M), where LIST(K), */
09927 /*               LIST(L), and LIST(M) are the indexes of N2 */
09928 /*               as a neighbor of N1, N3 as a neighbor of N2, */
09929 /*               and N1 as a neighbor of N3.  The Voronoi */
09930 /*               region associated with a node is defined by */
09931 /*               the CCW-ordered sequence of circumcenters in */
09932 /*               one-to-one correspondence with its adjacency */
09933 /*               list (in the extended triangulation). */
09934 
09935 /*       NB = Number of boundary nodes unless IER = 1. */
09936 
09937 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
09938 /*                  nates of the triangle circumcenters */
09939 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
09940 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
09941 /*                  correspond to pseudo-triangles if NB > 0. */
09942 
09943 /*       RC = Array containing circumradii (the arc lengths */
09944 /*            or angles between the circumcenters and associ- */
09945 /*            ated triangle vertices) in 1-1 correspondence */
09946 /*            with circumcenters. */
09947 
09948 /*       IER = Error indicator: */
09949 /*             IER = 0 if no errors were encountered. */
09950 /*             IER = 1 if N < 3. */
09951 /*             IER = 2 if NCOL < NB-2. */
09952 /*             IER = 3 if a triangle is degenerate (has ver- */
09953 /*                     tices lying on a common geodesic). */
09954 
09955 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
09956 
09957 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
09958 
09959 /* *********************************************************** */
09960 
09961 
09962 /* Local parameters: */
09963 
09964 /* C =         Circumcenter returned by Subroutine CIRCUM */
09965 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
09966 /* I4 =        LTRI row index in the range 1 to 3 */
09967 /* IERR =      Error flag for calls to CIRCUM */
09968 /* KT =        Triangle index */
09969 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
09970 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
09971 /*               and N2 as vertices of KT1 */
09972 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
09973 /*               and N2 as vertices of KT2 */
09974 /* LP,LPN =    LIST pointers */
09975 /* LPL =       LIST pointer of the last neighbor of N1 */
09976 /* N0 =        Index of the first boundary node (initial */
09977 /*               value of N1) in the loop on boundary nodes */
09978 /*               used to store the pseudo-triangle indexes */
09979 /*               in LISTC */
09980 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
09981 /*               or pseudo-triangle (clockwise order) */
09982 /* N4 =        Index of the node opposite N2 -> N1 */
09983 /* NM2 =       N-2 */
09984 /* NN =        Local copy of N */
09985 /* NT =        Number of pseudo-triangles:  NB-2 */
09986 /* SWP =       long int variable set to TRUE in each optimiza- */
09987 /*               tion loop (loop on pseudo-arcs) iff a swap */
09988 /*               is performed */
09989 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
09990 /*               Subroutine CIRCUM */
09991 
09992     /* Parameter adjustments */
09993     --lend;
09994     --z__;
09995     --y;
09996     --x;
09997     ltri -= 7;
09998     --list;
09999     --lptr;
10000     --listc;
10001     --xc;
10002     --yc;
10003     --zc;
10004     --rc;
10005 
10006     /* Function Body */
10007     nn = *n;
10008     *nb = 0;
10009     nt = 0;
10010     if (nn < 3) {
10011         goto L21;
10012     }
10013 
10014 /* Search for a boundary node N1. */
10015 
10016     i__1 = nn;
10017     for (n1 = 1; n1 <= i__1; ++n1) {
10018         lp = lend[n1];
10019         if (list[lp] < 0) {
10020             goto L2;
10021         }
10022 /* L1: */
10023     }
10024 
10025 /* The triangulation already covers the sphere. */
10026 
10027     goto L9;
10028 
10029 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
10030 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
10031 /*   boundary nodes to which it is not already adjacent. */
10032 
10033 /*   Set N3 and N2 to the first and last neighbors, */
10034 /*     respectively, of N1. */
10035 
10036 L2:
10037     n2 = -list[lp];
10038     lp = lptr[lp];
10039     n3 = list[lp];
10040 
10041 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
10042 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
10043 /*     along with the indexes of the triangles opposite */
10044 /*     the vertices. */
10045 
10046 L3:
10047     ++nt;
10048     if (nt <= *ncol) {
10049         ltri[nt * 6 + 1] = n1;
10050         ltri[nt * 6 + 2] = n2;
10051         ltri[nt * 6 + 3] = n3;
10052         ltri[nt * 6 + 4] = nt + 1;
10053         ltri[nt * 6 + 5] = nt - 1;
10054         ltri[nt * 6 + 6] = 0;
10055     }
10056     n1 = n2;
10057     lp = lend[n1];
10058     n2 = -list[lp];
10059     if (n2 != n3) {
10060         goto L3;
10061     }
10062 
10063     *nb = nt + 2;
10064     if (*ncol < nt) {
10065         goto L22;
10066     }
10067     ltri[nt * 6 + 4] = 0;
10068     if (nt == 1) {
10069         goto L7;
10070     }
10071 
10072 /* Optimize the exterior triangulation (set of pseudo- */
10073 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
10074 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
10075 /*   The loop on pseudo-arcs is repeated until no swaps are */
10076 /*   performed. */
10077 
10078 L4:
10079     swp = FALSE_;
10080     i__1 = nt - 1;
10081     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10082         for (i3 = 1; i3 <= 3; ++i3) {
10083             kt2 = ltri[i3 + 3 + kt1 * 6];
10084             if (kt2 <= kt1) {
10085                 goto L5;
10086             }
10087 
10088 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10089 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10090 
10091             if (i3 == 1) {
10092                 i1 = 2;
10093                 i2 = 3;
10094             } else if (i3 == 2) {
10095                 i1 = 3;
10096                 i2 = 1;
10097             } else {
10098                 i1 = 1;
10099                 i2 = 2;
10100             }
10101             n1 = ltri[i1 + kt1 * 6];
10102             n2 = ltri[i2 + kt1 * 6];
10103             n3 = ltri[i3 + kt1 * 6];
10104 
10105 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10106 /*     LTRI(I+3,KT2) = KT1. */
10107 
10108             if (ltri[kt2 * 6 + 4] == kt1) {
10109                 i4 = 1;
10110             } else if (ltri[kt2 * 6 + 5] == kt1) {
10111                 i4 = 2;
10112             } else {
10113                 i4 = 3;
10114             }
10115             n4 = ltri[i4 + kt2 * 6];
10116 
10117 /*   The empty circumcircle test is reversed for the pseudo- */
10118 /*     triangles.  The reversal is implicit in the clockwise */
10119 /*     ordering of the vertices. */
10120 
10121             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10122                 goto L5;
10123             }
10124 
10125 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10126 /*     Nj as a vertex of KTi. */
10127 
10128             swp = TRUE_;
10129             kt11 = ltri[i1 + 3 + kt1 * 6];
10130             kt12 = ltri[i2 + 3 + kt1 * 6];
10131             if (i4 == 1) {
10132                 i2 = 2;
10133                 i1 = 3;
10134             } else if (i4 == 2) {
10135                 i2 = 3;
10136                 i1 = 1;
10137             } else {
10138                 i2 = 1;
10139                 i1 = 2;
10140             }
10141             kt21 = ltri[i1 + 3 + kt2 * 6];
10142             kt22 = ltri[i2 + 3 + kt2 * 6];
10143             ltri[kt1 * 6 + 1] = n4;
10144             ltri[kt1 * 6 + 2] = n3;
10145             ltri[kt1 * 6 + 3] = n1;
10146             ltri[kt1 * 6 + 4] = kt12;
10147             ltri[kt1 * 6 + 5] = kt22;
10148             ltri[kt1 * 6 + 6] = kt2;
10149             ltri[kt2 * 6 + 1] = n3;
10150             ltri[kt2 * 6 + 2] = n4;
10151             ltri[kt2 * 6 + 3] = n2;
10152             ltri[kt2 * 6 + 4] = kt21;
10153             ltri[kt2 * 6 + 5] = kt11;
10154             ltri[kt2 * 6 + 6] = kt1;
10155 
10156 /*   Correct the KT11 and KT22 entries that changed. */
10157 
10158             if (kt11 != 0) {
10159                 i4 = 4;
10160                 if (ltri[kt11 * 6 + 4] != kt1) {
10161                     i4 = 5;
10162                     if (ltri[kt11 * 6 + 5] != kt1) {
10163                         i4 = 6;
10164                     }
10165                 }
10166                 ltri[i4 + kt11 * 6] = kt2;
10167             }
10168             if (kt22 != 0) {
10169                 i4 = 4;
10170                 if (ltri[kt22 * 6 + 4] != kt2) {
10171                     i4 = 5;
10172                     if (ltri[kt22 * 6 + 5] != kt2) {
10173                         i4 = 6;
10174                     }
10175                 }
10176                 ltri[i4 + kt22 * 6] = kt1;
10177             }
10178 L5:
10179             ;
10180         }
10181 /* L6: */
10182     }
10183     if (swp) {
10184         goto L4;
10185     }
10186 
10187 /* Compute and store the negative circumcenters and radii of */
10188 /*   the pseudo-triangles in the first NT positions. */
10189 
10190 L7:
10191     i__1 = nt;
10192     for (kt = 1; kt <= i__1; ++kt) {
10193         n1 = ltri[kt * 6 + 1];
10194         n2 = ltri[kt * 6 + 2];
10195         n3 = ltri[kt * 6 + 3];
10196         v1[0] = x[n1];
10197         v1[1] = y[n1];
10198         v1[2] = z__[n1];
10199         v2[0] = x[n2];
10200         v2[1] = y[n2];
10201         v2[2] = z__[n2];
10202         v3[0] = x[n3];
10203         v3[1] = y[n3];
10204         v3[2] = z__[n3];
10205         circum_(v2, v1, v3, c__, &ierr);
10206         if (ierr != 0) {
10207             goto L23;
10208         }
10209 
10210 /*   Store the negative circumcenter and radius (computed */
10211 /*     from <V1,C>). */
10212 
10213         xc[kt] = -c__[0];
10214         yc[kt] = -c__[1];
10215         zc[kt] = -c__[2];
10216         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10217         if (t < -1.) {
10218             t = -1.;
10219         }
10220         if (t > 1.) {
10221             t = 1.;
10222         }
10223         rc[kt] = acos(t);
10224 /* L8: */
10225     }
10226 
10227 /* Compute and store the circumcenters and radii of the */
10228 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10229 /*   Also, store the triangle indexes KT in the appropriate */
10230 /*   LISTC positions. */
10231 
10232 L9:
10233     kt = nt;
10234 
10235 /*   Loop on nodes N1. */
10236 
10237     nm2 = nn - 2;
10238     i__1 = nm2;
10239     for (n1 = 1; n1 <= i__1; ++n1) {
10240         lpl = lend[n1];
10241         lp = lpl;
10242         n3 = list[lp];
10243 
10244 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10245 /*     and N3 > N1. */
10246 
10247 L10:
10248         lp = lptr[lp];
10249         n2 = n3;
10250         n3 = (i__2 = list[lp], abs(i__2));
10251         if (n2 <= n1 || n3 <= n1) {
10252             goto L11;
10253         }
10254         ++kt;
10255 
10256 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10257 
10258         v1[0] = x[n1];
10259         v1[1] = y[n1];
10260         v1[2] = z__[n1];
10261         v2[0] = x[n2];
10262         v2[1] = y[n2];
10263         v2[2] = z__[n2];
10264         v3[0] = x[n3];
10265         v3[1] = y[n3];
10266         v3[2] = z__[n3];
10267         circum_(v1, v2, v3, c__, &ierr);
10268         if (ierr != 0) {
10269             goto L23;
10270         }
10271 
10272 /*   Store the circumcenter, radius and triangle index. */
10273 
10274         xc[kt] = c__[0];
10275         yc[kt] = c__[1];
10276         zc[kt] = c__[2];
10277         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10278         if (t < -1.) {
10279             t = -1.;
10280         }
10281         if (t > 1.) {
10282             t = 1.;
10283         }
10284         rc[kt] = acos(t);
10285 
10286 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10287 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10288 /*     of N2, and N1 as a neighbor of N3. */
10289 
10290         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10291         listc[lpn] = kt;
10292         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10293         listc[lpn] = kt;
10294         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10295         listc[lpn] = kt;
10296 L11:
10297         if (lp != lpl) {
10298             goto L10;
10299         }
10300 /* L12: */
10301     }
10302     if (nt == 0) {
10303         goto L20;
10304     }
10305 
10306 /* Store the first NT triangle indexes in LISTC. */
10307 
10308 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10309 /*     boundary arc opposite N3. */
10310 
10311     kt1 = 0;
10312 L13:
10313     ++kt1;
10314     if (ltri[kt1 * 6 + 4] == 0) {
10315         i1 = 2;
10316         i2 = 3;
10317         i3 = 1;
10318         goto L14;
10319     } else if (ltri[kt1 * 6 + 5] == 0) {
10320         i1 = 3;
10321         i2 = 1;
10322         i3 = 2;
10323         goto L14;
10324     } else if (ltri[kt1 * 6 + 6] == 0) {
10325         i1 = 1;
10326         i2 = 2;
10327         i3 = 3;
10328         goto L14;
10329     }
10330     goto L13;
10331 L14:
10332     n1 = ltri[i1 + kt1 * 6];
10333     n0 = n1;
10334 
10335 /*   Loop on boundary nodes N1 in CCW order, storing the */
10336 /*     indexes of the clockwise-ordered sequence of triangles */
10337 /*     that contain N1.  The first triangle overwrites the */
10338 /*     last neighbor position, and the remaining triangles, */
10339 /*     if any, are appended to N1's adjacency list. */
10340 
10341 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10342 
10343 L15:
10344     lp = lend[n1];
10345     lpn = lptr[lp];
10346     listc[lp] = kt1;
10347 
10348 /*   Loop on triangles KT2 containing N1. */
10349 
10350 L16:
10351     kt2 = ltri[i2 + 3 + kt1 * 6];
10352     if (kt2 != 0) {
10353 
10354 /*   Append KT2 to N1's triangle list. */
10355 
10356         lptr[lp] = *lnew;
10357         lp = *lnew;
10358         listc[lp] = kt2;
10359         ++(*lnew);
10360 
10361 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10362 /*     LTRI(I1,KT1) = N1. */
10363 
10364         kt1 = kt2;
10365         if (ltri[kt1 * 6 + 1] == n1) {
10366             i1 = 1;
10367             i2 = 2;
10368             i3 = 3;
10369         } else if (ltri[kt1 * 6 + 2] == n1) {
10370             i1 = 2;
10371             i2 = 3;
10372             i3 = 1;
10373         } else {
10374             i1 = 3;
10375             i2 = 1;
10376             i3 = 2;
10377         }
10378         goto L16;
10379     }
10380 
10381 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10382 /*     N1 to the next boundary node, test for termination, */
10383 /*     and permute the indexes:  the last triangle containing */
10384 /*     a boundary node is the first triangle containing the */
10385 /*     next boundary node. */
10386 
10387     lptr[lp] = lpn;
10388     n1 = ltri[i3 + kt1 * 6];
10389     if (n1 != n0) {
10390         i4 = i3;
10391         i3 = i2;
10392         i2 = i1;
10393         i1 = i4;
10394         goto L15;
10395     }
10396 
10397 /* No errors encountered. */
10398 
10399 L20:
10400     *ier = 0;
10401     return 0;
10402 
10403 /* N < 3. */
10404 
10405 L21:
10406     *ier = 1;
10407     return 0;
10408 
10409 /* Insufficient space reserved for LTRI. */
10410 
10411 L22:
10412     *ier = 2;
10413     return 0;
10414 
10415 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10416 
10417 L23:
10418     *ier = 3;
10419     return 0;
10420 } /* crlist_ */
10421 
10422 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10423         list, int *lptr, int *lend, int *lnew, int *ier)
10424 {
10425     /* System generated locals */
10426     int i__1;
10427 
10428     /* Local variables */
10429     static int n1, n2, n3, lp, lph, lpl;
10430     /* Subroutine */ int delnb_(int *, int *, int *,
10431             int *, int *, int *, int *, int *);
10432     int lstptr_(int *, int *, int *, int *);
10433 
10434 
10435 /* *********************************************************** */
10436 
10437 /*                                              From STRIPACK */
10438 /*                                            Robert J. Renka */
10439 /*                                  Dept. of Computer Science */
10440 /*                                       Univ. of North Texas */
10441 /*                                           renka@cs.unt.edu */
10442 /*                                                   07/17/96 */
10443 
10444 /*   This subroutine deletes a boundary arc from a triangula- */
10445 /* tion.  It may be used to remove a null triangle from the */
10446 /* convex hull boundary.  Note, however, that if the union of */
10447 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10448 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10449 /* NEARND should not be called following an arc deletion. */
10450 
10451 /*   This routine is identical to the similarly named routine */
10452 /* in TRIPACK. */
10453 
10454 
10455 /* On input: */
10456 
10457 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10458 
10459 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10460 /*                 adjacent boundary nodes defining the arc */
10461 /*                 to be removed. */
10462 
10463 /* The above parameters are not altered by this routine. */
10464 
10465 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10466 /*                             created by Subroutine TRMESH. */
10467 
10468 /* On output: */
10469 
10470 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10471 /*                             the removal of arc IO1-IO2 */
10472 /*                             unless IER > 0. */
10473 
10474 /*       IER = Error indicator: */
10475 /*             IER = 0 if no errors were encountered. */
10476 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10477 /*                     range, or IO1 = IO2. */
10478 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10479 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10480 /*                     ready a boundary node, and thus IO1 */
10481 /*                     or IO2 has only two neighbors or a */
10482 /*                     deletion would result in two triangu- */
10483 /*                     lations sharing a single node. */
10484 /*             IER = 4 if one of the nodes is a neighbor of */
10485 /*                     the other, but not vice versa, imply- */
10486 /*                     ing an invalid triangulation data */
10487 /*                     structure. */
10488 
10489 /* Module required by DELARC:  DELNB, LSTPTR */
10490 
10491 /* Intrinsic function called by DELARC:  ABS */
10492 
10493 /* *********************************************************** */
10494 
10495 
10496 /* Local parameters: */
10497 
10498 /* LP =       LIST pointer */
10499 /* LPH =      LIST pointer or flag returned by DELNB */
10500 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10501 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10502 /*              is the directed boundary edge associated */
10503 /*              with IO1-IO2 */
10504 
10505     /* Parameter adjustments */
10506     --lend;
10507     --list;
10508     --lptr;
10509 
10510     /* Function Body */
10511     n1 = *io1;
10512     n2 = *io2;
10513 
10514 /* Test for errors, and set N1->N2 to the directed boundary */
10515 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10516 /*   for some N3. */
10517 
10518     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10519         *ier = 1;
10520         return 0;
10521     }
10522 
10523     lpl = lend[n2];
10524     if (-list[lpl] != n1) {
10525         n1 = n2;
10526         n2 = *io1;
10527         lpl = lend[n2];
10528         if (-list[lpl] != n1) {
10529             *ier = 2;
10530             return 0;
10531         }
10532     }
10533 
10534 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10535 /*   of N1), and test for error 3 (N3 already a boundary */
10536 /*   node). */
10537 
10538     lpl = lend[n1];
10539     lp = lptr[lpl];
10540     lp = lptr[lp];
10541     n3 = (i__1 = list[lp], abs(i__1));
10542     lpl = lend[n3];
10543     if (list[lpl] <= 0) {
10544         *ier = 3;
10545         return 0;
10546     }
10547 
10548 /* Delete N2 as a neighbor of N1, making N3 the first */
10549 /*   neighbor, and test for error 4 (N2 not a neighbor */
10550 /*   of N1).  Note that previously computed pointers may */
10551 /*   no longer be valid following the call to DELNB. */
10552 
10553     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10554     if (lph < 0) {
10555         *ier = 4;
10556         return 0;
10557     }
10558 
10559 /* Delete N1 as a neighbor of N2, making N3 the new last */
10560 /*   neighbor. */
10561 
10562     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10563 
10564 /* Make N3 a boundary node with first neighbor N2 and last */
10565 /*   neighbor N1. */
10566 
10567     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10568     lend[n3] = lp;
10569     list[lp] = -n1;
10570 
10571 /* No errors encountered. */
10572 
10573     *ier = 0;
10574     return 0;
10575 } /* delarc_ */
10576 
10577 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10578         list, int *lptr, int *lend, int *lnew, int *lph)
10579 {
10580     /* System generated locals */
10581     int i__1;
10582 
10583     /* Local variables */
10584     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10585 
10586 
10587 /* *********************************************************** */
10588 
10589 /*                                              From STRIPACK */
10590 /*                                            Robert J. Renka */
10591 /*                                  Dept. of Computer Science */
10592 /*                                       Univ. of North Texas */
10593 /*                                           renka@cs.unt.edu */
10594 /*                                                   07/29/98 */
10595 
10596 /*   This subroutine deletes a neighbor NB from the adjacency */
10597 /* list of node N0 (but N0 is not deleted from the adjacency */
10598 /* list of NB) and, if NB is a boundary node, makes N0 a */
10599 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10600 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10601 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10602 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10603 /* ted.  This requires a search of LEND and LPTR entailing an */
10604 /* expected operation count of O(N). */
10605 
10606 /*   This routine is identical to the similarly named routine */
10607 /* in TRIPACK. */
10608 
10609 
10610 /* On input: */
10611 
10612 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10613 /*               nodes such that NB is a neighbor of N0. */
10614 /*               (N0 need not be a neighbor of NB.) */
10615 
10616 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10617 
10618 /* The above parameters are not altered by this routine. */
10619 
10620 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10621 /*                             triangulation. */
10622 
10623 /* On output: */
10624 
10625 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10626 /*                             the removal of NB from the ad- */
10627 /*                             jacency list of N0 unless */
10628 /*                             LPH < 0. */
10629 
10630 /*       LPH = List pointer to the hole (NB as a neighbor of */
10631 /*             N0) filled in by the values at LNEW-1 or error */
10632 /*             indicator: */
10633 /*             LPH > 0 if no errors were encountered. */
10634 /*             LPH = -1 if N0, NB, or N is outside its valid */
10635 /*                      range. */
10636 /*             LPH = -2 if NB is not a neighbor of N0. */
10637 
10638 /* Modules required by DELNB:  None */
10639 
10640 /* Intrinsic function called by DELNB:  ABS */
10641 
10642 /* *********************************************************** */
10643 
10644 
10645 /* Local parameters: */
10646 
10647 /* I =   DO-loop index */
10648 /* LNW = LNEW-1 (output value of LNEW) */
10649 /* LP =  LIST pointer of the last neighbor of NB */
10650 /* LPB = Pointer to NB as a neighbor of N0 */
10651 /* LPL = Pointer to the last neighbor of N0 */
10652 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10653 /* NN =  Local copy of N */
10654 
10655     /* Parameter adjustments */
10656     --lend;
10657     --list;
10658     --lptr;
10659 
10660     /* Function Body */
10661     nn = *n;
10662 
10663 /* Test for error 1. */
10664 
10665     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10666         *lph = -1;
10667         return 0;
10668     }
10669 
10670 /*   Find pointers to neighbors of N0: */
10671 
10672 /*     LPL points to the last neighbor, */
10673 /*     LPP points to the neighbor NP preceding NB, and */
10674 /*     LPB points to NB. */
10675 
10676     lpl = lend[*n0];
10677     lpp = lpl;
10678     lpb = lptr[lpp];
10679 L1:
10680     if (list[lpb] == *nb) {
10681         goto L2;
10682     }
10683     lpp = lpb;
10684     lpb = lptr[lpp];
10685     if (lpb != lpl) {
10686         goto L1;
10687     }
10688 
10689 /*   Test for error 2 (NB not found). */
10690 
10691     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10692         *lph = -2;
10693         return 0;
10694     }
10695 
10696 /*   NB is the last neighbor of N0.  Make NP the new last */
10697 /*     neighbor and, if NB is a boundary node, then make N0 */
10698 /*     a boundary node. */
10699 
10700     lend[*n0] = lpp;
10701     lp = lend[*nb];
10702     if (list[lp] < 0) {
10703         list[lpp] = -list[lpp];
10704     }
10705     goto L3;
10706 
10707 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10708 /*     node and N0 is not, then make N0 a boundary node with */
10709 /*     last neighbor NP. */
10710 
10711 L2:
10712     lp = lend[*nb];
10713     if (list[lp] < 0 && list[lpl] > 0) {
10714         lend[*n0] = lpp;
10715         list[lpp] = -list[lpp];
10716     }
10717 
10718 /*   Update LPTR so that the neighbor following NB now fol- */
10719 /*     lows NP, and fill in the hole at location LPB. */
10720 
10721 L3:
10722     lptr[lpp] = lptr[lpb];
10723     lnw = *lnew - 1;
10724     list[lpb] = list[lnw];
10725     lptr[lpb] = lptr[lnw];
10726     for (i__ = nn; i__ >= 1; --i__) {
10727         if (lend[i__] == lnw) {
10728             lend[i__] = lpb;
10729             goto L5;
10730         }
10731 /* L4: */
10732     }
10733 
10734 L5:
10735     i__1 = lnw - 1;
10736     for (i__ = 1; i__ <= i__1; ++i__) {
10737         if (lptr[i__] == lnw) {
10738             lptr[i__] = lpb;
10739         }
10740 /* L6: */
10741     }
10742 
10743 /* No errors encountered. */
10744 
10745     *lnew = lnw;
10746     *lph = lpb;
10747     return 0;
10748 } /* delnb_ */
10749 
10750 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10751         double *y, double *z__, int *list, int *lptr, int
10752         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10753 {
10754     /* System generated locals */
10755     int i__1;
10756 
10757     /* Local variables */
10758     static int i__, j, n1, n2;
10759     static double x1, x2, y1, y2, z1, z2;
10760     static int nl, lp, nn, nr;
10761     static double xl, yl, zl, xr, yr, zr;
10762     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10763     static long int bdry;
10764     static int ierr, lwkl;
10765     /* Subroutine */ int swap_(int *, int *, int *,
10766             int *, int *, int *, int *, int *), delnb_(
10767             int *, int *, int *, int *, int *, int *,
10768             int *, int *);
10769     int nbcnt_(int *, int *);
10770     /* Subroutine */ int optim_(double *, double *, double
10771             *, int *, int *, int *, int *, int *, int
10772             *, int *);
10773     static int nfrst;
10774     int lstptr_(int *, int *, int *, int *);
10775 
10776 
10777 /* *********************************************************** */
10778 
10779 /*                                              From STRIPACK */
10780 /*                                            Robert J. Renka */
10781 /*                                  Dept. of Computer Science */
10782 /*                                       Univ. of North Texas */
10783 /*                                           renka@cs.unt.edu */
10784 /*                                                   11/30/99 */
10785 
10786 /*   This subroutine deletes node K (along with all arcs */
10787 /* incident on node K) from a triangulation of N nodes on the */
10788 /* unit sphere, and inserts arcs as necessary to produce a */
10789 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10790 /* triangulation is input, a Delaunay triangulation will */
10791 /* result, and thus, DELNOD reverses the effect of a call to */
10792 /* Subroutine ADDNOD. */
10793 
10794 
10795 /* On input: */
10796 
10797 /*       K = Index (for X, Y, and Z) of the node to be */
10798 /*           deleted.  1 .LE. K .LE. N. */
10799 
10800 /* K is not altered by this routine. */
10801 
10802 /*       N = Number of nodes in the triangulation on input. */
10803 /*           N .GE. 4.  Note that N will be decremented */
10804 /*           following the deletion. */
10805 
10806 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10807 /*               coordinates of the nodes in the triangula- */
10808 /*               tion. */
10809 
10810 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10811 /*                             triangulation.  Refer to Sub- */
10812 /*                             routine TRMESH. */
10813 
10814 /*       LWK = Number of columns reserved for IWK.  LWK must */
10815 /*             be at least NNB-3, where NNB is the number of */
10816 /*             neighbors of node K, including an extra */
10817 /*             pseudo-node if K is a boundary node. */
10818 
10819 /*       IWK = int work array dimensioned 2 by LWK (or */
10820 /*             array of length .GE. 2*LWK). */
10821 
10822 /* On output: */
10823 
10824 /*       N = Number of nodes in the triangulation on output. */
10825 /*           The input value is decremented unless 1 .LE. IER */
10826 /*           .LE. 4. */
10827 
10828 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10829 /*               (with elements K+1,...,N+1 shifted up one */
10830 /*               position, thus overwriting element K) unless */
10831 /*               1 .LE. IER .LE. 4. */
10832 
10833 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10834 /*                             structure reflecting the dele- */
10835 /*                             tion unless 1 .LE. IER .LE. 4. */
10836 /*                             Note that the data structure */
10837 /*                             may have been altered if IER > */
10838 /*                             3. */
10839 
10840 /*       LWK = Number of IWK columns required unless IER = 1 */
10841 /*             or IER = 3. */
10842 
10843 /*       IWK = Indexes of the endpoints of the new arcs added */
10844 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10845 /*             are associated with columns, or pairs of */
10846 /*             adjacent elements if IWK is declared as a */
10847 /*             singly-subscripted array.) */
10848 
10849 /*       IER = Error indicator: */
10850 /*             IER = 0 if no errors were encountered. */
10851 /*             IER = 1 if K or N is outside its valid range */
10852 /*                     or LWK < 0 on input. */
10853 /*             IER = 2 if more space is required in IWK. */
10854 /*                     Refer to LWK. */
10855 /*             IER = 3 if the triangulation data structure is */
10856 /*                     invalid on input. */
10857 /*             IER = 4 if K indexes an interior node with */
10858 /*                     four or more neighbors, none of which */
10859 /*                     can be swapped out due to collineari- */
10860 /*                     ty, and K cannot therefore be deleted. */
10861 /*             IER = 5 if an error flag (other than IER = 1) */
10862 /*                     was returned by OPTIM.  An error */
10863 /*                     message is written to the standard */
10864 /*                     output unit in this case. */
10865 /*             IER = 6 if error flag 1 was returned by OPTIM. */
10866 /*                     This is not necessarily an error, but */
10867 /*                     the arcs may not be optimal. */
10868 
10869 /*   Note that the deletion may result in all remaining nodes */
10870 /* being collinear.  This situation is not flagged. */
10871 
10872 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
10873 /*                                OPTIM, SWAP, SWPTST */
10874 
10875 /* Intrinsic function called by DELNOD:  ABS */
10876 
10877 /* *********************************************************** */
10878 
10879 
10880 /* Local parameters: */
10881 
10882 /* BDRY =    long int variable with value TRUE iff N1 is a */
10883 /*             boundary node */
10884 /* I,J =     DO-loop indexes */
10885 /* IERR =    Error flag returned by OPTIM */
10886 /* IWL =     Number of IWK columns containing arcs */
10887 /* LNW =     Local copy of LNEW */
10888 /* LP =      LIST pointer */
10889 /* LP21 =    LIST pointer returned by SWAP */
10890 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
10891 /* LPH =     Pointer (or flag) returned by DELNB */
10892 /* LPL2 =    Pointer to the last neighbor of N2 */
10893 /* LPN =     Pointer to a neighbor of N1 */
10894 /* LWKL =    Input value of LWK */
10895 /* N1 =      Local copy of K */
10896 /* N2 =      Neighbor of N1 */
10897 /* NFRST =   First neighbor of N1:  LIST(LPF) */
10898 /* NIT =     Number of iterations in OPTIM */
10899 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
10900 /*             following (to the left of) N2, respectively */
10901 /* NN =      Number of nodes in the triangulation */
10902 /* NNB =     Number of neighbors of N1 (including a pseudo- */
10903 /*             node representing the boundary if N1 is a */
10904 /*             boundary node) */
10905 /* X1,Y1,Z1 = Coordinates of N1 */
10906 /* X2,Y2,Z2 = Coordinates of N2 */
10907 /* XL,YL,ZL = Coordinates of NL */
10908 /* XR,YR,ZR = Coordinates of NR */
10909 
10910 
10911 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
10912 /*   one if N1 is a boundary node), and test for errors.  LPF */
10913 /*   and LPL are LIST indexes of the first and last neighbors */
10914 /*   of N1, IWL is the number of IWK columns containing arcs, */
10915 /*   and BDRY is TRUE iff N1 is a boundary node. */
10916 
10917     /* Parameter adjustments */
10918     iwk -= 3;
10919     --lend;
10920     --lptr;
10921     --list;
10922     --z__;
10923     --y;
10924     --x;
10925 
10926     /* Function Body */
10927     n1 = *k;
10928     nn = *n;
10929     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10930         goto L21;
10931     }
10932     lpl = lend[n1];
10933     lpf = lptr[lpl];
10934     nnb = nbcnt_(&lpl, &lptr[1]);
10935     bdry = list[lpl] < 0;
10936     if (bdry) {
10937         ++nnb;
10938     }
10939     if (nnb < 3) {
10940         goto L23;
10941     }
10942     lwkl = *lwk;
10943     *lwk = nnb - 3;
10944     if (lwkl < *lwk) {
10945         goto L22;
10946     }
10947     iwl = 0;
10948     if (nnb == 3) {
10949         goto L3;
10950     }
10951 
10952 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
10953 /*   beginning with the second neighbor.  NR and NL are the */
10954 /*   neighbors preceding and following N2, respectively, and */
10955 /*   LP indexes NL.  The loop is exited when all possible */
10956 /*   swaps have been applied to arcs incident on N1. */
10957 
10958     x1 = x[n1];
10959     y1 = y[n1];
10960     z1 = z__[n1];
10961     nfrst = list[lpf];
10962     nr = nfrst;
10963     xr = x[nr];
10964     yr = y[nr];
10965     zr = z__[nr];
10966     lp = lptr[lpf];
10967     n2 = list[lp];
10968     x2 = x[n2];
10969     y2 = y[n2];
10970     z2 = z__[n2];
10971     lp = lptr[lp];
10972 
10973 /* Top of loop:  set NL to the neighbor following N2. */
10974 
10975 L1:
10976     nl = (i__1 = list[lp], abs(i__1));
10977     if (nl == nfrst && bdry) {
10978         goto L3;
10979     }
10980     xl = x[nl];
10981     yl = y[nl];
10982     zl = z__[nl];
10983 
10984 /*   Test for a convex quadrilateral.  To avoid an incorrect */
10985 /*     test caused by collinearity, use the fact that if N1 */
10986 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
10987 /*     a boundary node, then N2 LEFT NL->NR. */
10988 
10989     lpl2 = lend[n2];
10990     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10991             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10992             z2)))) {
10993 
10994 /*   Nonconvex quadrilateral -- no swap is possible. */
10995 
10996         nr = n2;
10997         xr = x2;
10998         yr = y2;
10999         zr = z2;
11000         goto L2;
11001     }
11002 
11003 /*   The quadrilateral defined by adjacent triangles */
11004 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
11005 /*     NL-NR and store it in IWK unless NL and NR are */
11006 /*     already adjacent, in which case the swap is not */
11007 /*     possible.  Indexes larger than N1 must be decremented */
11008 /*     since N1 will be deleted from X, Y, and Z. */
11009 
11010     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11011     if (lp21 == 0) {
11012         nr = n2;
11013         xr = x2;
11014         yr = y2;
11015         zr = z2;
11016         goto L2;
11017     }
11018     ++iwl;
11019     if (nl <= n1) {
11020         iwk[(iwl << 1) + 1] = nl;
11021     } else {
11022         iwk[(iwl << 1) + 1] = nl - 1;
11023     }
11024     if (nr <= n1) {
11025         iwk[(iwl << 1) + 2] = nr;
11026     } else {
11027         iwk[(iwl << 1) + 2] = nr - 1;
11028     }
11029 
11030 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
11031 
11032     lpl = lend[n1];
11033     --nnb;
11034     if (nnb == 3) {
11035         goto L3;
11036     }
11037     lpf = lptr[lpl];
11038     nfrst = list[lpf];
11039     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11040     if (nr == nfrst) {
11041         goto L2;
11042     }
11043 
11044 /*   NR is not the first neighbor of N1. */
11045 /*     Back up and test N1-NR for a swap again:  Set N2 to */
11046 /*     NR and NR to the previous neighbor of N1 -- the */
11047 /*     neighbor of NR which follows N1.  LP21 points to NL */
11048 /*     as a neighbor of NR. */
11049 
11050     n2 = nr;
11051     x2 = xr;
11052     y2 = yr;
11053     z2 = zr;
11054     lp21 = lptr[lp21];
11055     lp21 = lptr[lp21];
11056     nr = (i__1 = list[lp21], abs(i__1));
11057     xr = x[nr];
11058     yr = y[nr];
11059     zr = z__[nr];
11060     goto L1;
11061 
11062 /*   Bottom of loop -- test for termination of loop. */
11063 
11064 L2:
11065     if (n2 == nfrst) {
11066         goto L3;
11067     }
11068     n2 = nl;
11069     x2 = xl;
11070     y2 = yl;
11071     z2 = zl;
11072     lp = lptr[lp];
11073     goto L1;
11074 
11075 /* Delete N1 and all its incident arcs.  If N1 is an interior */
11076 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
11077 /*   then N1 must be separated from its neighbors by a plane */
11078 /*   containing the origin -- its removal reverses the effect */
11079 /*   of a call to COVSPH, and all its neighbors become */
11080 /*   boundary nodes.  This is achieved by treating it as if */
11081 /*   it were a boundary node (setting BDRY to TRUE, changing */
11082 /*   a sign in LIST, and incrementing NNB). */
11083 
11084 L3:
11085     if (! bdry) {
11086         if (nnb > 3) {
11087             bdry = TRUE_;
11088         } else {
11089             lpf = lptr[lpl];
11090             nr = list[lpf];
11091             lp = lptr[lpf];
11092             n2 = list[lp];
11093             nl = list[lpl];
11094             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11095                     x[n2], &y[n2], &z__[n2]);
11096         }
11097         if (bdry) {
11098 
11099 /*   IF a boundary node already exists, then N1 and its */
11100 /*     neighbors cannot be converted to boundary nodes. */
11101 /*     (They must be collinear.)  This is a problem if */
11102 /*     NNB > 3. */
11103 
11104             i__1 = nn;
11105             for (i__ = 1; i__ <= i__1; ++i__) {
11106                 if (list[lend[i__]] < 0) {
11107                     bdry = FALSE_;
11108                     goto L5;
11109                 }
11110 /* L4: */
11111             }
11112             list[lpl] = -list[lpl];
11113             ++nnb;
11114         }
11115     }
11116 L5:
11117     if (! bdry && nnb > 3) {
11118         goto L24;
11119     }
11120 
11121 /* Initialize for loop on neighbors.  LPL points to the last */
11122 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11123 
11124     lp = lpl;
11125     lnw = *lnew;
11126 
11127 /* Loop on neighbors N2 of N1, beginning with the first. */
11128 
11129 L6:
11130     lp = lptr[lp];
11131     n2 = (i__1 = list[lp], abs(i__1));
11132     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11133     if (lph < 0) {
11134         goto L23;
11135     }
11136 
11137 /*   LP and LPL may require alteration. */
11138 
11139     if (lpl == lnw) {
11140         lpl = lph;
11141     }
11142     if (lp == lnw) {
11143         lp = lph;
11144     }
11145     if (lp != lpl) {
11146         goto L6;
11147     }
11148 
11149 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11150 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11151 /*   which are larger than N1 must be decremented. */
11152 
11153     --nn;
11154     if (n1 > nn) {
11155         goto L9;
11156     }
11157     i__1 = nn;
11158     for (i__ = n1; i__ <= i__1; ++i__) {
11159         x[i__] = x[i__ + 1];
11160         y[i__] = y[i__ + 1];
11161         z__[i__] = z__[i__ + 1];
11162         lend[i__] = lend[i__ + 1];
11163 /* L7: */
11164     }
11165 
11166     i__1 = lnw - 1;
11167     for (i__ = 1; i__ <= i__1; ++i__) {
11168         if (list[i__] > n1) {
11169             --list[i__];
11170         }
11171         if (list[i__] < -n1) {
11172             ++list[i__];
11173         }
11174 /* L8: */
11175     }
11176 
11177 /*   For LPN = first to last neighbors of N1, delete the */
11178 /*     preceding neighbor (indexed by LP). */
11179 
11180 /*   Each empty LIST,LPTR location LP is filled in with the */
11181 /*     values at LNW-1, and LNW is decremented.  All pointers */
11182 /*     (including those in LPTR and LEND) with value LNW-1 */
11183 /*     must be changed to LP. */
11184 
11185 /*  LPL points to the last neighbor of N1. */
11186 
11187 L9:
11188     if (bdry) {
11189         --nnb;
11190     }
11191     lpn = lpl;
11192     i__1 = nnb;
11193     for (j = 1; j <= i__1; ++j) {
11194         --lnw;
11195         lp = lpn;
11196         lpn = lptr[lp];
11197         list[lp] = list[lnw];
11198         lptr[lp] = lptr[lnw];
11199         if (lptr[lpn] == lnw) {
11200             lptr[lpn] = lp;
11201         }
11202         if (lpn == lnw) {
11203             lpn = lp;
11204         }
11205         for (i__ = nn; i__ >= 1; --i__) {
11206             if (lend[i__] == lnw) {
11207                 lend[i__] = lp;
11208                 goto L11;
11209             }
11210 /* L10: */
11211         }
11212 
11213 L11:
11214         for (i__ = lnw - 1; i__ >= 1; --i__) {
11215             if (lptr[i__] == lnw) {
11216                 lptr[i__] = lp;
11217             }
11218 /* L12: */
11219         }
11220 /* L13: */
11221     }
11222 
11223 /* Update N and LNEW, and optimize the patch of triangles */
11224 /*   containing K (on input) by applying swaps to the arcs */
11225 /*   in IWK. */
11226 
11227     *n = nn;
11228     *lnew = lnw;
11229     if (iwl > 0) {
11230         nit = iwl << 2;
11231         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11232                 nit, &iwk[3], &ierr);
11233         if (ierr != 0 && ierr != 1) {
11234             goto L25;
11235         }
11236         if (ierr == 1) {
11237             goto L26;
11238         }
11239     }
11240 
11241 /* Successful termination. */
11242 
11243     *ier = 0;
11244     return 0;
11245 
11246 /* Invalid input parameter. */
11247 
11248 L21:
11249     *ier = 1;
11250     return 0;
11251 
11252 /* Insufficient space reserved for IWK. */
11253 
11254 L22:
11255     *ier = 2;
11256     return 0;
11257 
11258 /* Invalid triangulation data structure.  NNB < 3 on input or */
11259 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11260 
11261 L23:
11262     *ier = 3;
11263     return 0;
11264 
11265 /* N1 is interior but NNB could not be reduced to 3. */
11266 
11267 L24:
11268     *ier = 4;
11269     return 0;
11270 
11271 /* Error flag (other than 1) returned by OPTIM. */
11272 
11273 L25:
11274     *ier = 5;
11275 /*      WRITE (*,100) NIT, IERR */
11276 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11277 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11278     return 0;
11279 
11280 /* Error flag 1 returned by OPTIM. */
11281 
11282 L26:
11283     *ier = 6;
11284     return 0;
11285 } /* delnod_ */
11286 
11287 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11288         double *tol, int *nseg)
11289 {
11290     /* System generated locals */
11291     int i__1;
11292     double d__1;
11293 
11294     /* Builtin functions */
11295     //double sqrt(double);
11296 
11297     /* Local variables */
11298     static int i__, k;
11299     static double s, p1[3], p2[3], u1, u2, v1, v2;
11300     static int na;
11301     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11302 
11303 
11304 /* *********************************************************** */
11305 
11306 /*                                              From STRIPACK */
11307 /*                                            Robert J. Renka */
11308 /*                                  Dept. of Computer Science */
11309 /*                                       Univ. of North Texas */
11310 /*                                           renka@cs.unt.edu */
11311 /*                                                   03/04/03 */
11312 
11313 /*   Given unit vectors P and Q corresponding to northern */
11314 /* hemisphere points (with positive third components), this */
11315 /* subroutine draws a polygonal line which approximates the */
11316 /* projection of arc P-Q onto the plane containing the */
11317 /* equator. */
11318 
11319 /*   The line segment is drawn by writing a sequence of */
11320 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11321 /* is assumed that an open file is attached to the unit, */
11322 /* header comments have been written to the file, a window- */
11323 /* to-viewport mapping has been established, etc. */
11324 
11325 /* On input: */
11326 
11327 /*       LUN = long int unit number in the range 0 to 99. */
11328 
11329 /*       P,Q = Arrays of length 3 containing the endpoints of */
11330 /*             the arc to be drawn. */
11331 
11332 /*       TOL = Maximum distance in world coordinates between */
11333 /*             the projected arc and polygonal line. */
11334 
11335 /* Input parameters are not altered by this routine. */
11336 
11337 /* On output: */
11338 
11339 /*       NSEG = Number of line segments in the polygonal */
11340 /*              approximation to the projected arc.  This is */
11341 /*              a decreasing function of TOL.  NSEG = 0 and */
11342 /*              no drawing is performed if P = Q or P = -Q */
11343 /*              or an error is encountered in writing to unit */
11344 /*              LUN. */
11345 
11346 /* STRIPACK modules required by DRWARC:  None */
11347 
11348 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11349 
11350 /* *********************************************************** */
11351 
11352 
11353 /* Local parameters: */
11354 
11355 /* DP =    (Q-P)/NSEG */
11356 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11357 /*           onto the projection plane */
11358 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11359 /* ERR =   Orthogonal distance from the projected midpoint */
11360 /*           PM' to the line defined by P' and Q': */
11361 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11362 /* I,K =   DO-loop indexes */
11363 /* NA =    Number of arcs (segments) in the partition of P-Q */
11364 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11365 /*           arc P-Q into NSEG segments; obtained by normal- */
11366 /*           izing PM values */
11367 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11368 /*           uniform partition of the line segment P-Q into */
11369 /*           NSEG segments */
11370 /* S =     Scale factor 1/NA */
11371 /* U1,V1 = Components of P' */
11372 /* U2,V2 = Components of Q' */
11373 /* UM,VM = Components of the midpoint PM' */
11374 
11375 
11376 /* Compute the midpoint PM of arc P-Q. */
11377 
11378     /* Parameter adjustments */
11379     --q;
11380     --p;
11381 
11382     /* Function Body */
11383     enrm = 0.;
11384     for (i__ = 1; i__ <= 3; ++i__) {
11385         pm[i__ - 1] = p[i__] + q[i__];
11386         enrm += pm[i__ - 1] * pm[i__ - 1];
11387 /* L1: */
11388     }
11389     if (enrm == 0.) {
11390         goto L5;
11391     }
11392     enrm = sqrt(enrm);
11393     pm[0] /= enrm;
11394     pm[1] /= enrm;
11395     pm[2] /= enrm;
11396 
11397 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11398 /*   PM' = (UM,VM), respectively. */
11399 
11400     u1 = p[1];
11401     v1 = p[2];
11402     u2 = q[1];
11403     v2 = q[2];
11404     um = pm[0];
11405     vm = pm[1];
11406 
11407 /* Compute the orthogonal distance ERR from PM' to the line */
11408 /*   defined by P' and Q'.  This is the maximum deviation */
11409 /*   between the projected arc and the line segment.  It is */
11410 /*   undefined if P' = Q'. */
11411 
11412     du = u2 - u1;
11413     dv = v2 - v1;
11414     enrm = du * du + dv * dv;
11415     if (enrm == 0.) {
11416         goto L5;
11417     }
11418     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11419 
11420 /* Compute the number of arcs into which P-Q will be parti- */
11421 /*   tioned (the number of line segments to be drawn): */
11422 /*   NA = ERR/TOL. */
11423 
11424     na = (int) (err / *tol + 1.);
11425 
11426 /* Initialize for loop on arcs P1-P2, where the intermediate */
11427 /*   points are obtained by normalizing PM = P + k*DP for */
11428 /*   DP = (Q-P)/NA */
11429 
11430     s = 1. / (double) na;
11431     for (i__ = 1; i__ <= 3; ++i__) {
11432         dp[i__ - 1] = s * (q[i__] - p[i__]);
11433         pm[i__ - 1] = p[i__];
11434         p1[i__ - 1] = p[i__];
11435 /* L2: */
11436     }
11437 
11438 /* Loop on arcs P1-P2, drawing the line segments associated */
11439 /*   with the projected endpoints. */
11440 
11441     i__1 = na - 1;
11442     for (k = 1; k <= i__1; ++k) {
11443         enrm = 0.;
11444         for (i__ = 1; i__ <= 3; ++i__) {
11445             pm[i__ - 1] += dp[i__ - 1];
11446             enrm += pm[i__ - 1] * pm[i__ - 1];
11447 /* L3: */
11448         }
11449         if (enrm == 0.) {
11450             goto L5;
11451         }
11452         enrm = sqrt(enrm);
11453         p2[0] = pm[0] / enrm;
11454         p2[1] = pm[1] / enrm;
11455         p2[2] = pm[2] / enrm;
11456 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11457 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11458         p1[0] = p2[0];
11459         p1[1] = p2[1];
11460         p1[2] = p2[2];
11461 /* L4: */
11462     }
11463 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11464 
11465 /* No error encountered. */
11466 
11467     *nseg = na;
11468     return 0;
11469 
11470 /* Invalid input value of P or Q. */
11471 
11472 L5:
11473     *nseg = 0;
11474     return 0;
11475 } /* drwarc_ */
11476 
11477 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11478         double *y, double *z__, int *lwk, int *iwk, int *
11479         list, int *lptr, int *lend, int *ier)
11480 {
11481     /* System generated locals */
11482     int i__1;
11483 
11484     /* Local variables */
11485     static int i__, n0, n1, n2;
11486     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11487     static int nl, lp, nr;
11488     static double dp12;
11489     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11490     static double dp1l, dp2l, dp1r, dp2r;
11491     static int ierr;
11492     /* Subroutine */ int swap_(int *, int *, int *,
11493             int *, int *, int *, int *, int *);
11494     static int next, iwcp1, n1lst, iwend;
11495     /* Subroutine */ int optim_(double *, double *, double
11496             *, int *, int *, int *, int *, int *, int
11497             *, int *);
11498     static int n1frst;
11499 
11500 
11501 /* *********************************************************** */
11502 
11503 /*                                              From STRIPACK */
11504 /*                                            Robert J. Renka */
11505 /*                                  Dept. of Computer Science */
11506 /*                                       Univ. of North Texas */
11507 /*                                           renka@cs.unt.edu */
11508 /*                                                   07/30/98 */
11509 
11510 /*   Given a triangulation of N nodes and a pair of nodal */
11511 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11512 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11513 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11514 /* lation is input, the resulting triangulation is as close */
11515 /* as possible to a Delaunay triangulation in the sense that */
11516 /* all arcs other than IN1-IN2 are locally optimal. */
11517 
11518 /*   A sequence of calls to EDGE may be used to force the */
11519 /* presence of a set of edges defining the boundary of a non- */
11520 /* convex and/or multiply connected region, or to introduce */
11521 /* barriers into the triangulation.  Note that Subroutine */
11522 /* GETNP will not necessarily return closest nodes if the */
11523 /* triangulation has been constrained by a call to EDGE. */
11524 /* However, this is appropriate in some applications, such */
11525 /* as triangle-based interpolation on a nonconvex domain. */
11526 
11527 
11528 /* On input: */
11529 
11530 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11531 /*                 N defining a pair of nodes to be connected */
11532 /*                 by an arc. */
11533 
11534 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11535 /*               coordinates of the nodes. */
11536 
11537 /* The above parameters are not altered by this routine. */
11538 
11539 /*       LWK = Number of columns reserved for IWK.  This must */
11540 /*             be at least NI -- the number of arcs that */
11541 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11542 
11543 /*       IWK = int work array of length at least 2*LWK. */
11544 
11545 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11546 /*                        gulation.  Refer to Subroutine */
11547 /*                        TRMESH. */
11548 
11549 /* On output: */
11550 
11551 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11552 /*             not more than the input value of LWK) unless */
11553 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11554 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11555 
11556 /*       IWK = Array containing the indexes of the endpoints */
11557 /*             of the new arcs other than IN1-IN2 unless */
11558 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11559 /*             IN1->IN2 are stored in the first K-1 columns */
11560 /*             (left portion of IWK), column K contains */
11561 /*             zeros, and new arcs to the right of IN1->IN2 */
11562 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11563 /*             mined by searching IWK for the zeros.) */
11564 
11565 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11566 /*                        to reflect the presence of an arc */
11567 /*                        connecting IN1 and IN2 unless IER > */
11568 /*                        0.  The data structure has been */
11569 /*                        altered if IER >= 4. */
11570 
11571 /*       IER = Error indicator: */
11572 /*             IER = 0 if no errors were encountered. */
11573 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11574 /*                     or LWK < 0 on input. */
11575 /*             IER = 2 if more space is required in IWK. */
11576 /*                     Refer to LWK. */
11577 /*             IER = 3 if IN1 and IN2 could not be connected */
11578 /*                     due to either an invalid data struc- */
11579 /*                     ture or collinear nodes (and floating */
11580 /*                     point error). */
11581 /*             IER = 4 if an error flag other than IER = 1 */
11582 /*                     was returned by OPTIM. */
11583 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11584 /*                     This is not necessarily an error, but */
11585 /*                     the arcs other than IN1-IN2 may not */
11586 /*                     be optimal. */
11587 
11588 /*   An error message is written to the standard output unit */
11589 /* in the case of IER = 3 or IER = 4. */
11590 
11591 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11592 /*                              SWPTST */
11593 
11594 /* Intrinsic function called by EDGE:  ABS */
11595 
11596 /* *********************************************************** */
11597 
11598 
11599 /* Local parameters: */
11600 
11601 /* DPij =     Dot product <Ni,Nj> */
11602 /* I =        DO-loop index and column index for IWK */
11603 /* IERR =     Error flag returned by Subroutine OPTIM */
11604 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11605 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11606 /* IWCP1 =    IWC + 1 */
11607 /* IWEND =    Input or output value of LWK */
11608 /* IWF =      IWK (column) index of the first (leftmost) arc */
11609 /*              which intersects IN1->IN2 */
11610 /* IWL =      IWK (column) index of the last (rightmost) are */
11611 /*              which intersects IN1->IN2 */
11612 /* LFT =      Flag used to determine if a swap results in the */
11613 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11614 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11615 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11616 /* LP =       List pointer (index for LIST and LPTR) */
11617 /* LP21 =     Unused parameter returned by SWAP */
11618 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11619 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11620 /* N1,N2 =    Local copies of IN1 and IN2 */
11621 /* N1FRST =   First neighbor of IN1 */
11622 /* N1LST =    (Signed) last neighbor of IN1 */
11623 /* NEXT =     Node opposite NL->NR */
11624 /* NIT =      Flag or number of iterations employed by OPTIM */
11625 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11626 /*              with NL LEFT IN1->IN2 */
11627 /* X0,Y0,Z0 = Coordinates of N0 */
11628 /* X1,Y1,Z1 = Coordinates of IN1 */
11629 /* X2,Y2,Z2 = Coordinates of IN2 */
11630 
11631 
11632 /* Store IN1, IN2, and LWK in local variables and test for */
11633 /*   errors. */
11634 
11635     /* Parameter adjustments */
11636     --lend;
11637     --lptr;
11638     --list;
11639     iwk -= 3;
11640     --z__;
11641     --y;
11642     --x;
11643 
11644     /* Function Body */
11645     n1 = *in1;
11646     n2 = *in2;
11647     iwend = *lwk;
11648     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11649         goto L31;
11650     }
11651 
11652 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11653 /*   neighbor of N1. */
11654 
11655     lpl = lend[n1];
11656     n0 = (i__1 = list[lpl], abs(i__1));
11657     lp = lpl;
11658 L1:
11659     if (n0 == n2) {
11660         goto L30;
11661     }
11662     lp = lptr[lp];
11663     n0 = list[lp];
11664     if (lp != lpl) {
11665         goto L1;
11666     }
11667 
11668 /* Initialize parameters. */
11669 
11670     iwl = 0;
11671     nit = 0;
11672 
11673 /* Store the coordinates of N1 and N2. */
11674 
11675 L2:
11676     x1 = x[n1];
11677     y1 = y[n1];
11678     z1 = z__[n1];
11679     x2 = x[n2];
11680     y2 = y[n2];
11681     z2 = z__[n2];
11682 
11683 /* Set NR and NL to adjacent neighbors of N1 such that */
11684 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11685 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11686 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11687 
11688 /*   Initialization:  Set N1FRST and N1LST to the first and */
11689 /*     (signed) last neighbors of N1, respectively, and */
11690 /*     initialize NL to N1FRST. */
11691 
11692     lpl = lend[n1];
11693     n1lst = list[lpl];
11694     lp = lptr[lpl];
11695     n1frst = list[lp];
11696     nl = n1frst;
11697     if (n1lst < 0) {
11698         goto L4;
11699     }
11700 
11701 /*   N1 is an interior node.  Set NL to the first candidate */
11702 /*     for NR (NL LEFT N2->N1). */
11703 
11704 L3:
11705     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11706         goto L4;
11707     }
11708     lp = lptr[lp];
11709     nl = list[lp];
11710     if (nl != n1frst) {
11711         goto L3;
11712     }
11713 
11714 /*   All neighbors of N1 are strictly left of N1->N2. */
11715 
11716     goto L5;
11717 
11718 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11719 /*     following neighbor of N1. */
11720 
11721 L4:
11722     nr = nl;
11723     lp = lptr[lp];
11724     nl = (i__1 = list[lp], abs(i__1));
11725     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11726 
11727 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11728 /*     are employed to avoid an error associated with */
11729 /*     collinear nodes. */
11730 
11731         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11732         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11733         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11734         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11735         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11736         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11737                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11738             goto L6;
11739         }
11740 
11741 /*   NL-NR does not intersect N1-N2.  However, there is */
11742 /*     another candidate for the first arc if NL lies on */
11743 /*     the line N1-N2. */
11744 
11745         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11746             goto L5;
11747         }
11748     }
11749 
11750 /*   Bottom of loop. */
11751 
11752     if (nl != n1frst) {
11753         goto L4;
11754     }
11755 
11756 /* Either the triangulation is invalid or N1-N2 lies on the */
11757 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11758 /*   intersecting N1-N2) was not found due to floating point */
11759 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11760 /*   has already been done. */
11761 
11762 L5:
11763     if (nit > 0) {
11764         goto L33;
11765     }
11766     nit = 1;
11767     n1 = n2;
11768     n2 = *in1;
11769     goto L2;
11770 
11771 /* Store the ordered sequence of intersecting edges NL->NR in */
11772 /*   IWK(1,IWL)->IWK(2,IWL). */
11773 
11774 L6:
11775     ++iwl;
11776     if (iwl > iwend) {
11777         goto L32;
11778     }
11779     iwk[(iwl << 1) + 1] = nl;
11780     iwk[(iwl << 1) + 2] = nr;
11781 
11782 /*   Set NEXT to the neighbor of NL which follows NR. */
11783 
11784     lpl = lend[nl];
11785     lp = lptr[lpl];
11786 
11787 /*   Find NR as a neighbor of NL.  The search begins with */
11788 /*     the first neighbor. */
11789 
11790 L7:
11791     if (list[lp] == nr) {
11792         goto L8;
11793     }
11794     lp = lptr[lp];
11795     if (lp != lpl) {
11796         goto L7;
11797     }
11798 
11799 /*   NR must be the last neighbor, and NL->NR cannot be a */
11800 /*     boundary edge. */
11801 
11802     if (list[lp] != nr) {
11803         goto L33;
11804     }
11805 
11806 /*   Set NEXT to the neighbor following NR, and test for */
11807 /*     termination of the store loop. */
11808 
11809 L8:
11810     lp = lptr[lp];
11811     next = (i__1 = list[lp], abs(i__1));
11812     if (next == n2) {
11813         goto L9;
11814     }
11815 
11816 /*   Set NL or NR to NEXT. */
11817 
11818     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11819         nl = next;
11820     } else {
11821         nr = next;
11822     }
11823     goto L6;
11824 
11825 /* IWL is the number of arcs which intersect N1-N2. */
11826 /*   Store LWK. */
11827 
11828 L9:
11829     *lwk = iwl;
11830     iwend = iwl;
11831 
11832 /* Initialize for edge swapping loop -- all possible swaps */
11833 /*   are applied (even if the new arc again intersects */
11834 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11835 /*   left portion of IWK, and arcs to the right are stored in */
11836 /*   the right portion.  IWF and IWL index the first and last */
11837 /*   intersecting arcs. */
11838 
11839     iwf = 1;
11840 
11841 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11842 /*   IWC points to the arc currently being processed.  LFT */
11843 /*   .LE. 0 iff N0 LEFT N1->N2. */
11844 
11845 L10:
11846     lft = 0;
11847     n0 = n1;
11848     x0 = x1;
11849     y0 = y1;
11850     z0 = z1;
11851     nl = iwk[(iwf << 1) + 1];
11852     nr = iwk[(iwf << 1) + 2];
11853     iwc = iwf;
11854 
11855 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
11856 /*     last arc. */
11857 
11858 L11:
11859     if (iwc == iwl) {
11860         goto L21;
11861     }
11862     iwcp1 = iwc + 1;
11863     next = iwk[(iwcp1 << 1) + 1];
11864     if (next != nl) {
11865         goto L16;
11866     }
11867     next = iwk[(iwcp1 << 1) + 2];
11868 
11869 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
11870 /*     swap. */
11871 
11872     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11873             z__[next])) {
11874         goto L14;
11875     }
11876     if (lft >= 0) {
11877         goto L12;
11878     }
11879     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11880             z__[next])) {
11881         goto L14;
11882     }
11883 
11884 /*   Replace NL->NR with N0->NEXT. */
11885 
11886     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11887     iwk[(iwc << 1) + 1] = n0;
11888     iwk[(iwc << 1) + 2] = next;
11889     goto L15;
11890 
11891 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
11892 /*     the left, and store N0-NEXT in the right portion of */
11893 /*     IWK. */
11894 
11895 L12:
11896     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11897     i__1 = iwl;
11898     for (i__ = iwcp1; i__ <= i__1; ++i__) {
11899         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11900         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11901 /* L13: */
11902     }
11903     iwk[(iwl << 1) + 1] = n0;
11904     iwk[(iwl << 1) + 2] = next;
11905     --iwl;
11906     nr = next;
11907     goto L11;
11908 
11909 /*   A swap is not possible.  Set N0 to NR. */
11910 
11911 L14:
11912     n0 = nr;
11913     x0 = x[n0];
11914     y0 = y[n0];
11915     z0 = z__[n0];
11916     lft = 1;
11917 
11918 /*   Advance to the next arc. */
11919 
11920 L15:
11921     nr = next;
11922     ++iwc;
11923     goto L11;
11924 
11925 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
11926 /*     Test for a possible swap. */
11927 
11928 L16:
11929     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11930             z__[next])) {
11931         goto L19;
11932     }
11933     if (lft <= 0) {
11934         goto L17;
11935     }
11936     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11937             z__[next])) {
11938         goto L19;
11939     }
11940 
11941 /*   Replace NL->NR with NEXT->N0. */
11942 
11943     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11944     iwk[(iwc << 1) + 1] = next;
11945     iwk[(iwc << 1) + 2] = n0;
11946     goto L20;
11947 
11948 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
11949 /*     the right, and store N0-NEXT in the left portion of */
11950 /*     IWK. */
11951 
11952 L17:
11953     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11954     i__1 = iwf;
11955     for (i__ = iwc - 1; i__ >= i__1; --i__) {
11956         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11957         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11958 /* L18: */
11959     }
11960     iwk[(iwf << 1) + 1] = n0;
11961     iwk[(iwf << 1) + 2] = next;
11962     ++iwf;
11963     goto L20;
11964 
11965 /*   A swap is not possible.  Set N0 to NL. */
11966 
11967 L19:
11968     n0 = nl;
11969     x0 = x[n0];
11970     y0 = y[n0];
11971     z0 = z__[n0];
11972     lft = -1;
11973 
11974 /*   Advance to the next arc. */
11975 
11976 L20:
11977     nl = next;
11978     ++iwc;
11979     goto L11;
11980 
11981 /*   N2 is opposite NL->NR (IWC = IWL). */
11982 
11983 L21:
11984     if (n0 == n1) {
11985         goto L24;
11986     }
11987     if (lft < 0) {
11988         goto L22;
11989     }
11990 
11991 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
11992 
11993     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11994         goto L10;
11995     }
11996 
11997 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
11998 /*     portion of IWK. */
11999 
12000     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12001     iwk[(iwl << 1) + 1] = n0;
12002     iwk[(iwl << 1) + 2] = n2;
12003     --iwl;
12004     goto L10;
12005 
12006 /*   N0 LEFT N1->N2.  Test for a possible swap. */
12007 
12008 L22:
12009     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12010         goto L10;
12011     }
12012 
12013 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
12014 /*     right, and store N0-N2 in the left portion of IWK. */
12015 
12016     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12017     i__ = iwl;
12018 L23:
12019     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12020     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12021     --i__;
12022     if (i__ > iwf) {
12023         goto L23;
12024     }
12025     iwk[(iwf << 1) + 1] = n0;
12026     iwk[(iwf << 1) + 2] = n2;
12027     ++iwf;
12028     goto L10;
12029 
12030 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
12031 /*   store zeros in IWK. */
12032 
12033 L24:
12034     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12035     iwk[(iwc << 1) + 1] = 0;
12036     iwk[(iwc << 1) + 2] = 0;
12037 
12038 /* Optimization procedure -- */
12039 
12040     *ier = 0;
12041     if (iwc > 1) {
12042 
12043 /*   Optimize the set of new arcs to the left of IN1->IN2. */
12044 
12045         nit = iwc - (1<<2);
12046         i__1 = iwc - 1;
12047         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12048                 nit, &iwk[3], &ierr);
12049         if (ierr != 0 && ierr != 1) {
12050             goto L34;
12051         }
12052         if (ierr == 1) {
12053             *ier = 5;
12054         }
12055     }
12056     if (iwc < iwend) {
12057 
12058 /*   Optimize the set of new arcs to the right of IN1->IN2. */
12059 
12060         nit = iwend - (iwc<<2);
12061         i__1 = iwend - iwc;
12062         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12063                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12064         if (ierr != 0 && ierr != 1) {
12065             goto L34;
12066         }
12067         if (ierr == 1) {
12068             goto L35;
12069         }
12070     }
12071     if (*ier == 5) {
12072         goto L35;
12073     }
12074 
12075 /* Successful termination (IER = 0). */
12076 
12077     return 0;
12078 
12079 /* IN1 and IN2 were adjacent on input. */
12080 
12081 L30:
12082     *ier = 0;
12083     return 0;
12084 
12085 /* Invalid input parameter. */
12086 
12087 L31:
12088     *ier = 1;
12089     return 0;
12090 
12091 /* Insufficient space reserved for IWK. */
12092 
12093 L32:
12094     *ier = 2;
12095     return 0;
12096 
12097 /* Invalid triangulation data structure or collinear nodes */
12098 /*   on convex hull boundary. */
12099 
12100 L33:
12101     *ier = 3;
12102 /*      WRITE (*,130) IN1, IN2 */
12103 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12104 /*     .        'tion or null triangles on boundary'/ */
12105 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12106     return 0;
12107 
12108 /* Error flag (other than 1) returned by OPTIM. */
12109 
12110 L34:
12111     *ier = 4;
12112 /*      WRITE (*,140) NIT, IERR */
12113 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12114 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12115     return 0;
12116 
12117 /* Error flag 1 returned by OPTIM. */
12118 
12119 L35:
12120     *ier = 5;
12121     return 0;
12122 } /* edge_ */
12123 
12124 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12125         int *list, int *lptr, int *lend, int *l, int *
12126         npts, double *df, int *ier)
12127 {
12128     /* System generated locals */
12129     int i__1, i__2;
12130 
12131     /* Local variables */
12132     static int i__, n1;
12133     static double x1, y1, z1;
12134     static int nb, ni, lp, np, lm1;
12135     static double dnb, dnp;
12136     static int lpl;
12137 
12138 
12139 /* *********************************************************** */
12140 
12141 /*                                              From STRIPACK */
12142 /*                                            Robert J. Renka */
12143 /*                                  Dept. of Computer Science */
12144 /*                                       Univ. of North Texas */
12145 /*                                           renka@cs.unt.edu */
12146 /*                                                   07/28/98 */
12147 
12148 /*   Given a Delaunay triangulation of N nodes on the unit */
12149 /* sphere and an array NPTS containing the indexes of L-1 */
12150 /* nodes ordered by angular distance from NPTS(1), this sub- */
12151 /* routine sets NPTS(L) to the index of the next node in the */
12152 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12153 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12154 /* of K closest nodes to N1 (including N1) may be determined */
12155 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12156 /* for K .GE. 2. */
12157 
12158 /*   The algorithm uses the property of a Delaunay triangula- */
12159 /* tion that the K-th closest node to N1 is a neighbor of one */
12160 /* of the K-1 closest nodes to N1. */
12161 
12162 
12163 /* On input: */
12164 
12165 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12166 /*               coordinates of the nodes. */
12167 
12168 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12169 /*                        fer to Subroutine TRMESH. */
12170 
12171 /*       L = Number of nodes in the sequence on output.  2 */
12172 /*           .LE. L .LE. N. */
12173 
12174 /* The above parameters are not altered by this routine. */
12175 
12176 /*       NPTS = Array of length .GE. L containing the indexes */
12177 /*              of the L-1 closest nodes to NPTS(1) in the */
12178 /*              first L-1 locations. */
12179 
12180 /* On output: */
12181 
12182 /*       NPTS = Array updated with the index of the L-th */
12183 /*              closest node to NPTS(1) in position L unless */
12184 /*              IER = 1. */
12185 
12186 /*       DF = Value of an increasing function (negative cos- */
12187 /*            ine) of the angular distance between NPTS(1) */
12188 /*            and NPTS(L) unless IER = 1. */
12189 
12190 /*       IER = Error indicator: */
12191 /*             IER = 0 if no errors were encountered. */
12192 /*             IER = 1 if L < 2. */
12193 
12194 /* Modules required by GETNP:  None */
12195 
12196 /* Intrinsic function called by GETNP:  ABS */
12197 
12198 /* *********************************************************** */
12199 
12200 
12201 /* Local parameters: */
12202 
12203 /* DNB,DNP =  Negative cosines of the angular distances from */
12204 /*              N1 to NB and to NP, respectively */
12205 /* I =        NPTS index and DO-loop index */
12206 /* LM1 =      L-1 */
12207 /* LP =       LIST pointer of a neighbor of NI */
12208 /* LPL =      Pointer to the last neighbor of NI */
12209 /* N1 =       NPTS(1) */
12210 /* NB =       Neighbor of NI and candidate for NP */
12211 /* NI =       NPTS(I) */
12212 /* NP =       Candidate for NPTS(L) */
12213 /* X1,Y1,Z1 = Coordinates of N1 */
12214 
12215     /* Parameter adjustments */
12216     --x;
12217     --y;
12218     --z__;
12219     --list;
12220     --lptr;
12221     --lend;
12222     --npts;
12223 
12224     /* Function Body */
12225     lm1 = *l - 1;
12226     if (lm1 < 1) {
12227         goto L6;
12228     }
12229     *ier = 0;
12230 
12231 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12232 
12233     n1 = npts[1];
12234     x1 = x[n1];
12235     y1 = y[n1];
12236     z1 = z__[n1];
12237     i__1 = lm1;
12238     for (i__ = 1; i__ <= i__1; ++i__) {
12239         ni = npts[i__];
12240         lend[ni] = -lend[ni];
12241 /* L1: */
12242     }
12243 
12244 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12245 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12246 /*   (the maximum distance). */
12247 
12248     dnp = 2.;
12249 
12250 /* Loop on nodes NI in NPTS. */
12251 
12252     i__1 = lm1;
12253     for (i__ = 1; i__ <= i__1; ++i__) {
12254         ni = npts[i__];
12255         lpl = -lend[ni];
12256         lp = lpl;
12257 
12258 /* Loop on neighbors NB of NI. */
12259 
12260 L2:
12261         nb = (i__2 = list[lp], abs(i__2));
12262         if (lend[nb] < 0) {
12263             goto L3;
12264         }
12265 
12266 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12267 /*   closer to N1. */
12268 
12269         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12270         if (dnb >= dnp) {
12271             goto L3;
12272         }
12273         np = nb;
12274         dnp = dnb;
12275 L3:
12276         lp = lptr[lp];
12277         if (lp != lpl) {
12278             goto L2;
12279         }
12280 /* L4: */
12281     }
12282     npts[*l] = np;
12283     *df = dnp;
12284 
12285 /* Unmark the elements of NPTS. */
12286 
12287     i__1 = lm1;
12288     for (i__ = 1; i__ <= i__1; ++i__) {
12289         ni = npts[i__];
12290         lend[ni] = -lend[ni];
12291 /* L5: */
12292     }
12293     return 0;
12294 
12295 /* L is outside its valid range. */
12296 
12297 L6:
12298     *ier = 1;
12299     return 0;
12300 } /* getnp_ */
12301 
12302 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12303         lptr, int *lnew)
12304 {
12305     static int lsav;
12306 
12307 
12308 /* *********************************************************** */
12309 
12310 /*                                              From STRIPACK */
12311 /*                                            Robert J. Renka */
12312 /*                                  Dept. of Computer Science */
12313 /*                                       Univ. of North Texas */
12314 /*                                           renka@cs.unt.edu */
12315 /*                                                   07/17/96 */
12316 
12317 /*   This subroutine inserts K as a neighbor of N1 following */
12318 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12319 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12320 /* become the first neighbor (even if N1 is a boundary node). */
12321 
12322 /*   This routine is identical to the similarly named routine */
12323 /* in TRIPACK. */
12324 
12325 
12326 /* On input: */
12327 
12328 /*       K = Index of the node to be inserted. */
12329 
12330 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12331 
12332 /* The above parameters are not altered by this routine. */
12333 
12334 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12335 /*                        gulation.  Refer to Subroutine */
12336 /*                        TRMESH. */
12337 
12338 /* On output: */
12339 
12340 /*       LIST,LPTR,LNEW = Data structure updated with the */
12341 /*                        addition of node K. */
12342 
12343 /* Modules required by INSERT:  None */
12344 
12345 /* *********************************************************** */
12346 
12347 
12348     /* Parameter adjustments */
12349     --lptr;
12350     --list;
12351 
12352     /* Function Body */
12353     lsav = lptr[*lp];
12354     lptr[*lp] = *lnew;
12355     list[*lnew] = *k;
12356     lptr[*lnew] = lsav;
12357     ++(*lnew);
12358     return 0;
12359 } /* insert_ */
12360 
12361 long int inside_(double *p, int *lv, double *xv, double *yv,
12362         double *zv, int *nv, int *listv, int *ier)
12363 {
12364     /* Initialized data */
12365 
12366     static double eps = .001;
12367 
12368     /* System generated locals */
12369     int i__1;
12370     long int ret_val = 0;
12371 
12372     /* Builtin functions */
12373     //double sqrt(double);
12374 
12375     /* Local variables */
12376     static double b[3], d__;
12377     static int k, n;
12378     static double q[3];
12379     static int i1, i2, k0;
12380     static double v1[3], v2[3], cn[3], bp, bq;
12381     static int ni;
12382     static double pn[3], qn[3], vn[3];
12383     static int imx;
12384     static long int lft1, lft2, even;
12385     static int ierr;
12386     static long int pinr, qinr;
12387     static double qnrm, vnrm;
12388     /* Subroutine */ int intrsc_(double *, double *,
12389             double *, double *, int *);
12390 
12391 
12392 /* *********************************************************** */
12393 
12394 /*                                              From STRIPACK */
12395 /*                                            Robert J. Renka */
12396 /*                                  Dept. of Computer Science */
12397 /*                                       Univ. of North Texas */
12398 /*                                           renka@cs.unt.edu */
12399 /*                                                   12/27/93 */
12400 
12401 /*   This function locates a point P relative to a polygonal */
12402 /* region R on the surface of the unit sphere, returning */
12403 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12404 /* defined by a cyclically ordered sequence of vertices which */
12405 /* form a positively-oriented simple closed curve.  Adjacent */
12406 /* vertices need not be distinct but the curve must not be */
12407 /* self-intersecting.  Also, while polygon edges are by defi- */
12408 /* nition restricted to a single hemisphere, R is not so */
12409 /* restricted.  Its interior is the region to the left as the */
12410 /* vertices are traversed in order. */
12411 
12412 /*   The algorithm consists of selecting a point Q in R and */
12413 /* then finding all points at which the great circle defined */
12414 /* by P and Q intersects the boundary of R.  P lies inside R */
12415 /* if and only if there is an even number of intersection */
12416 /* points between Q and P.  Q is taken to be a point immedi- */
12417 /* ately to the left of a directed boundary edge -- the first */
12418 /* one that results in no consistency-check failures. */
12419 
12420 /*   If P is close to the polygon boundary, the problem is */
12421 /* ill-conditioned and the decision may be incorrect.  Also, */
12422 /* an incorrect decision may result from a poor choice of Q */
12423 /* (if, for example, a boundary edge lies on the great cir- */
12424 /* cle defined by P and Q).  A more reliable result could be */
12425 /* obtained by a sequence of calls to INSIDE with the ver- */
12426 /* tices cyclically permuted before each call (to alter the */
12427 /* choice of Q). */
12428 
12429 
12430 /* On input: */
12431 
12432 /*       P = Array of length 3 containing the Cartesian */
12433 /*           coordinates of the point (unit vector) to be */
12434 /*           located. */
12435 
12436 /*       LV = Length of arrays XV, YV, and ZV. */
12437 
12438 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12439 /*                  sian coordinates of unit vectors (points */
12440 /*                  on the unit sphere).  These values are */
12441 /*                  not tested for validity. */
12442 
12443 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12444 /*            .LE. LV. */
12445 
12446 /*       LISTV = Array of length NV containing the indexes */
12447 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12448 /*               (and CCW-ordered) sequence of vertices that */
12449 /*               define R.  The last vertex (indexed by */
12450 /*               LISTV(NV)) is followed by the first (indexed */
12451 /*               by LISTV(1)).  LISTV entries must be in the */
12452 /*               range 1 to LV. */
12453 
12454 /* Input parameters are not altered by this function. */
12455 
12456 /* On output: */
12457 
12458 /*       INSIDE = TRUE if and only if P lies inside R unless */
12459 /*                IER .NE. 0, in which case the value is not */
12460 /*                altered. */
12461 
12462 /*       IER = Error indicator: */
12463 /*             IER = 0 if no errors were encountered. */
12464 /*             IER = 1 if LV or NV is outside its valid */
12465 /*                     range. */
12466 /*             IER = 2 if a LISTV entry is outside its valid */
12467 /*                     range. */
12468 /*             IER = 3 if the polygon boundary was found to */
12469 /*                     be self-intersecting.  This error will */
12470 /*                     not necessarily be detected. */
12471 /*             IER = 4 if every choice of Q (one for each */
12472 /*                     boundary edge) led to failure of some */
12473 /*                     internal consistency check.  The most */
12474 /*                     likely cause of this error is invalid */
12475 /*                     input:  P = (0,0,0), a null or self- */
12476 /*                     intersecting polygon, etc. */
12477 
12478 /* Module required by INSIDE:  INTRSC */
12479 
12480 /* Intrinsic function called by INSIDE:  SQRT */
12481 
12482 /* *********************************************************** */
12483 
12484 
12485 /* Local parameters: */
12486 
12487 /* B =         Intersection point between the boundary and */
12488 /*               the great circle defined by P and Q */
12489 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12490 /*               intersection points B that lie between P and */
12491 /*               Q (on the shorter arc) -- used to find the */
12492 /*               closest intersection points to P and Q */
12493 /* CN =        Q X P = normal to the plane of P and Q */
12494 /* D =         Dot product <B,P> or <B,Q> */
12495 /* EPS =       Parameter used to define Q as the point whose */
12496 /*               orthogonal distance to (the midpoint of) */
12497 /*               boundary edge V1->V2 is approximately EPS/ */
12498 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12499 /* EVEN =      TRUE iff an even number of intersection points */
12500 /*               lie between P and Q (on the shorter arc) */
12501 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12502 /*               boundary vertices (endpoints of a boundary */
12503 /*               edge) */
12504 /* IERR =      Error flag for calls to INTRSC (not tested) */
12505 /* IMX =       Local copy of LV and maximum value of I1 and */
12506 /*               I2 */
12507 /* K =         DO-loop index and LISTV index */
12508 /* K0 =        LISTV index of the first endpoint of the */
12509 /*               boundary edge used to compute Q */
12510 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12511 /*               the boundary traversal:  TRUE iff the vertex */
12512 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12513 /* N =         Local copy of NV */
12514 /* NI =        Number of intersections (between the boundary */
12515 /*               curve and the great circle P-Q) encountered */
12516 /* PINR =      TRUE iff P is to the left of the directed */
12517 /*               boundary edge associated with the closest */
12518 /*               intersection point to P that lies between P */
12519 /*               and Q (a left-to-right intersection as */
12520 /*               viewed from Q), or there is no intersection */
12521 /*               between P and Q (on the shorter arc) */
12522 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12523 /*               locate intersections B relative to arc Q->P */
12524 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12525 /*               the boundary edge indexed by LISTV(K0) -> */
12526 /*               LISTV(K0+1) */
12527 /* QINR =      TRUE iff Q is to the left of the directed */
12528 /*               boundary edge associated with the closest */
12529 /*               intersection point to Q that lies between P */
12530 /*               and Q (a right-to-left intersection as */
12531 /*               viewed from Q), or there is no intersection */
12532 /*               between P and Q (on the shorter arc) */
12533 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12534 /*               compute (normalize) Q */
12535 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12536 /*               traversal */
12537 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12538 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12539 /* VNRM =      Euclidean norm of VN */
12540 
12541     /* Parameter adjustments */
12542     --p;
12543     --zv;
12544     --yv;
12545     --xv;
12546     --listv;
12547 
12548     /* Function Body */
12549 
12550 /* Store local parameters, test for error 1, and initialize */
12551 /*   K0. */
12552 
12553     imx = *lv;
12554     n = *nv;
12555     if (n < 3 || n > imx) {
12556         goto L11;
12557     }
12558     k0 = 0;
12559     i1 = listv[1];
12560     if (i1 < 1 || i1 > imx) {
12561         goto L12;
12562     }
12563 
12564 /* Increment K0 and set Q to a point immediately to the left */
12565 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12566 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12567 
12568 L1:
12569     ++k0;
12570     if (k0 > n) {
12571         goto L14;
12572     }
12573     i1 = listv[k0];
12574     if (k0 < n) {
12575         i2 = listv[k0 + 1];
12576     } else {
12577         i2 = listv[1];
12578     }
12579     if (i2 < 1 || i2 > imx) {
12580         goto L12;
12581     }
12582     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12583     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12584     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12585     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12586     if (vnrm == 0.) {
12587         goto L1;
12588     }
12589     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12590     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12591     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12592     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12593     q[0] /= qnrm;
12594     q[1] /= qnrm;
12595     q[2] /= qnrm;
12596 
12597 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12598 
12599     cn[0] = q[1] * p[3] - q[2] * p[2];
12600     cn[1] = q[2] * p[1] - q[0] * p[3];
12601     cn[2] = q[0] * p[2] - q[1] * p[1];
12602     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12603         goto L1;
12604     }
12605     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12606     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12607     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12608     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12609     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12610     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12611 
12612 /* Initialize parameters for the boundary traversal. */
12613 
12614     ni = 0;
12615     even = TRUE_;
12616     bp = -2.;
12617     bq = -2.;
12618     pinr = TRUE_;
12619     qinr = TRUE_;
12620     i2 = listv[n];
12621     if (i2 < 1 || i2 > imx) {
12622         goto L12;
12623     }
12624     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12625 
12626 /* Loop on boundary arcs I1->I2. */
12627 
12628     i__1 = n;
12629     for (k = 1; k <= i__1; ++k) {
12630         i1 = i2;
12631         lft1 = lft2;
12632         i2 = listv[k];
12633         if (i2 < 1 || i2 > imx) {
12634             goto L12;
12635         }
12636         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12637         if (lft1 == lft2) {
12638             goto L2;
12639         }
12640 
12641 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12642 /*     point of intersection B. */
12643 
12644         ++ni;
12645         v1[0] = xv[i1];
12646         v1[1] = yv[i1];
12647         v1[2] = zv[i1];
12648         v2[0] = xv[i2];
12649         v2[1] = yv[i2];
12650         v2[2] = zv[i2];
12651         intrsc_(v1, v2, cn, b, &ierr);
12652 
12653 /*   B is between Q and P (on the shorter arc) iff */
12654 /*     B Forward Q->P and B Forward P->Q       iff */
12655 /*     <B,QN> > 0 and <B,PN> > 0. */
12656 
12657         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12658                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12659 
12660 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12661 
12662             even = ! even;
12663             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12664             if (d__ > bq) {
12665                 bq = d__;
12666                 qinr = lft2;
12667             }
12668             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12669             if (d__ > bp) {
12670                 bp = d__;
12671                 pinr = lft1;
12672             }
12673         }
12674 L2:
12675         ;
12676     }
12677 
12678 /* Test for consistency:  NI must be even and QINR must be */
12679 /*   TRUE. */
12680 
12681     if (ni != ni / 2 << 1 || ! qinr) {
12682         goto L1;
12683     }
12684 
12685 /* Test for error 3:  different values of PINR and EVEN. */
12686 
12687     if (pinr != even) {
12688         goto L13;
12689     }
12690 
12691 /* No error encountered. */
12692 
12693     *ier = 0;
12694     ret_val = even;
12695     return ret_val;
12696 
12697 /* LV or NV is outside its valid range. */
12698 
12699 L11:
12700     *ier = 1;
12701     return ret_val;
12702 
12703 /* A LISTV entry is outside its valid range. */
12704 
12705 L12:
12706     *ier = 2;
12707     return ret_val;
12708 
12709 /* The polygon boundary is self-intersecting. */
12710 
12711 L13:
12712     *ier = 3;
12713     return ret_val;
12714 
12715 /* Consistency tests failed for all values of Q. */
12716 
12717 L14:
12718     *ier = 4;
12719     return ret_val;
12720 } /* inside_ */
12721 
12722 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12723         i3, int *list, int *lptr, int *lend, int *lnew)
12724 {
12725     static int k, n1, n2, n3, lp;
12726     /* Subroutine */ int insert_(int *, int *, int *,
12727             int *, int *);
12728     int lstptr_(int *, int *, int *, int *);
12729 
12730 
12731 /* *********************************************************** */
12732 
12733 /*                                              From STRIPACK */
12734 /*                                            Robert J. Renka */
12735 /*                                  Dept. of Computer Science */
12736 /*                                       Univ. of North Texas */
12737 /*                                           renka@cs.unt.edu */
12738 /*                                                   07/17/96 */
12739 
12740 /*   This subroutine adds an interior node to a triangulation */
12741 /* of a set of points on the unit sphere.  The data structure */
12742 /* is updated with the insertion of node KK into the triangle */
12743 /* whose vertices are I1, I2, and I3.  No optimization of the */
12744 /* triangulation is performed. */
12745 
12746 /*   This routine is identical to the similarly named routine */
12747 /* in TRIPACK. */
12748 
12749 
12750 /* On input: */
12751 
12752 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12753 /*            and KK must not be equal to I1, I2, or I3. */
12754 
12755 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12756 /*                  sequence of vertices of a triangle which */
12757 /*                  contains node KK. */
12758 
12759 /* The above parameters are not altered by this routine. */
12760 
12761 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12762 /*                             triangulation.  Refer to Sub- */
12763 /*                             routine TRMESH.  Triangle */
12764 /*                             (I1,I2,I3) must be included */
12765 /*                             in the triangulation. */
12766 
12767 /* On output: */
12768 
12769 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12770 /*                             the addition of node KK.  KK */
12771 /*                             will be connected to nodes I1, */
12772 /*                             I2, and I3. */
12773 
12774 /* Modules required by INTADD:  INSERT, LSTPTR */
12775 
12776 /* *********************************************************** */
12777 
12778 
12779 /* Local parameters: */
12780 
12781 /* K =        Local copy of KK */
12782 /* LP =       LIST pointer */
12783 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12784 
12785     /* Parameter adjustments */
12786     --lend;
12787     --lptr;
12788     --list;
12789 
12790     /* Function Body */
12791     k = *kk;
12792 
12793 /* Initialization. */
12794 
12795     n1 = *i1;
12796     n2 = *i2;
12797     n3 = *i3;
12798 
12799 /* Add K as a neighbor of I1, I2, and I3. */
12800 
12801     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12802     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12803     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12804     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12805     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12806     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12807 
12808 /* Add I1, I2, and I3 as neighbors of K. */
12809 
12810     list[*lnew] = n1;
12811     list[*lnew + 1] = n2;
12812     list[*lnew + 2] = n3;
12813     lptr[*lnew] = *lnew + 1;
12814     lptr[*lnew + 1] = *lnew + 2;
12815     lptr[*lnew + 2] = *lnew;
12816     lend[k] = *lnew + 2;
12817     *lnew += 3;
12818     return 0;
12819 } /* intadd_ */
12820 
12821 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12822         double *p, int *ier)
12823 {
12824     /* Builtin functions */
12825     //double sqrt(double);
12826 
12827     /* Local variables */
12828     static int i__;
12829     static double t, d1, d2, pp[3], ppn;
12830 
12831 
12832 /* *********************************************************** */
12833 
12834 /*                                              From STRIPACK */
12835 /*                                            Robert J. Renka */
12836 /*                                  Dept. of Computer Science */
12837 /*                                       Univ. of North Texas */
12838 /*                                           renka@cs.unt.edu */
12839 /*                                                   07/19/90 */
12840 
12841 /*   Given a great circle C and points P1 and P2 defining an */
12842 /* arc A on the surface of the unit sphere, where A is the */
12843 /* shorter of the two portions of the great circle C12 assoc- */
12844 /* iated with P1 and P2, this subroutine returns the point */
12845 /* of intersection P between C and C12 that is closer to A. */
12846 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12847 /* C, P is the point of intersection of C with A. */
12848 
12849 
12850 /* On input: */
12851 
12852 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
12853 /*               coordinates of unit vectors. */
12854 
12855 /*       CN = Array of length 3 containing the Cartesian */
12856 /*            coordinates of a nonzero vector which defines C */
12857 /*            as the intersection of the plane whose normal */
12858 /*            is CN with the unit sphere.  Thus, if C is to */
12859 /*            be the great circle defined by P and Q, CN */
12860 /*            should be P X Q. */
12861 
12862 /* The above parameters are not altered by this routine. */
12863 
12864 /*       P = Array of length 3. */
12865 
12866 /* On output: */
12867 
12868 /*       P = Point of intersection defined above unless IER */
12869 /*           .NE. 0, in which case P is not altered. */
12870 
12871 /*       IER = Error indicator. */
12872 /*             IER = 0 if no errors were encountered. */
12873 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
12874 /*                     iff P1 = P2 or CN = 0 or there are */
12875 /*                     two intersection points at the same */
12876 /*                     distance from A. */
12877 /*             IER = 2 if P2 = -P1 and the definition of A is */
12878 /*                     therefore ambiguous. */
12879 
12880 /* Modules required by INTRSC:  None */
12881 
12882 /* Intrinsic function called by INTRSC:  SQRT */
12883 
12884 /* *********************************************************** */
12885 
12886 
12887 /* Local parameters: */
12888 
12889 /* D1 =  <CN,P1> */
12890 /* D2 =  <CN,P2> */
12891 /* I =   DO-loop index */
12892 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
12893 /*         line defined by P1 and P2 */
12894 /* PPN = Norm of PP */
12895 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
12896 /*         in the plane of C */
12897 
12898     /* Parameter adjustments */
12899     --p;
12900     --cn;
12901     --p2;
12902     --p1;
12903 
12904     /* Function Body */
12905     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12906     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12907 
12908     if (d1 == d2) {
12909         *ier = 1;
12910         return 0;
12911     }
12912 
12913 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
12914 
12915     t = d1 / (d1 - d2);
12916     ppn = 0.;
12917     for (i__ = 1; i__ <= 3; ++i__) {
12918         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12919         ppn += pp[i__ - 1] * pp[i__ - 1];
12920 /* L1: */
12921     }
12922 
12923 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
12924 
12925     if (ppn == 0.) {
12926         *ier = 2;
12927         return 0;
12928     }
12929     ppn = sqrt(ppn);
12930 
12931 /* Compute P = PP/PPN. */
12932 
12933     for (i__ = 1; i__ <= 3; ++i__) {
12934         p[i__] = pp[i__ - 1] / ppn;
12935 /* L2: */
12936     }
12937     *ier = 0;
12938     return 0;
12939 } /* intrsc_ */
12940 
12941 int jrand_(int *n, int *ix, int *iy, int *iz)
12942 {
12943     /* System generated locals */
12944     int ret_val;
12945 
12946     /* Local variables */
12947     static float u, x;
12948 
12949 
12950 /* *********************************************************** */
12951 
12952 /*                                              From STRIPACK */
12953 /*                                            Robert J. Renka */
12954 /*                                  Dept. of Computer Science */
12955 /*                                       Univ. of North Texas */
12956 /*                                           renka@cs.unt.edu */
12957 /*                                                   07/28/98 */
12958 
12959 /*   This function returns a uniformly distributed pseudo- */
12960 /* random int in the range 1 to N. */
12961 
12962 
12963 /* On input: */
12964 
12965 /*       N = Maximum value to be returned. */
12966 
12967 /* N is not altered by this function. */
12968 
12969 /*       IX,IY,IZ = int seeds initialized to values in */
12970 /*                  the range 1 to 30,000 before the first */
12971 /*                  call to JRAND, and not altered between */
12972 /*                  subsequent calls (unless a sequence of */
12973 /*                  random numbers is to be repeated by */
12974 /*                  reinitializing the seeds). */
12975 
12976 /* On output: */
12977 
12978 /*       IX,IY,IZ = Updated int seeds. */
12979 
12980 /*       JRAND = Random int in the range 1 to N. */
12981 
12982 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
12983 /*             and Portable Pseudo-random Number Generator", */
12984 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
12985 /*             pp. 188-190. */
12986 
12987 /* Modules required by JRAND:  None */
12988 
12989 /* Intrinsic functions called by JRAND:  INT, MOD, float */
12990 
12991 /* *********************************************************** */
12992 
12993 
12994 /* Local parameters: */
12995 
12996 /* U = Pseudo-random number uniformly distributed in the */
12997 /*     interval (0,1). */
12998 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
12999 /*       tional part is U. */
13000 
13001     *ix = *ix * 171 % 30269;
13002     *iy = *iy * 172 % 30307;
13003     *iz = *iz * 170 % 30323;
13004     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13005             30323.f;
13006     u = x - (int) x;
13007     ret_val = (int) ((float) (*n) * u + 1.f);
13008     return ret_val;
13009 } /* jrand_ */
13010 
13011 long int left_(double *x1, double *y1, double *z1, double *x2,
13012         double *y2, double *z2, double *x0, double *y0,
13013         double *z0)
13014 {
13015     /* System generated locals */
13016     long int ret_val;
13017 
13018 
13019 /* *********************************************************** */
13020 
13021 /*                                              From STRIPACK */
13022 /*                                            Robert J. Renka */
13023 /*                                  Dept. of Computer Science */
13024 /*                                       Univ. of North Texas */
13025 /*                                           renka@cs.unt.edu */
13026 /*                                                   07/15/96 */
13027 
13028 /*   This function determines whether node N0 is in the */
13029 /* (closed) left hemisphere defined by the plane containing */
13030 /* N1, N2, and the origin, where left is defined relative to */
13031 /* an observer at N1 facing N2. */
13032 
13033 
13034 /* On input: */
13035 
13036 /*       X1,Y1,Z1 = Coordinates of N1. */
13037 
13038 /*       X2,Y2,Z2 = Coordinates of N2. */
13039 
13040 /*       X0,Y0,Z0 = Coordinates of N0. */
13041 
13042 /* Input parameters are not altered by this function. */
13043 
13044 /* On output: */
13045 
13046 /*       LEFT = TRUE if and only if N0 is in the closed */
13047 /*              left hemisphere. */
13048 
13049 /* Modules required by LEFT:  None */
13050 
13051 /* *********************************************************** */
13052 
13053 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
13054 
13055     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13056             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13057 
13058 
13059     return ret_val;
13060 } /* left_ */
13061 
13062 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13063 {
13064     /* System generated locals */
13065     int ret_val;
13066 
13067     /* Local variables */
13068     static int nd, lp;
13069 
13070 
13071 /* *********************************************************** */
13072 
13073 /*                                              From STRIPACK */
13074 /*                                            Robert J. Renka */
13075 /*                                  Dept. of Computer Science */
13076 /*                                       Univ. of North Texas */
13077 /*                                           renka@cs.unt.edu */
13078 /*                                                   07/15/96 */
13079 
13080 /*   This function returns the index (LIST pointer) of NB in */
13081 /* the adjacency list for N0, where LPL = LEND(N0). */
13082 
13083 /*   This function is identical to the similarly named */
13084 /* function in TRIPACK. */
13085 
13086 
13087 /* On input: */
13088 
13089 /*       LPL = LEND(N0) */
13090 
13091 /*       NB = Index of the node whose pointer is to be re- */
13092 /*            turned.  NB must be connected to N0. */
13093 
13094 /*       LIST,LPTR = Data structure defining the triangula- */
13095 /*                   tion.  Refer to Subroutine TRMESH. */
13096 
13097 /* Input parameters are not altered by this function. */
13098 
13099 /* On output: */
13100 
13101 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13102 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13103 /*                neighbor of N0, in which case LSTPTR = LPL. */
13104 
13105 /* Modules required by LSTPTR:  None */
13106 
13107 /* *********************************************************** */
13108 
13109 
13110 /* Local parameters: */
13111 
13112 /* LP = LIST pointer */
13113 /* ND = Nodal index */
13114 
13115     /* Parameter adjustments */
13116     --lptr;
13117     --list;
13118 
13119     /* Function Body */
13120     lp = lptr[*lpl];
13121 L1:
13122     nd = list[lp];
13123     if (nd == *nb) {
13124         goto L2;
13125     }
13126     lp = lptr[lp];
13127     if (lp != *lpl) {
13128         goto L1;
13129     }
13130 
13131 L2:
13132     ret_val = lp;
13133     return ret_val;
13134 } /* lstptr_ */
13135 
13136 int nbcnt_(int *lpl, int *lptr)
13137 {
13138     /* System generated locals */
13139     int ret_val;
13140 
13141     /* Local variables */
13142     static int k, lp;
13143 
13144 
13145 /* *********************************************************** */
13146 
13147 /*                                              From STRIPACK */
13148 /*                                            Robert J. Renka */
13149 /*                                  Dept. of Computer Science */
13150 /*                                       Univ. of North Texas */
13151 /*                                           renka@cs.unt.edu */
13152 /*                                                   07/15/96 */
13153 
13154 /*   This function returns the number of neighbors of a node */
13155 /* N0 in a triangulation created by Subroutine TRMESH. */
13156 
13157 /*   This function is identical to the similarly named */
13158 /* function in TRIPACK. */
13159 
13160 
13161 /* On input: */
13162 
13163 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13164 /*             LPL = LEND(N0). */
13165 
13166 /*       LPTR = Array of pointers associated with LIST. */
13167 
13168 /* Input parameters are not altered by this function. */
13169 
13170 /* On output: */
13171 
13172 /*       NBCNT = Number of neighbors of N0. */
13173 
13174 /* Modules required by NBCNT:  None */
13175 
13176 /* *********************************************************** */
13177 
13178 
13179 /* Local parameters: */
13180 
13181 /* K =  Counter for computing the number of neighbors */
13182 /* LP = LIST pointer */
13183 
13184     /* Parameter adjustments */
13185     --lptr;
13186 
13187     /* Function Body */
13188     lp = *lpl;
13189     k = 1;
13190 
13191 L1:
13192     lp = lptr[lp];
13193     if (lp == *lpl) {
13194         goto L2;
13195     }
13196     ++k;
13197     goto L1;
13198 
13199 L2:
13200     ret_val = k;
13201     return ret_val;
13202 } /* nbcnt_ */
13203 
13204 int nearnd_(double *p, int *ist, int *n, double *x,
13205         double *y, double *z__, int *list, int *lptr, int
13206         *lend, double *al)
13207 {
13208     /* System generated locals */
13209     int ret_val, i__1;
13210 
13211     /* Builtin functions */
13212     //double acos(double);
13213 
13214     /* Local variables */
13215     static int l;
13216     static double b1, b2, b3;
13217     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13218     static double ds1;
13219     static int lp1, lp2;
13220     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13221     static int lpl;
13222     static double dsr;
13223     static int nst, listp[25], lptrp[25];
13224     /* Subroutine */ int trfind_(int *, double *, int *,
13225             double *, double *, double *, int *, int *,
13226             int *, double *, double *, double *, int *,
13227             int *, int *);
13228     int lstptr_(int *, int *, int *, int *);
13229 
13230 
13231 /* *********************************************************** */
13232 
13233 /*                                              From STRIPACK */
13234 /*                                            Robert J. Renka */
13235 /*                                  Dept. of Computer Science */
13236 /*                                       Univ. of North Texas */
13237 /*                                           renka@cs.unt.edu */
13238 /*                                                   07/28/98 */
13239 
13240 /*   Given a point P on the surface of the unit sphere and a */
13241 /* Delaunay triangulation created by Subroutine TRMESH, this */
13242 /* function returns the index of the nearest triangulation */
13243 /* node to P. */
13244 
13245 /*   The algorithm consists of implicitly adding P to the */
13246 /* triangulation, finding the nearest neighbor to P, and */
13247 /* implicitly deleting P from the triangulation.  Thus, it */
13248 /* is based on the fact that, if P is a node in a Delaunay */
13249 /* triangulation, the nearest node to P is a neighbor of P. */
13250 
13251 
13252 /* On input: */
13253 
13254 /*       P = Array of length 3 containing the Cartesian coor- */
13255 /*           dinates of the point P to be located relative to */
13256 /*           the triangulation.  It is assumed without a test */
13257 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13258 
13259 /*       IST = Index of a node at which TRFIND begins the */
13260 /*             search.  Search time depends on the proximity */
13261 /*             of this node to P. */
13262 
13263 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13264 
13265 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13266 /*               coordinates of the nodes. */
13267 
13268 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13269 /*                        gulation.  Refer to TRMESH. */
13270 
13271 /* Input parameters are not altered by this function. */
13272 
13273 /* On output: */
13274 
13275 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13276 /*                if N < 3 or the triangulation data struc- */
13277 /*                ture is invalid. */
13278 
13279 /*       AL = Arc length (angular distance in radians) be- */
13280 /*            tween P and NEARND unless NEARND = 0. */
13281 
13282 /*       Note that the number of candidates for NEARND */
13283 /*       (neighbors of P) is limited to LMAX defined in */
13284 /*       the PARAMETER statement below. */
13285 
13286 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13287 
13288 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13289 
13290 /* *********************************************************** */
13291 
13292 
13293 /* Local parameters: */
13294 
13295 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13296 /*               by TRFIND */
13297 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13298 /* DSR =       (Negative cosine of the) distance from P to NR */
13299 /* DX1,..DZ3 = Components of vectors used by the swap test */
13300 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13301 /*               the rightmost (I1) and leftmost (I2) visible */
13302 /*               boundary nodes as viewed from P */
13303 /* L =         Length of LISTP/LPTRP and number of neighbors */
13304 /*               of P */
13305 /* LMAX =      Maximum value of L */
13306 /* LISTP =     Indexes of the neighbors of P */
13307 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13308 /*               LISTP elements */
13309 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13310 /*               pointer */
13311 /* LP1,LP2 =   LISTP indexes (pointers) */
13312 /* LPL =       Pointer to the last neighbor of N1 */
13313 /* N1 =        Index of a node visible from P */
13314 /* N2 =        Index of an endpoint of an arc opposite P */
13315 /* N3 =        Index of the node opposite N1->N2 */
13316 /* NN =        Local copy of N */
13317 /* NR =        Index of a candidate for the nearest node to P */
13318 /* NST =       Index of the node at which TRFIND begins the */
13319 /*               search */
13320 
13321 
13322 /* Store local parameters and test for N invalid. */
13323 
13324     /* Parameter adjustments */
13325     --p;
13326     --lend;
13327     --z__;
13328     --y;
13329     --x;
13330     --list;
13331     --lptr;
13332 
13333     /* Function Body */
13334     nn = *n;
13335     if (nn < 3) {
13336         goto L6;
13337     }
13338     nst = *ist;
13339     if (nst < 1 || nst > nn) {
13340         nst = 1;
13341     }
13342 
13343 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13344 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13345 /*   from P. */
13346 
13347     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13348             1], &b1, &b2, &b3, &i1, &i2, &i3);
13349 
13350 /* Test for collinear nodes. */
13351 
13352     if (i1 == 0) {
13353         goto L6;
13354     }
13355 
13356 /* Store the linked list of 'neighbors' of P in LISTP and */
13357 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13358 /*   the last neighbor if P is not contained in a triangle. */
13359 /*   L is the length of LISTP and LPTRP, and is limited to */
13360 /*   LMAX. */
13361 
13362     if (i3 != 0) {
13363         listp[0] = i1;
13364         lptrp[0] = 2;
13365         listp[1] = i2;
13366         lptrp[1] = 3;
13367         listp[2] = i3;
13368         lptrp[2] = 1;
13369         l = 3;
13370     } else {
13371         n1 = i1;
13372         l = 1;
13373         lp1 = 2;
13374         listp[l - 1] = n1;
13375         lptrp[l - 1] = lp1;
13376 
13377 /*   Loop on the ordered sequence of visible boundary nodes */
13378 /*     N1 from I1 to I2. */
13379 
13380 L1:
13381         lpl = lend[n1];
13382         n1 = -list[lpl];
13383         l = lp1;
13384         lp1 = l + 1;
13385         listp[l - 1] = n1;
13386         lptrp[l - 1] = lp1;
13387         if (n1 != i2 && lp1 < 25) {
13388             goto L1;
13389         }
13390         l = lp1;
13391         listp[l - 1] = 0;
13392         lptrp[l - 1] = 1;
13393     }
13394 
13395 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13396 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13397 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13398 /*   indexes of N1 and N2. */
13399 
13400     lp2 = 1;
13401     n2 = i1;
13402     lp1 = lptrp[0];
13403     n1 = listp[lp1 - 1];
13404 
13405 /* Begin loop:  find the node N3 opposite N1->N2. */
13406 
13407 L2:
13408     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13409     if (list[lp] < 0) {
13410         goto L3;
13411     }
13412     lp = lptr[lp];
13413     n3 = (i__1 = list[lp], abs(i__1));
13414 
13415 /* Swap test:  Exit the loop if L = LMAX. */
13416 
13417     if (l == 25) {
13418         goto L4;
13419     }
13420     dx1 = x[n1] - p[1];
13421     dy1 = y[n1] - p[2];
13422     dz1 = z__[n1] - p[3];
13423 
13424     dx2 = x[n2] - p[1];
13425     dy2 = y[n2] - p[2];
13426     dz2 = z__[n2] - p[3];
13427 
13428     dx3 = x[n3] - p[1];
13429     dy3 = y[n3] - p[2];
13430     dz3 = z__[n3] - p[3];
13431     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13432             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13433         goto L3;
13434     }
13435 
13436 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13437 /*        The two new arcs opposite P must be tested. */
13438 
13439     ++l;
13440     lptrp[lp2 - 1] = l;
13441     listp[l - 1] = n3;
13442     lptrp[l - 1] = lp1;
13443     lp1 = l;
13444     n1 = n3;
13445     goto L2;
13446 
13447 /* No swap:  Advance to the next arc and test for termination */
13448 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13449 
13450 L3:
13451     if (lp1 == 1) {
13452         goto L4;
13453     }
13454     lp2 = lp1;
13455     n2 = n1;
13456     lp1 = lptrp[lp1 - 1];
13457     n1 = listp[lp1 - 1];
13458     if (n1 == 0) {
13459         goto L4;
13460     }
13461     goto L2;
13462 
13463 /* Set NR and DSR to the index of the nearest node to P and */
13464 /*   an increasing function (negative cosine) of its distance */
13465 /*   from P, respectively. */
13466 
13467 L4:
13468     nr = i1;
13469     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13470     i__1 = l;
13471     for (lp = 2; lp <= i__1; ++lp) {
13472         n1 = listp[lp - 1];
13473         if (n1 == 0) {
13474             goto L5;
13475         }
13476         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13477         if (ds1 < dsr) {
13478             nr = n1;
13479             dsr = ds1;
13480         }
13481 L5:
13482         ;
13483     }
13484     dsr = -dsr;
13485     if (dsr > 1.) {
13486         dsr = 1.;
13487     }
13488     *al = acos(dsr);
13489     ret_val = nr;
13490     return ret_val;
13491 
13492 /* Invalid input. */
13493 
13494 L6:
13495     ret_val = 0;
13496     return ret_val;
13497 } /* nearnd_ */
13498 
13499 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13500         int *na, int *list, int *lptr, int *lend, int *
13501         nit, int *iwk, int *ier)
13502 {
13503     /* System generated locals */
13504     int i__1, i__2;
13505 
13506     /* Local variables */
13507     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13508     static long int swp;
13509     static int iter;
13510     /* Subroutine */ int swap_(int *, int *, int *,
13511             int *, int *, int *, int *, int *);
13512     static int maxit;
13513     long int swptst_(int *, int *, int *, int *,
13514             double *, double *, double *);
13515 
13516 
13517 /* *********************************************************** */
13518 
13519 /*                                              From STRIPACK */
13520 /*                                            Robert J. Renka */
13521 /*                                  Dept. of Computer Science */
13522 /*                                       Univ. of North Texas */
13523 /*                                           renka@cs.unt.edu */
13524 /*                                                   07/30/98 */
13525 
13526 /*   Given a set of NA triangulation arcs, this subroutine */
13527 /* optimizes the portion of the triangulation consisting of */
13528 /* the quadrilaterals (pairs of adjacent triangles) which */
13529 /* have the arcs as diagonals by applying the circumcircle */
13530 /* test and appropriate swaps to the arcs. */
13531 
13532 /*   An iteration consists of applying the swap test and */
13533 /* swaps to all NA arcs in the order in which they are */
13534 /* stored.  The iteration is repeated until no swap occurs */
13535 /* or NIT iterations have been performed.  The bound on the */
13536 /* number of iterations may be necessary to prevent an */
13537 /* infinite loop caused by cycling (reversing the effect of a */
13538 /* previous swap) due to floating point inaccuracy when four */
13539 /* or more nodes are nearly cocircular. */
13540 
13541 
13542 /* On input: */
13543 
13544 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13545 
13546 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13547 
13548 /* The above parameters are not altered by this routine. */
13549 
13550 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13551 /*                        gulation.  Refer to Subroutine */
13552 /*                        TRMESH. */
13553 
13554 /*       NIT = Maximum number of iterations to be performed. */
13555 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13556 
13557 /*       IWK = int array dimensioned 2 by NA containing */
13558 /*             the nodal indexes of the arc endpoints (pairs */
13559 /*             of endpoints are stored in columns). */
13560 
13561 /* On output: */
13562 
13563 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13564 /*                        ture reflecting the swaps. */
13565 
13566 /*       NIT = Number of iterations performed. */
13567 
13568 /*       IWK = Endpoint indexes of the new set of arcs */
13569 /*             reflecting the swaps. */
13570 
13571 /*       IER = Error indicator: */
13572 /*             IER = 0 if no errors were encountered. */
13573 /*             IER = 1 if a swap occurred on the last of */
13574 /*                     MAXIT iterations, where MAXIT is the */
13575 /*                     value of NIT on input.  The new set */
13576 /*                     of arcs is not necessarily optimal */
13577 /*                     in this case. */
13578 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13579 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13580 /*                     IWK(1,I) for some I in the range 1 */
13581 /*                     to NA.  A swap may have occurred in */
13582 /*                     this case. */
13583 /*             IER = 4 if a zero pointer was returned by */
13584 /*                     Subroutine SWAP. */
13585 
13586 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13587 
13588 /* Intrinsic function called by OPTIM:  ABS */
13589 
13590 /* *********************************************************** */
13591 
13592 
13593 /* Local parameters: */
13594 
13595 /* I =       Column index for IWK */
13596 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13597 /* ITER =    Iteration count */
13598 /* LP =      LIST pointer */
13599 /* LP21 =    Parameter returned by SWAP (not used) */
13600 /* LPL =     Pointer to the last neighbor of IO1 */
13601 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13602 /*             of IO1 */
13603 /* MAXIT =   Input value of NIT */
13604 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13605 /*             respectively */
13606 /* NNA =     Local copy of NA */
13607 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13608 /*             optimization loop */
13609 
13610     /* Parameter adjustments */
13611     --x;
13612     --y;
13613     --z__;
13614     iwk -= 3;
13615     --list;
13616     --lptr;
13617     --lend;
13618 
13619     /* Function Body */
13620     nna = *na;
13621     maxit = *nit;
13622     if (nna < 0 || maxit < 1) {
13623         goto L7;
13624     }
13625 
13626 /* Initialize iteration count ITER and test for NA = 0. */
13627 
13628     iter = 0;
13629     if (nna == 0) {
13630         goto L5;
13631     }
13632 
13633 /* Top of loop -- */
13634 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13635 
13636 L1:
13637     if (iter == maxit) {
13638         goto L6;
13639     }
13640     ++iter;
13641     swp = FALSE_;
13642 
13643 /*   Inner loop on arcs IO1-IO2 -- */
13644 
13645     i__1 = nna;
13646     for (i__ = 1; i__ <= i__1; ++i__) {
13647         io1 = iwk[(i__ << 1) + 1];
13648         io2 = iwk[(i__ << 1) + 2];
13649 
13650 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13651 /*     IO2->IO1, respectively.  Determine the following: */
13652 
13653 /*     LPL = pointer to the last neighbor of IO1, */
13654 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13655 /*     LPP = pointer to the node N2 preceding IO2. */
13656 
13657         lpl = lend[io1];
13658         lpp = lpl;
13659         lp = lptr[lpp];
13660 L2:
13661         if (list[lp] == io2) {
13662             goto L3;
13663         }
13664         lpp = lp;
13665         lp = lptr[lpp];
13666         if (lp != lpl) {
13667             goto L2;
13668         }
13669 
13670 /*   IO2 should be the last neighbor of IO1.  Test for no */
13671 /*     arc and bypass the swap test if IO1 is a boundary */
13672 /*     node. */
13673 
13674         if ((i__2 = list[lp], abs(i__2)) != io2) {
13675             goto L8;
13676         }
13677         if (list[lp] < 0) {
13678             goto L4;
13679         }
13680 
13681 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13682 /*     boundary node and IO2 is its first neighbor. */
13683 
13684 L3:
13685         n2 = list[lpp];
13686         if (n2 < 0) {
13687             goto L4;
13688         }
13689         lp = lptr[lp];
13690         n1 = (i__2 = list[lp], abs(i__2));
13691 
13692 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13693 
13694         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13695             goto L4;
13696         }
13697         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13698         if (lp21 == 0) {
13699             goto L9;
13700         }
13701         swp = TRUE_;
13702         iwk[(i__ << 1) + 1] = n1;
13703         iwk[(i__ << 1) + 2] = n2;
13704 L4:
13705         ;
13706     }
13707     if (swp) {
13708         goto L1;
13709     }
13710 
13711 /* Successful termination. */
13712 
13713 L5:
13714     *nit = iter;
13715     *ier = 0;
13716     return 0;
13717 
13718 /* MAXIT iterations performed without convergence. */
13719 
13720 L6:
13721     *nit = maxit;
13722     *ier = 1;
13723     return 0;
13724 
13725 /* Invalid input parameter. */
13726 
13727 L7:
13728     *nit = 0;
13729     *ier = 2;
13730     return 0;
13731 
13732 /* IO2 is not a neighbor of IO1. */
13733 
13734 L8:
13735     *nit = iter;
13736     *ier = 3;
13737     return 0;
13738 
13739 /* Zero pointer returned by SWAP. */
13740 
13741 L9:
13742     *nit = iter;
13743     *ier = 4;
13744     return 0;
13745 } /* optim_ */
13746 
13747 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13748         double *ox, double *oy, double *oz, double *ex,
13749         double *ey, double *ez, double *vx, double *vy,
13750         double *vz, long int *init, double *x, double *y,
13751         double *z__, int *ier)
13752 {
13753     /* Builtin functions */
13754     //double sqrt(double);
13755 
13756     /* Local variables */
13757     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13758             oes, xoe, yoe, zoe, xep, yep, zep;
13759 
13760 
13761 /* *********************************************************** */
13762 
13763 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13764 /*                                            Robert J. Renka */
13765 /*                                  Dept. of Computer Science */
13766 /*                                       Univ. of North Texas */
13767 /*                                           renka@cs.unt.edu */
13768 /*                                                   07/18/90 */
13769 
13770 /*   Given a projection plane and associated coordinate sys- */
13771 /* tem defined by an origin O, eye position E, and up-vector */
13772 /* V, this subroutine applies a perspective depth transform- */
13773 /* ation T to a point P = (PX,PY,PZ), returning the point */
13774 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13775 /* coordinates of the point that lies in the projection */
13776 /* plane and on the line defined by P and E, and Z is the */
13777 /* depth associated with P. */
13778 
13779 /*   The projection plane is defined to be the plane that */
13780 /* contains O and has normal defined by O and E. */
13781 
13782 /*   The depth Z is defined in such a way that Z < 1, T maps */
13783 /* lines to lines (and planes to planes), and if two distinct */
13784 /* points have the same projection plane coordinates, then */
13785 /* the one closer to E has a smaller depth.  (Z increases */
13786 /* monotonically with orthogonal distance from P to the plane */
13787 /* that is parallel to the projection plane and contains E.) */
13788 /* This depth value facilitates depth sorting and depth buf- */
13789 /* fer methods. */
13790 
13791 
13792 /* On input: */
13793 
13794 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13795 /*                  be mapped onto the projection plane.  The */
13796 /*                  half line that contains P and has end- */
13797 /*                  point at E must intersect the plane. */
13798 
13799 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13800 /*                  nate system in the projection plane).  A */
13801 /*                  reasonable value for O is a point near */
13802 /*                  the center of an object or scene to be */
13803 /*                  viewed. */
13804 
13805 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13806 /*                  ing the normal to the plane and the line */
13807 /*                  of sight for the projection.  E must not */
13808 /*                  coincide with O or P, and the angle be- */
13809 /*                  tween the vectors O-E and P-E must be */
13810 /*                  less than 90 degrees.  Note that E and P */
13811 /*                  may lie on opposite sides of the projec- */
13812 /*                  tion plane. */
13813 
13814 /*       VX,VY,VZ = Coordinates of a point V which defines */
13815 /*                  the positive Y axis of an X-Y coordinate */
13816 /*                  system in the projection plane as the */
13817 /*                  half-line containing O and the projection */
13818 /*                  of O+V onto the plane.  The positive X */
13819 /*                  axis has direction defined by the cross */
13820 /*                  product V X (E-O). */
13821 
13822 /* The above parameters are not altered by this routine. */
13823 
13824 /*       INIT = long int switch which must be set to TRUE on */
13825 /*              the first call and when the values of O, E, */
13826 /*              or V have been altered since a previous call. */
13827 /*              If INIT = FALSE, it is assumed that only the */
13828 /*              coordinates of P have changed since a previ- */
13829 /*              ous call.  Previously stored quantities are */
13830 /*              used for increased efficiency in this case. */
13831 
13832 /* On output: */
13833 
13834 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13835 
13836 /*       X,Y = Projection plane coordinates of the point */
13837 /*             that lies in the projection plane and on the */
13838 /*             line defined by E and P.  X and Y are not */
13839 /*             altered if IER .NE. 0. */
13840 
13841 /*       Z = Depth value defined above unless IER .NE. 0. */
13842 
13843 /*       IER = Error indicator. */
13844 /*             IER = 0 if no errors were encountered. */
13845 /*             IER = 1 if the inner product of O-E with P-E */
13846 /*                     is not positive, implying that E is */
13847 /*                     too close to the plane. */
13848 /*             IER = 2 if O, E, and O+V are collinear.  See */
13849 /*                     the description of VX,VY,VZ. */
13850 
13851 /* Modules required by PROJCT:  None */
13852 
13853 /* Intrinsic function called by PROJCT:  SQRT */
13854 
13855 /* *********************************************************** */
13856 
13857 
13858 /* Local parameters: */
13859 
13860 /* OES =         Norm squared of OE -- inner product (OE,OE) */
13861 /* S =           Scale factor for computing projections */
13862 /* SC =          Scale factor for normalizing VN and HN */
13863 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
13864 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
13865 /* XH,YH,ZH =    Components of a unit vector HN defining the */
13866 /*                 positive X-axis in the plane */
13867 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
13868 /* XV,YV,ZV =    Components of a unit vector VN defining the */
13869 /*                 positive Y-axis in the plane */
13870 /* XW,YW,ZW =    Components of the vector W from O to the */
13871 /*                 projection of P onto the plane */
13872 
13873     if (*init) {
13874 
13875 /* Compute parameters defining the transformation: */
13876 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
13877 /*   2 square roots. */
13878 
13879 /* Set the coordinates of E to local variables, compute */
13880 /*   OE = E-O and OES, and test for OE = 0. */
13881 
13882         xe = *ex;
13883         ye = *ey;
13884         ze = *ez;
13885         xoe = xe - *ox;
13886         yoe = ye - *oy;
13887         zoe = ze - *oz;
13888         oes = xoe * xoe + yoe * yoe + zoe * zoe;
13889         if (oes == 0.) {
13890             goto L1;
13891         }
13892 
13893 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
13894 
13895         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13896         xv = *vx - s * xoe;
13897         yv = *vy - s * yoe;
13898         zv = *vz - s * zoe;
13899 
13900 /* Normalize VN to a unit vector. */
13901 
13902         sc = xv * xv + yv * yv + zv * zv;
13903         if (sc == 0.) {
13904             goto L2;
13905         }
13906         sc = 1. / sqrt(sc);
13907         xv = sc * xv;
13908         yv = sc * yv;
13909         zv = sc * zv;
13910 
13911 /* Compute HN = VN X OE (normalized). */
13912 
13913         xh = yv * zoe - yoe * zv;
13914         yh = xoe * zv - xv * zoe;
13915         zh = xv * yoe - xoe * yv;
13916         sc = sqrt(xh * xh + yh * yh + zh * zh);
13917         if (sc == 0.) {
13918             goto L2;
13919         }
13920         sc = 1. / sc;
13921         xh = sc * xh;
13922         yh = sc * yh;
13923         zh = sc * zh;
13924     }
13925 
13926 /* Apply the transformation:  13 adds, 12 multiplies, */
13927 /*                            1 divide, and 1 compare. */
13928 
13929 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
13930 
13931     xep = *px - xe;
13932     yep = *py - ye;
13933     zep = *pz - ze;
13934     s = xoe * xep + yoe * yep + zoe * zep;
13935     if (s >= 0.) {
13936         goto L1;
13937     }
13938     s = oes / s;
13939     xw = xoe - s * xep;
13940     yw = yoe - s * yep;
13941     zw = zoe - s * zep;
13942 
13943 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
13944 /*   reset INIT. */
13945 
13946     *x = xw * xh + yw * yh + zw * zh;
13947     *y = xw * xv + yw * yv + zw * zv;
13948     *z__ = s + 1.;
13949     *init = FALSE_;
13950     *ier = 0;
13951     return 0;
13952 
13953 /* (OE,EP) .GE. 0. */
13954 
13955 L1:
13956     *ier = 1;
13957     return 0;
13958 
13959 /* O, E, and O+V are collinear. */
13960 
13961 L2:
13962     *ier = 2;
13963     return 0;
13964 } /* projct_ */
13965 
13966 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
13967         double *plat, double *plon, double *pnrm)
13968 {
13969     /* Builtin functions */
13970     //double sqrt(double), atan2(double, double), asin(double);
13971 
13972 
13973 /* *********************************************************** */
13974 
13975 /*                                              From STRIPACK */
13976 /*                                            Robert J. Renka */
13977 /*                                  Dept. of Computer Science */
13978 /*                                       Univ. of North Texas */
13979 /*                                           renka@cs.unt.edu */
13980 /*                                                   08/27/90 */
13981 
13982 /*   This subroutine converts a point P from Cartesian coor- */
13983 /* dinates to spherical coordinates. */
13984 
13985 
13986 /* On input: */
13987 
13988 /*       PX,PY,PZ = Cartesian coordinates of P. */
13989 
13990 /* Input parameters are not altered by this routine. */
13991 
13992 /* On output: */
13993 
13994 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
13995 /*              0 if PNRM = 0.  PLAT should be scaled by */
13996 /*              180/PI to obtain the value in degrees. */
13997 
13998 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
13999 /*              if P lies on the Z-axis.  PLON should be */
14000 /*              scaled by 180/PI to obtain the value in */
14001 /*              degrees. */
14002 
14003 /*       PNRM = Magnitude (Euclidean norm) of P. */
14004 
14005 /* Modules required by SCOORD:  None */
14006 
14007 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
14008 
14009 /* *********************************************************** */
14010 
14011     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14012     if (*px != 0. || *py != 0.) {
14013         *plon = atan2(*py, *px);
14014     } else {
14015         *plon = 0.;
14016     }
14017     if (*pnrm != 0.) {
14018         *plat = asin(*pz / *pnrm);
14019     } else {
14020         *plat = 0.;
14021     }
14022     return 0;
14023 } /* scoord_ */
14024 
14025 double store_(double *x)
14026 {
14027     /* System generated locals */
14028     double ret_val;
14029 
14030 
14031 /* *********************************************************** */
14032 
14033 /*                                              From STRIPACK */
14034 /*                                            Robert J. Renka */
14035 /*                                  Dept. of Computer Science */
14036 /*                                       Univ. of North Texas */
14037 /*                                           renka@cs.unt.edu */
14038 /*                                                   05/09/92 */
14039 
14040 /*   This function forces its argument X to be stored in a */
14041 /* memory location, thus providing a means of determining */
14042 /* floating point number characteristics (such as the machine */
14043 /* precision) when it is necessary to avoid computation in */
14044 /* high precision registers. */
14045 
14046 
14047 /* On input: */
14048 
14049 /*       X = Value to be stored. */
14050 
14051 /* X is not altered by this function. */
14052 
14053 /* On output: */
14054 
14055 /*       STORE = Value of X after it has been stored and */
14056 /*               possibly truncated or rounded to the single */
14057 /*               precision word length. */
14058 
14059 /* Modules required by STORE:  None */
14060 
14061 /* *********************************************************** */
14062 
14063     stcom_1.y = *x;
14064     ret_val = stcom_1.y;
14065     return ret_val;
14066 } /* store_ */
14067 
14068 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
14069         io2, int *list, int *lptr, int *lend, int *lp21)
14070 {
14071     /* System generated locals */
14072     int i__1;
14073 
14074     /* Local variables */
14075     static int lp, lph, lpsav;
14076     int lstptr_(int *, int *, int *, int *);
14077 
14078 
14079 /* *********************************************************** */
14080 
14081 /*                                              From STRIPACK */
14082 /*                                            Robert J. Renka */
14083 /*                                  Dept. of Computer Science */
14084 /*                                       Univ. of North Texas */
14085 /*                                           renka@cs.unt.edu */
14086 /*                                                   06/22/98 */
14087 
14088 /*   Given a triangulation of a set of points on the unit */
14089 /* sphere, this subroutine replaces a diagonal arc in a */
14090 /* strictly convex quadrilateral (defined by a pair of adja- */
14091 /* cent triangles) with the other diagonal.  Equivalently, a */
14092 /* pair of adjacent triangles is replaced by another pair */
14093 /* having the same union. */
14094 
14095 
14096 /* On input: */
14097 
14098 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14099 /*                         the quadrilateral.  IO1-IO2 is re- */
14100 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14101 /*                         and (IO2,IO1,IN2) must be trian- */
14102 /*                         gles on input. */
14103 
14104 /* The above parameters are not altered by this routine. */
14105 
14106 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14107 /*                        gulation.  Refer to Subroutine */
14108 /*                        TRMESH. */
14109 
14110 /* On output: */
14111 
14112 /*       LIST,LPTR,LEND = Data structure updated with the */
14113 /*                        swap -- triangles (IO1,IO2,IN1) and */
14114 /*                        (IO2,IO1,IN2) are replaced by */
14115 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14116 /*                        unless LP21 = 0. */
14117 
14118 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14119 /*              swap is performed unless IN1 and IN2 are */
14120 /*              adjacent on input, in which case LP21 = 0. */
14121 
14122 /* Module required by SWAP:  LSTPTR */
14123 
14124 /* Intrinsic function called by SWAP:  ABS */
14125 
14126 /* *********************************************************** */
14127 
14128 
14129 /* Local parameters: */
14130 
14131 /* LP,LPH,LPSAV = LIST pointers */
14132 
14133 
14134 /* Test for IN1 and IN2 adjacent. */
14135 
14136     /* Parameter adjustments */
14137     --lend;
14138     --lptr;
14139     --list;
14140 
14141     /* Function Body */
14142     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14143     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14144         *lp21 = 0;
14145         return 0;
14146     }
14147 
14148 /* Delete IO2 as a neighbor of IO1. */
14149 
14150     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14151     lph = lptr[lp];
14152     lptr[lp] = lptr[lph];
14153 
14154 /* If IO2 is the last neighbor of IO1, make IN2 the */
14155 /*   last neighbor. */
14156 
14157     if (lend[*io1] == lph) {
14158         lend[*io1] = lp;
14159     }
14160 
14161 /* Insert IN2 as a neighbor of IN1 following IO1 */
14162 /*   using the hole created above. */
14163 
14164     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14165     lpsav = lptr[lp];
14166     lptr[lp] = lph;
14167     list[lph] = *in2;
14168     lptr[lph] = lpsav;
14169 
14170 /* Delete IO1 as a neighbor of IO2. */
14171 
14172     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14173     lph = lptr[lp];
14174     lptr[lp] = lptr[lph];
14175 
14176 /* If IO1 is the last neighbor of IO2, make IN1 the */
14177 /*   last neighbor. */
14178 
14179     if (lend[*io2] == lph) {
14180         lend[*io2] = lp;
14181     }
14182 
14183 /* Insert IN1 as a neighbor of IN2 following IO2. */
14184 
14185     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14186     lpsav = lptr[lp];
14187     lptr[lp] = lph;
14188     list[lph] = *in1;
14189     lptr[lph] = lpsav;
14190     *lp21 = lph;
14191     return 0;
14192 } /* swap_ */
14193 
14194 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14195         double *x, double *y, double *z__)
14196 {
14197     /* System generated locals */
14198     long int ret_val;
14199 
14200     /* Local variables */
14201     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14202 
14203 
14204 /* *********************************************************** */
14205 
14206 /*                                              From STRIPACK */
14207 /*                                            Robert J. Renka */
14208 /*                                  Dept. of Computer Science */
14209 /*                                       Univ. of North Texas */
14210 /*                                           renka@cs.unt.edu */
14211 /*                                                   03/29/91 */
14212 
14213 /*   This function decides whether or not to replace a */
14214 /* diagonal arc in a quadrilateral with the other diagonal. */
14215 /* The decision will be to swap (SWPTST = TRUE) if and only */
14216 /* if N4 lies above the plane (in the half-space not contain- */
14217 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14218 /* the projection of N4 onto this plane is interior to the */
14219 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14220 /* swap if the quadrilateral is not strictly convex. */
14221 
14222 
14223 /* On input: */
14224 
14225 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14226 /*                     quadrilateral with N1 adjacent to N2, */
14227 /*                     and (N1,N2,N3) in counterclockwise */
14228 /*                     order.  The arc connecting N1 to N2 */
14229 /*                     should be replaced by an arc connec- */
14230 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14231 /*                     to Subroutine SWAP. */
14232 
14233 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14234 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14235 /*               define node I for I = N1, N2, N3, and N4. */
14236 
14237 /* Input parameters are not altered by this routine. */
14238 
14239 /* On output: */
14240 
14241 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14242 /*                and N2 should be swapped for an arc con- */
14243 /*                necting N3 and N4. */
14244 
14245 /* Modules required by SWPTST:  None */
14246 
14247 /* *********************************************************** */
14248 
14249 
14250 /* Local parameters: */
14251 
14252 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14253 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14254 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14255 /* X4,Y4,Z4 =    Coordinates of N4 */
14256 
14257     /* Parameter adjustments */
14258     --z__;
14259     --y;
14260     --x;
14261 
14262     /* Function Body */
14263     x4 = x[*n4];
14264     y4 = y[*n4];
14265     z4 = z__[*n4];
14266     dx1 = x[*n1] - x4;
14267     dx2 = x[*n2] - x4;
14268     dx3 = x[*n3] - x4;
14269     dy1 = y[*n1] - y4;
14270     dy2 = y[*n2] - y4;
14271     dy3 = y[*n3] - y4;
14272     dz1 = z__[*n1] - z4;
14273     dz2 = z__[*n2] - z4;
14274     dz3 = z__[*n3] - z4;
14275 
14276 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14277 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14278 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14279 
14280     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14281             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14282     return ret_val;
14283 } /* swptst_ */
14284 
14285 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14286         double *x, double *y, double *z__)
14287 {
14288     /* System generated locals */
14289     int i__1;
14290 
14291     /* Builtin functions */
14292     //double cos(double), sin(double);
14293 
14294     /* Local variables */
14295     static int i__, nn;
14296     static double phi, theta, cosphi;
14297 
14298 
14299 /* *********************************************************** */
14300 
14301 /*                                              From STRIPACK */
14302 /*                                            Robert J. Renka */
14303 /*                                  Dept. of Computer Science */
14304 /*                                       Univ. of North Texas */
14305 /*                                           renka@cs.unt.edu */
14306 /*                                                   04/08/90 */
14307 
14308 /*   This subroutine transforms spherical coordinates into */
14309 /* Cartesian coordinates on the unit sphere for input to */
14310 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14311 /* storage for RLAT and RLON if the latter need not be saved. */
14312 
14313 
14314 /* On input: */
14315 
14316 /*       N = Number of nodes (points on the unit sphere) */
14317 /*           whose coordinates are to be transformed. */
14318 
14319 /*       RLAT = Array of length N containing latitudinal */
14320 /*              coordinates of the nodes in radians. */
14321 
14322 /*       RLON = Array of length N containing longitudinal */
14323 /*              coordinates of the nodes in radians. */
14324 
14325 /* The above parameters are not altered by this routine. */
14326 
14327 /*       X,Y,Z = Arrays of length at least N. */
14328 
14329 /* On output: */
14330 
14331 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14332 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14333 /*               to N. */
14334 
14335 /* Modules required by TRANS:  None */
14336 
14337 /* Intrinsic functions called by TRANS:  COS, SIN */
14338 
14339 /* *********************************************************** */
14340 
14341 
14342 /* Local parameters: */
14343 
14344 /* COSPHI = cos(PHI) */
14345 /* I =      DO-loop index */
14346 /* NN =     Local copy of N */
14347 /* PHI =    Latitude */
14348 /* THETA =  Longitude */
14349 
14350     /* Parameter adjustments */
14351     --z__;
14352     --y;
14353     --x;
14354     --rlon;
14355     --rlat;
14356 
14357     /* Function Body */
14358     nn = *n;
14359     i__1 = nn;
14360     for (i__ = 1; i__ <= i__1; ++i__) {
14361         phi = rlat[i__];
14362         theta = rlon[i__];
14363         cosphi = cos(phi);
14364         x[i__] = cosphi * cos(theta);
14365         y[i__] = cosphi * sin(theta);
14366         z__[i__] = sin(phi);
14367 /* L1: */
14368     }
14369     return 0;
14370 } /* trans_ */
14371 
14372 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14373         double *x, double *y, double *z__, int *list, int
14374         *lptr, int *lend, double *b1, double *b2, double *b3,
14375         int *i1, int *i2, int *i3)
14376 {
14377     /* Initialized data */
14378 
14379     static int ix = 1;
14380     static int iy = 2;
14381     static int iz = 3;
14382 
14383     /* System generated locals */
14384     int i__1;
14385     double d__1, d__2;
14386 
14387     /* Local variables */
14388     static double q[3];
14389     static int n0, n1, n2, n3, n4, nf;
14390     static double s12;
14391     static int nl, lp;
14392     static double xp, yp, zp;
14393     static int n1s, n2s;
14394     static double eps, tol, ptn1, ptn2;
14395     static int next;
14396     int jrand_(int *, int *, int *, int *);
14397     double store_(double *);
14398     int lstptr_(int *, int *, int *, int *);
14399 
14400 
14401 /* *********************************************************** */
14402 
14403 /*                                              From STRIPACK */
14404 /*                                            Robert J. Renka */
14405 /*                                  Dept. of Computer Science */
14406 /*                                       Univ. of North Texas */
14407 /*                                           renka@cs.unt.edu */
14408 /*                                                   11/30/99 */
14409 
14410 /*   This subroutine locates a point P relative to a triangu- */
14411 /* lation created by Subroutine TRMESH.  If P is contained in */
14412 /* a triangle, the three vertex indexes and barycentric coor- */
14413 /* dinates are returned.  Otherwise, the indexes of the */
14414 /* visible boundary nodes are returned. */
14415 
14416 
14417 /* On input: */
14418 
14419 /*       NST = Index of a node at which TRFIND begins its */
14420 /*             search.  Search time depends on the proximity */
14421 /*             of this node to P. */
14422 
14423 /*       P = Array of length 3 containing the x, y, and z */
14424 /*           coordinates (in that order) of the point P to be */
14425 /*           located. */
14426 
14427 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14428 
14429 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14430 /*               coordinates of the triangulation nodes (unit */
14431 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14432 /*               for I = 1 to N. */
14433 
14434 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14435 /*                        gulation.  Refer to Subroutine */
14436 /*                        TRMESH. */
14437 
14438 /* Input parameters are not altered by this routine. */
14439 
14440 /* On output: */
14441 
14442 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14443 /*                  the central projection of P onto the un- */
14444 /*                  derlying planar triangle if P is in the */
14445 /*                  convex hull of the nodes.  These parame- */
14446 /*                  ters are not altered if I1 = 0. */
14447 
14448 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14449 /*                  of a triangle containing P if P is con- */
14450 /*                  tained in a triangle.  If P is not in the */
14451 /*                  convex hull of the nodes, I1 and I2 are */
14452 /*                  the rightmost and leftmost (boundary) */
14453 /*                  nodes that are visible from P, and */
14454 /*                  I3 = 0.  (If all boundary nodes are vis- */
14455 /*                  ible from P, then I1 and I2 coincide.) */
14456 /*                  I1 = I2 = I3 = 0 if P and all of the */
14457 /*                  nodes are coplanar (lie on a common great */
14458 /*                  circle. */
14459 
14460 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14461 
14462 /* Intrinsic function called by TRFIND:  ABS */
14463 
14464 /* *********************************************************** */
14465 
14466 
14467     /* Parameter adjustments */
14468     --p;
14469     --lend;
14470     --z__;
14471     --y;
14472     --x;
14473     --list;
14474     --lptr;
14475 
14476     /* Function Body */
14477 
14478 /* Local parameters: */
14479 
14480 /* EPS =      Machine precision */
14481 /* IX,IY,IZ = int seeds for JRAND */
14482 /* LP =       LIST pointer */
14483 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14484 /*              cone (with vertex N0) containing P, or end- */
14485 /*              points of a boundary edge such that P Right */
14486 /*              N1->N2 */
14487 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14488 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14489 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14490 /* NF,NL =    First and last neighbors of N0, or first */
14491 /*              (rightmost) and last (leftmost) nodes */
14492 /*              visible from P when P is exterior to the */
14493 /*              triangulation */
14494 /* PTN1 =     Scalar product <P,N1> */
14495 /* PTN2 =     Scalar product <P,N2> */
14496 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14497 /*              the boundary traversal when P is exterior */
14498 /* S12 =      Scalar product <N1,N2> */
14499 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14500 /*              bound on the magnitude of a negative bary- */
14501 /*              centric coordinate (B1 or B2) for P in a */
14502 /*              triangle -- used to avoid an infinite number */
14503 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14504 /*              B2 < 0 but small in magnitude */
14505 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14506 /* X0,Y0,Z0 = Dummy arguments for DET */
14507 /* X1,Y1,Z1 = Dummy arguments for DET */
14508 /* X2,Y2,Z2 = Dummy arguments for DET */
14509 
14510 /* Statement function: */
14511 
14512 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14513 /*                       (closed) left hemisphere defined by */
14514 /*                       the plane containing (0,0,0), */
14515 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14516 /*                       left is defined relative to an ob- */
14517 /*                       server at (X1,Y1,Z1) facing */
14518 /*                       (X2,Y2,Z2). */
14519 
14520 
14521 /* Initialize variables. */
14522 
14523     xp = p[1];
14524     yp = p[2];
14525     zp = p[3];
14526     n0 = *nst;
14527     if (n0 < 1 || n0 > *n) {
14528         n0 = jrand_(n, &ix, &iy, &iz);
14529     }
14530 
14531 /* Compute the relative machine precision EPS and TOL. */
14532 
14533     eps = 1.;
14534 L1:
14535     eps /= 2.;
14536     d__1 = eps + 1.;
14537     if (store_(&d__1) > 1.) {
14538         goto L1;
14539     }
14540     eps *= 2.;
14541     tol = eps * 4.;
14542 
14543 /* Set NF and NL to the first and last neighbors of N0, and */
14544 /*   initialize N1 = NF. */
14545 
14546 L2:
14547     lp = lend[n0];
14548     nl = list[lp];
14549     lp = lptr[lp];
14550     nf = list[lp];
14551     n1 = nf;
14552 
14553 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14554 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14555 
14556     if (nl > 0) {
14557 
14558 /*   N0 is an interior node.  Find N1. */
14559 
14560 L3:
14561         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14562                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14563                 -1e-10) {
14564             lp = lptr[lp];
14565             n1 = list[lp];
14566             if (n1 == nl) {
14567                 goto L6;
14568             }
14569             goto L3;
14570         }
14571     } else {
14572 
14573 /*   N0 is a boundary node.  Test for P exterior. */
14574 
14575         nl = -nl;
14576         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14577                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14578                 -1e-10) {
14579 
14580 /*   P is to the right of the boundary edge N0->NF. */
14581 
14582             n1 = n0;
14583             n2 = nf;
14584             goto L9;
14585         }
14586         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14587                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14588                 -1e-10) {
14589 
14590 /*   P is to the right of the boundary edge NL->N0. */
14591 
14592             n1 = nl;
14593             n2 = n0;
14594             goto L9;
14595         }
14596     }
14597 
14598 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14599 /*   next neighbor of N0 (following N1). */
14600 
14601 L4:
14602     lp = lptr[lp];
14603     n2 = (i__1 = list[lp], abs(i__1));
14604     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14605             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14606         goto L7;
14607     }
14608     n1 = n2;
14609     if (n1 != nl) {
14610         goto L4;
14611     }
14612     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14613             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14614         goto L6;
14615     }
14616 
14617 /* P is left of or on arcs N0->NB for all neighbors NB */
14618 /*   of N0.  Test for P = +/-N0. */
14619 
14620     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14621     if (store_(&d__2) < 1. - eps * 4.) {
14622 
14623 /*   All points are collinear iff P Left NB->N0 for all */
14624 /*     neighbors NB of N0.  Search the neighbors of N0. */
14625 /*     Note:  N1 = NL and LP points to NL. */
14626 
14627 L5:
14628         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14629                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14630                 -1e-10) {
14631             lp = lptr[lp];
14632             n1 = (i__1 = list[lp], abs(i__1));
14633             if (n1 == nl) {
14634                 goto L14;
14635             }
14636             goto L5;
14637         }
14638     }
14639 
14640 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14641 /*   and start over. */
14642 
14643     n0 = n1;
14644     goto L2;
14645 
14646 /* P is between arcs N0->N1 and N0->NF. */
14647 
14648 L6:
14649     n2 = nf;
14650 
14651 /* P is contained in a wedge defined by geodesics N0-N1 and */
14652 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14653 /*   test for cycling. */
14654 
14655 L7:
14656     n3 = n0;
14657     n1s = n1;
14658     n2s = n2;
14659 
14660 /* Top of edge-hopping loop: */
14661 
14662 L8:
14663 
14664     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14665             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14666      if (*b3 < -1e-10) {
14667 
14668 /*   Set N4 to the first neighbor of N2 following N1 (the */
14669 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14670 
14671         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14672         if (list[lp] < 0) {
14673             goto L9;
14674         }
14675         lp = lptr[lp];
14676         n4 = (i__1 = list[lp], abs(i__1));
14677 
14678 /*   Define a new arc N1->N2 which intersects the geodesic */
14679 /*     N0-P. */
14680         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14681                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14682                 -1e-10) {
14683             n3 = n2;
14684             n2 = n4;
14685             n1s = n1;
14686             if (n2 != n2s && n2 != n0) {
14687                 goto L8;
14688             }
14689         } else {
14690             n3 = n1;
14691             n1 = n4;
14692             n2s = n2;
14693             if (n1 != n1s && n1 != n0) {
14694                 goto L8;
14695             }
14696         }
14697 
14698 /*   The starting node N0 or edge N1-N2 was encountered */
14699 /*     again, implying a cycle (infinite loop).  Restart */
14700 /*     with N0 randomly selected. */
14701 
14702         n0 = jrand_(n, &ix, &iy, &iz);
14703         goto L2;
14704     }
14705 
14706 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14707 /*   or P is close to -N0. */
14708 
14709     if (*b3 >= eps) {
14710 
14711 /*   B3 .NE. 0. */
14712 
14713         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14714                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14715         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14716                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14717         if (*b1 < -tol || *b2 < -tol) {
14718 
14719 /*   Restart with N0 randomly selected. */
14720 
14721             n0 = jrand_(n, &ix, &iy, &iz);
14722             goto L2;
14723         }
14724     } else {
14725 
14726 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14727 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14728 
14729         *b3 = 0.;
14730         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14731         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14732         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14733         *b1 = ptn1 - s12 * ptn2;
14734         *b2 = ptn2 - s12 * ptn1;
14735         if (*b1 < -tol || *b2 < -tol) {
14736 
14737 /*   Restart with N0 randomly selected. */
14738 
14739             n0 = jrand_(n, &ix, &iy, &iz);
14740             goto L2;
14741         }
14742     }
14743 
14744 /* P is in (N1,N2,N3). */
14745 
14746     *i1 = n1;
14747     *i2 = n2;
14748     *i3 = n3;
14749     if (*b1 < 0.f) {
14750         *b1 = 0.f;
14751     }
14752     if (*b2 < 0.f) {
14753         *b2 = 0.f;
14754     }
14755     return 0;
14756 
14757 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14758 /*   Save N1 and N2, and set NL = 0 to indicate that */
14759 /*   NL has not yet been found. */
14760 
14761 L9:
14762     n1s = n1;
14763     n2s = n2;
14764     nl = 0;
14765 
14766 /*           Counterclockwise Boundary Traversal: */
14767 
14768 L10:
14769 
14770     lp = lend[n2];
14771     lp = lptr[lp];
14772     next = list[lp];
14773      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14774              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14775             >= -1e-10) {
14776 
14777 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14778 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14779 
14780         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14781         q[0] = x[n1] - s12 * x[n2];
14782         q[1] = y[n1] - s12 * y[n2];
14783         q[2] = z__[n1] - s12 * z__[n2];
14784         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14785             goto L11;
14786         }
14787         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14788             goto L11;
14789         }
14790 
14791 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14792 /*     the leftmost visible node. */
14793 
14794         nl = n2;
14795     }
14796 
14797 /* Bottom of counterclockwise loop: */
14798 
14799     n1 = n2;
14800     n2 = next;
14801     if (n2 != n1s) {
14802         goto L10;
14803     }
14804 
14805 /* All boundary nodes are visible from P. */
14806 
14807     *i1 = n1s;
14808     *i2 = n1s;
14809     *i3 = 0;
14810     return 0;
14811 
14812 /* N2 is the rightmost visible node. */
14813 
14814 L11:
14815     nf = n2;
14816     if (nl == 0) {
14817 
14818 /* Restore initial values of N1 and N2, and begin the search */
14819 /*   for the leftmost visible node. */
14820 
14821         n2 = n2s;
14822         n1 = n1s;
14823 
14824 /*           Clockwise Boundary Traversal: */
14825 
14826 L12:
14827         lp = lend[n1];
14828         next = -list[lp];
14829         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14830                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14831                  y[next]) >= -1e-10) {
14832 
14833 /*   N1 is the leftmost visible node if P or NEXT is */
14834 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14835 
14836             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14837             q[0] = x[n2] - s12 * x[n1];
14838             q[1] = y[n2] - s12 * y[n1];
14839             q[2] = z__[n2] - s12 * z__[n1];
14840             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14841                 goto L13;
14842             }
14843             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14844                 goto L13;
14845             }
14846 
14847 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14848 /*     rightmost visible node. */
14849 
14850             nf = n1;
14851         }
14852 
14853 /* Bottom of clockwise loop: */
14854 
14855         n2 = n1;
14856         n1 = next;
14857         if (n1 != n1s) {
14858             goto L12;
14859         }
14860 
14861 /* All boundary nodes are visible from P. */
14862 
14863         *i1 = n1;
14864         *i2 = n1;
14865         *i3 = 0;
14866         return 0;
14867 
14868 /* N1 is the leftmost visible node. */
14869 
14870 L13:
14871         nl = n1;
14872     }
14873 
14874 /* NF and NL have been found. */
14875 
14876     *i1 = nf;
14877     *i2 = nl;
14878     *i3 = 0;
14879     return 0;
14880 
14881 /* All points are collinear (coplanar). */
14882 
14883 L14:
14884     *i1 = 0;
14885     *i2 = 0;
14886     *i3 = 0;
14887     return 0;
14888 } /* trfind_ */
14889 
14890 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
14891         int *lend, int *nrow, int *nt, int *ltri, int *
14892         ier)
14893 {
14894     /* System generated locals */
14895     int ltri_dim1, ltri_offset, i__1, i__2;
14896 
14897     /* Local variables */
14898     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14899             lpl, isv;
14900     static long int arcs;
14901     static int lpln1;
14902 
14903 
14904 /* *********************************************************** */
14905 
14906 /*                                              From STRIPACK */
14907 /*                                            Robert J. Renka */
14908 /*                                  Dept. of Computer Science */
14909 /*                                       Univ. of North Texas */
14910 /*                                           renka@cs.unt.edu */
14911 /*                                                   07/20/96 */
14912 
14913 /*   This subroutine converts a triangulation data structure */
14914 /* from the linked list created by Subroutine TRMESH to a */
14915 /* triangle list. */
14916 
14917 /* On input: */
14918 
14919 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14920 
14921 /*       LIST,LPTR,LEND = Linked list data structure defin- */
14922 /*                        ing the triangulation.  Refer to */
14923 /*                        Subroutine TRMESH. */
14924 
14925 /*       NROW = Number of rows (entries per triangle) re- */
14926 /*              served for the triangle list LTRI.  The value */
14927 /*              must be 6 if only the vertex indexes and */
14928 /*              neighboring triangle indexes are to be */
14929 /*              stored, or 9 if arc indexes are also to be */
14930 /*              assigned and stored.  Refer to LTRI. */
14931 
14932 /* The above parameters are not altered by this routine. */
14933 
14934 /*       LTRI = int array of length at least NROW*NT, */
14935 /*              where NT is at most 2N-4.  (A sufficient */
14936 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
14937 
14938 /* On output: */
14939 
14940 /*       NT = Number of triangles in the triangulation unless */
14941 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
14942 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
14943 /*            number of boundary nodes. */
14944 
14945 /*       LTRI = NROW by NT array whose J-th column contains */
14946 /*              the vertex nodal indexes (first three rows), */
14947 /*              neighboring triangle indexes (second three */
14948 /*              rows), and, if NROW = 9, arc indexes (last */
14949 /*              three rows) associated with triangle J for */
14950 /*              J = 1,...,NT.  The vertices are ordered */
14951 /*              counterclockwise with the first vertex taken */
14952 /*              to be the one with smallest index.  Thus, */
14953 /*              LTRI(2,J) and LTRI(3,J) are larger than */
14954 /*              LTRI(1,J) and index adjacent neighbors of */
14955 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
14956 /*              and LTRI(I+6,J) index the triangle and arc, */
14957 /*              respectively, which are opposite (not shared */
14958 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
14959 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
14960 /*              indexes range from 1 to N, triangle indexes */
14961 /*              from 0 to NT, and, if included, arc indexes */
14962 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
14963 /*              or 3N-6 if NB = 0.  The triangles are or- */
14964 /*              dered on first (smallest) vertex indexes. */
14965 
14966 /*       IER = Error indicator. */
14967 /*             IER = 0 if no errors were encountered. */
14968 /*             IER = 1 if N or NROW is outside its valid */
14969 /*                     range on input. */
14970 /*             IER = 2 if the triangulation data structure */
14971 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
14972 /*                     however, that these arrays are not */
14973 /*                     completely tested for validity. */
14974 
14975 /* Modules required by TRLIST:  None */
14976 
14977 /* Intrinsic function called by TRLIST:  ABS */
14978 
14979 /* *********************************************************** */
14980 
14981 
14982 /* Local parameters: */
14983 
14984 /* ARCS =     long int variable with value TRUE iff are */
14985 /*              indexes are to be stored */
14986 /* I,J =      LTRI row indexes (1 to 3) associated with */
14987 /*              triangles KT and KN, respectively */
14988 /* I1,I2,I3 = Nodal indexes of triangle KN */
14989 /* ISV =      Variable used to permute indexes I1,I2,I3 */
14990 /* KA =       Arc index and number of currently stored arcs */
14991 /* KN =       Index of the triangle that shares arc I1-I2 */
14992 /*              with KT */
14993 /* KT =       Triangle index and number of currently stored */
14994 /*              triangles */
14995 /* LP =       LIST pointer */
14996 /* LP2 =      Pointer to N2 as a neighbor of N1 */
14997 /* LPL =      Pointer to the last neighbor of I1 */
14998 /* LPLN1 =    Pointer to the last neighbor of N1 */
14999 /* N1,N2,N3 = Nodal indexes of triangle KT */
15000 /* NM2 =      N-2 */
15001 
15002 
15003 /* Test for invalid input parameters. */
15004 
15005     /* Parameter adjustments */
15006     --lend;
15007     --list;
15008     --lptr;
15009     ltri_dim1 = *nrow;
15010     ltri_offset = 1 + ltri_dim1;
15011     ltri -= ltri_offset;
15012 
15013     /* Function Body */
15014     if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15015         goto L11;
15016     }
15017 
15018 /* Initialize parameters for loop on triangles KT = (N1,N2, */
15019 /*   N3), where N1 < N2 and N1 < N3. */
15020 
15021 /*   ARCS = TRUE iff arc indexes are to be stored. */
15022 /*   KA,KT = Numbers of currently stored arcs and triangles. */
15023 /*   NM2 = Upper bound on candidates for N1. */
15024 
15025     arcs = *nrow == 9;
15026     ka = 0;
15027     kt = 0;
15028     nm2 = *n - 2;
15029 
15030 /* Loop on nodes N1. */
15031 
15032     i__1 = nm2;
15033     for (n1 = 1; n1 <= i__1; ++n1) {
15034 
15035 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
15036 /*   to the last neighbor of N1, and LP2 points to N2. */
15037 
15038         lpln1 = lend[n1];
15039         lp2 = lpln1;
15040 L1:
15041         lp2 = lptr[lp2];
15042         n2 = list[lp2];
15043         lp = lptr[lp2];
15044         n3 = (i__2 = list[lp], abs(i__2));
15045         if (n2 < n1 || n3 < n1) {
15046             goto L8;
15047         }
15048 
15049 /* Add a new triangle KT = (N1,N2,N3). */
15050 
15051         ++kt;
15052         ltri[kt * ltri_dim1 + 1] = n1;
15053         ltri[kt * ltri_dim1 + 2] = n2;
15054         ltri[kt * ltri_dim1 + 3] = n3;
15055 
15056 /* Loop on triangle sides (I2,I1) with neighboring triangles */
15057 /*   KN = (I1,I2,I3). */
15058 
15059         for (i__ = 1; i__ <= 3; ++i__) {
15060             if (i__ == 1) {
15061                 i1 = n3;
15062                 i2 = n2;
15063             } else if (i__ == 2) {
15064                 i1 = n1;
15065                 i2 = n3;
15066             } else {
15067                 i1 = n2;
15068                 i2 = n1;
15069             }
15070 
15071 /* Set I3 to the neighbor of I1 that follows I2 unless */
15072 /*   I2->I1 is a boundary arc. */
15073 
15074             lpl = lend[i1];
15075             lp = lptr[lpl];
15076 L2:
15077             if (list[lp] == i2) {
15078                 goto L3;
15079             }
15080             lp = lptr[lp];
15081             if (lp != lpl) {
15082                 goto L2;
15083             }
15084 
15085 /*   I2 is the last neighbor of I1 unless the data structure */
15086 /*     is invalid.  Bypass the search for a neighboring */
15087 /*     triangle if I2->I1 is a boundary arc. */
15088 
15089             if ((i__2 = list[lp], abs(i__2)) != i2) {
15090                 goto L12;
15091             }
15092             kn = 0;
15093             if (list[lp] < 0) {
15094                 goto L6;
15095             }
15096 
15097 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15098 /*     a neighbor of I1. */
15099 
15100 L3:
15101             lp = lptr[lp];
15102             i3 = (i__2 = list[lp], abs(i__2));
15103 
15104 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15105 /*   and permute the vertex indexes of KN so that I1 is */
15106 /*   smallest. */
15107 
15108             if (i1 < i2 && i1 < i3) {
15109                 j = 3;
15110             } else if (i2 < i3) {
15111                 j = 2;
15112                 isv = i1;
15113                 i1 = i2;
15114                 i2 = i3;
15115                 i3 = isv;
15116             } else {
15117                 j = 1;
15118                 isv = i1;
15119                 i1 = i3;
15120                 i3 = i2;
15121                 i2 = isv;
15122             }
15123 
15124 /* Test for KN > KT (triangle index not yet assigned). */
15125 
15126             if (i1 > n1) {
15127                 goto L7;
15128             }
15129 
15130 /* Find KN, if it exists, by searching the triangle list in */
15131 /*   reverse order. */
15132 
15133             for (kn = kt - 1; kn >= 1; --kn) {
15134                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15135                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15136                     goto L5;
15137                 }
15138 /* L4: */
15139             }
15140             goto L7;
15141 
15142 /* Store KT as a neighbor of KN. */
15143 
15144 L5:
15145             ltri[j + 3 + kn * ltri_dim1] = kt;
15146 
15147 /* Store KN as a neighbor of KT, and add a new arc KA. */
15148 
15149 L6:
15150             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15151             if (arcs) {
15152                 ++ka;
15153                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15154                 if (kn != 0) {
15155                     ltri[j + 6 + kn * ltri_dim1] = ka;
15156                 }
15157             }
15158 L7:
15159             ;
15160         }
15161 
15162 /* Bottom of loop on triangles. */
15163 
15164 L8:
15165         if (lp2 != lpln1) {
15166             goto L1;
15167         }
15168 /* L9: */
15169     }
15170 
15171 /* No errors encountered. */
15172 
15173     *nt = kt;
15174     *ier = 0;
15175     return 0;
15176 
15177 /* Invalid input parameter. */
15178 
15179 L11:
15180     *nt = 0;
15181     *ier = 1;
15182     return 0;
15183 
15184 /* Invalid triangulation data structure:  I1 is a neighbor of */
15185 /*   I2, but I2 is not a neighbor of I1. */
15186 
15187 L12:
15188     *nt = 0;
15189     *ier = 2;
15190     return 0;
15191 } /* trlist_ */
15192 
15193 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15194         double *z__, int *iflag, int *nrow, int *nt, int *
15195         ltri, int *lout)
15196 {
15197     /* Initialized data */
15198 
15199     static int nmax = 9999;
15200     static int nlmax = 58;
15201 
15202     /* System generated locals */
15203     int ltri_dim1, ltri_offset, i__1;
15204 
15205     /* Local variables */
15206     static int i__, k, na, nb, nl, lun;
15207 
15208 
15209 /* *********************************************************** */
15210 
15211 /*                                              From STRIPACK */
15212 /*                                            Robert J. Renka */
15213 /*                                  Dept. of Computer Science */
15214 /*                                       Univ. of North Texas */
15215 /*                                           renka@cs.unt.edu */
15216 /*                                                   07/02/98 */
15217 
15218 /*   This subroutine prints the triangle list created by Sub- */
15219 /* routine TRLIST and, optionally, the nodal coordinates */
15220 /* (either latitude and longitude or Cartesian coordinates) */
15221 /* on long int unit LOUT.  The numbers of boundary nodes, */
15222 /* triangles, and arcs are also printed. */
15223 
15224 
15225 /* On input: */
15226 
15227 /*       N = Number of nodes in the triangulation. */
15228 /*           3 .LE. N .LE. 9999. */
15229 
15230 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15231 /*               coordinates of the nodes if IFLAG = 0, or */
15232 /*               (X and Y only) arrays of length N containing */
15233 /*               longitude and latitude, respectively, if */
15234 /*               IFLAG > 0, or unused dummy parameters if */
15235 /*               IFLAG < 0. */
15236 
15237 /*       IFLAG = Nodal coordinate option indicator: */
15238 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15239 /*                         Cartesian coordinates) are to be */
15240 /*                         printed (to 6 decimal places). */
15241 /*               IFLAG > 0 if only X and Y (assumed to con- */
15242 /*                         tain longitude and latitude) are */
15243 /*                         to be printed (to 6 decimal */
15244 /*                         places). */
15245 /*               IFLAG < 0 if only the adjacency lists are to */
15246 /*                         be printed. */
15247 
15248 /*       NROW = Number of rows (entries per triangle) re- */
15249 /*              served for the triangle list LTRI.  The value */
15250 /*              must be 6 if only the vertex indexes and */
15251 /*              neighboring triangle indexes are stored, or 9 */
15252 /*              if arc indexes are also stored. */
15253 
15254 /*       NT = Number of triangles in the triangulation. */
15255 /*            1 .LE. NT .LE. 9999. */
15256 
15257 /*       LTRI = NROW by NT array whose J-th column contains */
15258 /*              the vertex nodal indexes (first three rows), */
15259 /*              neighboring triangle indexes (second three */
15260 /*              rows), and, if NROW = 9, arc indexes (last */
15261 /*              three rows) associated with triangle J for */
15262 /*              J = 1,...,NT. */
15263 
15264 /*       LOUT = long int unit number for output.  If LOUT is */
15265 /*              not in the range 0 to 99, output is written */
15266 /*              to unit 6. */
15267 
15268 /* Input parameters are not altered by this routine. */
15269 
15270 /* On output: */
15271 
15272 /*   The triangle list and nodal coordinates (as specified by */
15273 /* IFLAG) are written to unit LOUT. */
15274 
15275 /* Modules required by TRLPRT:  None */
15276 
15277 /* *********************************************************** */
15278 
15279     /* Parameter adjustments */
15280     --z__;
15281     --y;
15282     --x;
15283     ltri_dim1 = *nrow;
15284     ltri_offset = 1 + ltri_dim1;
15285     ltri -= ltri_offset;
15286 
15287     /* Function Body */
15288 
15289 /* Local parameters: */
15290 
15291 /* I =     DO-loop, nodal index, and row index for LTRI */
15292 /* K =     DO-loop and triangle index */
15293 /* LUN =   long int unit number for output */
15294 /* NA =    Number of triangulation arcs */
15295 /* NB =    Number of boundary nodes */
15296 /* NL =    Number of lines printed on the current page */
15297 /* NLMAX = Maximum number of print lines per page (except */
15298 /*           for the last page which may have two addi- */
15299 /*           tional lines) */
15300 /* NMAX =  Maximum value of N and NT (4-digit format) */
15301 
15302     lun = *lout;
15303     if (lun < 0 || lun > 99) {
15304         lun = 6;
15305     }
15306 
15307 /* Print a heading and test for invalid input. */
15308 
15309 /*      WRITE (LUN,100) N */
15310     nl = 3;
15311     if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15312             nmax) {
15313 
15314 /* Print an error message and exit. */
15315 
15316 /*        WRITE (LUN,110) N, NROW, NT */
15317         return 0;
15318     }
15319     if (*iflag == 0) {
15320 
15321 /* Print X, Y, and Z. */
15322 
15323 /*        WRITE (LUN,101) */
15324         nl = 6;
15325         i__1 = *n;
15326         for (i__ = 1; i__ <= i__1; ++i__) {
15327             if (nl >= nlmax) {
15328 /*            WRITE (LUN,108) */
15329                 nl = 0;
15330             }
15331 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15332             ++nl;
15333 /* L1: */
15334         }
15335     } else if (*iflag > 0) {
15336 
15337 /* Print X (longitude) and Y (latitude). */
15338 
15339 /*        WRITE (LUN,102) */
15340         nl = 6;
15341         i__1 = *n;
15342         for (i__ = 1; i__ <= i__1; ++i__) {
15343             if (nl >= nlmax) {
15344 /*            WRITE (LUN,108) */
15345                 nl = 0;
15346             }
15347 /*          WRITE (LUN,104) I, X(I), Y(I) */
15348             ++nl;
15349 /* L2: */
15350         }
15351     }
15352 
15353 /* Print the triangulation LTRI. */
15354 
15355     if (nl > nlmax / 2) {
15356 /*        WRITE (LUN,108) */
15357         nl = 0;
15358     }
15359     if (*nrow == 6) {
15360 /*        WRITE (LUN,105) */
15361     } else {
15362 /*        WRITE (LUN,106) */
15363     }
15364     nl += 5;
15365     i__1 = *nt;
15366     for (k = 1; k <= i__1; ++k) {
15367         if (nl >= nlmax) {
15368 /*          WRITE (LUN,108) */
15369             nl = 0;
15370         }
15371 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15372         ++nl;
15373 /* L3: */
15374     }
15375 
15376 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15377 /*   triangles). */
15378 
15379     nb = (*n << 1) - *nt - 2;
15380     if (nb < 3) {
15381         nb = 0;
15382         na = *n * 3 - 6;
15383     } else {
15384         na = *nt + *n - 1;
15385     }
15386 /*      WRITE (LUN,109) NB, NA, NT */
15387     return 0;
15388 
15389 /* Print formats: */
15390 
15391 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15392 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15393 /*     .        'Z(Node)'//) */
15394 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15395 /*  103 FORMAT (8X,I4,3D17.6) */
15396 /*  104 FORMAT (16X,I4,2D17.6) */
15397 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15398 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15399 /*     .        'KT2',4X,'KT3'/) */
15400 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15401 /*     .        14X,'Arcs'/ */
15402 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15403 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15404 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15405 /*  108 FORMAT (///) */
15406 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15407 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15408 /*     .        ' Triangles') */
15409 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15410 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15411 } /* trlprt_ */
15412 
15413 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15414         double *z__, int *list, int *lptr, int *lend, int
15415         *lnew, int *near__, int *next, double *dist, int *ier)
15416 {
15417     /* System generated locals */
15418     int i__1, i__2;
15419 
15420     /* Local variables */
15421     static double d__;
15422     static int i__, j, k;
15423     static double d1, d2, d3;
15424     static int i0, lp, nn, lpl;
15425     long int left_(double *, double *, double *, double
15426             *, double *, double *, double *, double *,
15427             double *);
15428     static int nexti;
15429 
15430 
15431 /* *********************************************************** */
15432 
15433 /*                                              From STRIPACK */
15434 /*                                            Robert J. Renka */
15435 /*                                  Dept. of Computer Science */
15436 /*                                       Univ. of North Texas */
15437 /*                                           renka@cs.unt.edu */
15438 /*                                                   03/04/03 */
15439 
15440 /*   This subroutine creates a Delaunay triangulation of a */
15441 /* set of N arbitrarily distributed points, referred to as */
15442 /* nodes, on the surface of the unit sphere.  The Delaunay */
15443 /* triangulation is defined as a set of (spherical) triangles */
15444 /* with the following five properties: */
15445 
15446 /*  1)  The triangle vertices are nodes. */
15447 /*  2)  No triangle contains a node other than its vertices. */
15448 /*  3)  The interiors of the triangles are pairwise disjoint. */
15449 /*  4)  The union of triangles is the convex hull of the set */
15450 /*        of nodes (the smallest convex set that contains */
15451 /*        the nodes).  If the nodes are not contained in a */
15452 /*        single hemisphere, their convex hull is the en- */
15453 /*        tire sphere and there are no boundary nodes. */
15454 /*        Otherwise, there are at least three boundary nodes. */
15455 /*  5)  The interior of the circumcircle of each triangle */
15456 /*        contains no node. */
15457 
15458 /* The first four properties define a triangulation, and the */
15459 /* last property results in a triangulation which is as close */
15460 /* as possible to equiangular in a certain sense and which is */
15461 /* uniquely defined unless four or more nodes lie in a common */
15462 /* plane.  This property makes the triangulation well-suited */
15463 /* for solving closest-point problems and for triangle-based */
15464 /* interpolation. */
15465 
15466 /*   The algorithm has expected time complexity O(N*log(N)) */
15467 /* for most nodal distributions. */
15468 
15469 /*   Spherical coordinates (latitude and longitude) may be */
15470 /* converted to Cartesian coordinates by Subroutine TRANS. */
15471 
15472 /*   The following is a list of the software package modules */
15473 /* which a user may wish to call directly: */
15474 
15475 /*  ADDNOD - Updates the triangulation by appending a new */
15476 /*             node. */
15477 
15478 /*  AREAS  - Returns the area of a spherical triangle. */
15479 
15480 /*  AREAV  - Returns the area of a Voronoi region associated */
15481 /*           with an interior node without requiring that the */
15482 /*           entire Voronoi diagram be computed and stored. */
15483 
15484 /*  BNODES - Returns an array containing the indexes of the */
15485 /*             boundary nodes (if any) in counterclockwise */
15486 /*             order.  Counts of boundary nodes, triangles, */
15487 /*             and arcs are also returned. */
15488 
15489 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15490 /*           formly spaced points on the unit circle centered */
15491 /*           at (0,0). */
15492 
15493 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15494 /*             gle. */
15495 
15496 /*  CRLIST - Returns the set of triangle circumcenters */
15497 /*             (Voronoi vertices) and circumradii associated */
15498 /*             with a triangulation. */
15499 
15500 /*  DELARC - Deletes a boundary arc from a triangulation. */
15501 
15502 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15503 
15504 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15505 /*             ted by an arc in the triangulation. */
15506 
15507 /*  GETNP  - Determines the ordered sequence of L closest */
15508 /*             nodes to a given node, along with the associ- */
15509 /*             ated distances. */
15510 
15511 /*  INSIDE - Locates a point relative to a polygon on the */
15512 /*             surface of the sphere. */
15513 
15514 /*  INTRSC - Returns the point of intersection between a */
15515 /*             pair of great circle arcs. */
15516 
15517 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15518 /*             int. */
15519 
15520 /*  LEFT   - Locates a point relative to a great circle. */
15521 
15522 /*  NEARND - Returns the index of the nearest node to an */
15523 /*             arbitrary point, along with its squared */
15524 /*             distance. */
15525 
15526 /*  PROJCT - Applies a perspective-depth projection to a */
15527 /*             point in 3-space. */
15528 
15529 /*  SCOORD - Converts a point from Cartesian coordinates to */
15530 /*             spherical coordinates. */
15531 
15532 /*  STORE  - Forces a value to be stored in main memory so */
15533 /*             that the precision of floating point numbers */
15534 /*             in memory locations rather than registers is */
15535 /*             computed. */
15536 
15537 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15538 /*             coordinates on the unit sphere for input to */
15539 /*             Subroutine TRMESH. */
15540 
15541 /*  TRLIST - Converts the triangulation data structure to a */
15542 /*             triangle list more suitable for use in a fin- */
15543 /*             ite element code. */
15544 
15545 /*  TRLPRT - Prints the triangle list created by Subroutine */
15546 /*             TRLIST. */
15547 
15548 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15549 /*             nodes. */
15550 
15551 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15552 /*             file containing a triangulation plot. */
15553 
15554 /*  TRPRNT - Prints the triangulation data structure and, */
15555 /*             optionally, the nodal coordinates. */
15556 
15557 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15558 /*             file containing a Voronoi diagram plot. */
15559 
15560 
15561 /* On input: */
15562 
15563 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15564 
15565 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15566 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15567 /*               Z(K)) is referred to as node K, and K is re- */
15568 /*               ferred to as a nodal index.  It is required */
15569 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15570 /*               K.  The first three nodes must not be col- */
15571 /*               linear (lie on a common great circle). */
15572 
15573 /* The above parameters are not altered by this routine. */
15574 
15575 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15576 
15577 /*       LEND = Array of length at least N. */
15578 
15579 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15580 /*                        least N.  The space is used to */
15581 /*                        efficiently determine the nearest */
15582 /*                        triangulation node to each un- */
15583 /*                        processed node for use by ADDNOD. */
15584 
15585 /* On output: */
15586 
15587 /*       LIST = Set of nodal indexes which, along with LPTR, */
15588 /*              LEND, and LNEW, define the triangulation as a */
15589 /*              set of N adjacency lists -- counterclockwise- */
15590 /*              ordered sequences of neighboring nodes such */
15591 /*              that the first and last neighbors of a bound- */
15592 /*              ary node are boundary nodes (the first neigh- */
15593 /*              bor of an interior node is arbitrary).  In */
15594 /*              order to distinguish between interior and */
15595 /*              boundary nodes, the last neighbor of each */
15596 /*              boundary node is represented by the negative */
15597 /*              of its index. */
15598 
15599 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15600 /*              correspondence with the elements of LIST. */
15601 /*              LIST(LPTR(I)) indexes the node which follows */
15602 /*              LIST(I) in cyclical counterclockwise order */
15603 /*              (the first neighbor follows the last neigh- */
15604 /*              bor). */
15605 
15606 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15607 /*              points to the last neighbor of node K for */
15608 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15609 /*              only if K is a boundary node. */
15610 
15611 /*       LNEW = Pointer to the first empty location in LIST */
15612 /*              and LPTR (list length plus one).  LIST, LPTR, */
15613 /*              LEND, and LNEW are not altered if IER < 0, */
15614 /*              and are incomplete if IER > 0. */
15615 
15616 /*       NEAR,NEXT,DIST = Garbage. */
15617 
15618 /*       IER = Error indicator: */
15619 /*             IER =  0 if no errors were encountered. */
15620 /*             IER = -1 if N < 3 on input. */
15621 /*             IER = -2 if the first three nodes are */
15622 /*                      collinear. */
15623 /*             IER =  L if nodes L and M coincide for some */
15624 /*                      M > L.  The data structure represents */
15625 /*                      a triangulation of nodes 1 to M-1 in */
15626 /*                      this case. */
15627 
15628 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15629 /*                                INSERT, INTADD, JRAND, */
15630 /*                                LEFT, LSTPTR, STORE, SWAP, */
15631 /*                                SWPTST, TRFIND */
15632 
15633 /* Intrinsic function called by TRMESH:  ABS */
15634 
15635 /* *********************************************************** */
15636 
15637 
15638 /* Local parameters: */
15639 
15640 /* D =        (Negative cosine of) distance from node K to */
15641 /*              node I */
15642 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15643 /*              respectively */
15644 /* I,J =      Nodal indexes */
15645 /* I0 =       Index of the node preceding I in a sequence of */
15646 /*              unprocessed nodes:  I = NEXT(I0) */
15647 /* K =        Index of node to be added and DO-loop index: */
15648 /*              K > 3 */
15649 /* LP =       LIST index (pointer) of a neighbor of K */
15650 /* LPL =      Pointer to the last neighbor of K */
15651 /* NEXTI =    NEXT(I) */
15652 /* NN =       Local copy of N */
15653 
15654     /* Parameter adjustments */
15655     --dist;
15656     --next;
15657     --near__;
15658     --lend;
15659     --z__;
15660     --y;
15661     --x;
15662     --list;
15663     --lptr;
15664 
15665     /* Function Body */
15666     nn = *n;
15667     if (nn < 3) {
15668         *ier = -1;
15669         return 0;
15670     }
15671 
15672 /* Store the first triangle in the linked list. */
15673 
15674     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15675             z__[3])) {
15676 
15677 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15678 
15679         list[1] = 3;
15680         lptr[1] = 2;
15681         list[2] = -2;
15682         lptr[2] = 1;
15683         lend[1] = 2;
15684 
15685         list[3] = 1;
15686         lptr[3] = 4;
15687         list[4] = -3;
15688         lptr[4] = 3;
15689         lend[2] = 4;
15690 
15691         list[5] = 2;
15692         lptr[5] = 6;
15693         list[6] = -1;
15694         lptr[6] = 5;
15695         lend[3] = 6;
15696 
15697     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15698             y[3], &z__[3])) {
15699 
15700 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15701 /*     i.e., node 3 lies in the left hemisphere defined by */
15702 /*     arc 1->2. */
15703 
15704         list[1] = 2;
15705         lptr[1] = 2;
15706         list[2] = -3;
15707         lptr[2] = 1;
15708         lend[1] = 2;
15709 
15710         list[3] = 3;
15711         lptr[3] = 4;
15712         list[4] = -1;
15713         lptr[4] = 3;
15714         lend[2] = 4;
15715 
15716         list[5] = 1;
15717         lptr[5] = 6;
15718         list[6] = -2;
15719         lptr[6] = 5;
15720         lend[3] = 6;
15721 
15722     } else {
15723 
15724 /*   The first three nodes are collinear. */
15725 
15726         *ier = -2;
15727         return 0;
15728     }
15729 
15730 /* Initialize LNEW and test for N = 3. */
15731 
15732     *lnew = 7;
15733     if (nn == 3) {
15734         *ier = 0;
15735         return 0;
15736     }
15737 
15738 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15739 /*   used to obtain an expected-time (N*log(N)) incremental */
15740 /*   algorithm by enabling constant search time for locating */
15741 /*   each new node in the triangulation. */
15742 
15743 /* For each unprocessed node K, NEAR(K) is the index of the */
15744 /*   triangulation node closest to K (used as the starting */
15745 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15746 /*   is an increasing function of the arc length (angular */
15747 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15748 /*   length a. */
15749 
15750 /* Since it is necessary to efficiently find the subset of */
15751 /*   unprocessed nodes associated with each triangulation */
15752 /*   node J (those that have J as their NEAR entries), the */
15753 /*   subsets are stored in NEAR and NEXT as follows:  for */
15754 /*   each node J in the triangulation, I = NEAR(J) is the */
15755 /*   first unprocessed node in J's set (with I = 0 if the */
15756 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15757 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15758 /*   set are initially ordered by increasing indexes (which */
15759 /*   maximizes efficiency) but that ordering is not main- */
15760 /*   tained as the data structure is updated. */
15761 
15762 /* Initialize the data structure for the single triangle. */
15763 
15764     near__[1] = 0;
15765     near__[2] = 0;
15766     near__[3] = 0;
15767     for (k = nn; k >= 4; --k) {
15768         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15769         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15770         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15771         if (d1 <= d2 && d1 <= d3) {
15772             near__[k] = 1;
15773             dist[k] = d1;
15774             next[k] = near__[1];
15775             near__[1] = k;
15776         } else if (d2 <= d1 && d2 <= d3) {
15777             near__[k] = 2;
15778             dist[k] = d2;
15779             next[k] = near__[2];
15780             near__[2] = k;
15781         } else {
15782             near__[k] = 3;
15783             dist[k] = d3;
15784             next[k] = near__[3];
15785             near__[3] = k;
15786         }
15787 /* L1: */
15788     }
15789 
15790 /* Add the remaining nodes */
15791 
15792     i__1 = nn;
15793     for (k = 4; k <= i__1; ++k) {
15794         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15795                 lend[1], lnew, ier);
15796         if (*ier != 0) {
15797             return 0;
15798         }
15799 
15800 /* Remove K from the set of unprocessed nodes associated */
15801 /*   with NEAR(K). */
15802 
15803         i__ = near__[k];
15804         if (near__[i__] == k) {
15805             near__[i__] = next[k];
15806         } else {
15807             i__ = near__[i__];
15808 L2:
15809             i0 = i__;
15810             i__ = next[i0];
15811             if (i__ != k) {
15812                 goto L2;
15813             }
15814             next[i0] = next[k];
15815         }
15816         near__[k] = 0;
15817 
15818 /* Loop on neighbors J of node K. */
15819 
15820         lpl = lend[k];
15821         lp = lpl;
15822 L3:
15823         lp = lptr[lp];
15824         j = (i__2 = list[lp], abs(i__2));
15825 
15826 /* Loop on elements I in the sequence of unprocessed nodes */
15827 /*   associated with J:  K is a candidate for replacing J */
15828 /*   as the nearest triangulation node to I.  The next value */
15829 /*   of I in the sequence, NEXT(I), must be saved before I */
15830 /*   is moved because it is altered by adding I to K's set. */
15831 
15832         i__ = near__[j];
15833 L4:
15834         if (i__ == 0) {
15835             goto L5;
15836         }
15837         nexti = next[i__];
15838 
15839 /* Test for the distance from I to K less than the distance */
15840 /*   from I to J. */
15841 
15842         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15843         if (d__ < dist[i__]) {
15844 
15845 /* Replace J by K as the nearest triangulation node to I: */
15846 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15847 /*   of unprocessed nodes and add it to K's set. */
15848 
15849             near__[i__] = k;
15850             dist[i__] = d__;
15851             if (i__ == near__[j]) {
15852                 near__[j] = nexti;
15853             } else {
15854                 next[i0] = nexti;
15855             }
15856             next[i__] = near__[k];
15857             near__[k] = i__;
15858         } else {
15859             i0 = i__;
15860         }
15861 
15862 /* Bottom of loop on I. */
15863 
15864         i__ = nexti;
15865         goto L4;
15866 
15867 /* Bottom of loop on neighbors J. */
15868 
15869 L5:
15870         if (lp != lpl) {
15871             goto L3;
15872         }
15873 /* L6: */
15874     }
15875     return 0;
15876 } /* trmesh_ */
15877 
15878 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
15879         elat, double *elon, double *a, int *n, double *x,
15880         double *y, double *z__, int *list, int *lptr, int
15881         *lend, char *, long int *numbr, int *ier, short )
15882 {
15883     /* Initialized data */
15884 
15885     static long int annot = TRUE_;
15886     static double fsizn = 10.;
15887     static double fsizt = 16.;
15888     static double tol = .5;
15889 
15890     /* System generated locals */
15891     int i__1, i__2;
15892     double d__1;
15893 
15894     /* Builtin functions */
15895     //double atan(double), sin(double);
15896     //int i_dnnt(double *);
15897     //double cos(double), sqrt(double);
15898 
15899     /* Local variables */
15900     static double t;
15901     static int n0, n1;
15902     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15903     static int ir, lp;
15904     static double ex, ey, ez, wr, tx, ty;
15905     static int lpl;
15906     static double wrs;
15907     static int ipx1, ipx2, ipy1, ipy2, nseg;
15908     /* Subroutine */ int drwarc_(int *, double *, double *,
15909              double *, int *);
15910 
15911 
15912 /* *********************************************************** */
15913 
15914 /*                                              From STRIPACK */
15915 /*                                            Robert J. Renka */
15916 /*                                  Dept. of Computer Science */
15917 /*                                       Univ. of North Texas */
15918 /*                                           renka@cs.unt.edu */
15919 /*                                                   03/04/03 */
15920 
15921 /*   This subroutine creates a level-2 Encapsulated Post- */
15922 /* script (EPS) file containing a graphical display of a */
15923 /* triangulation of a set of nodes on the surface of the unit */
15924 /* sphere.  The visible portion of the triangulation is */
15925 /* projected onto the plane that contains the origin and has */
15926 /* normal defined by a user-specified eye-position. */
15927 
15928 
15929 /* On input: */
15930 
15931 /*       LUN = long int unit number in the range 0 to 99. */
15932 /*             The unit should be opened with an appropriate */
15933 /*             file name before the call to this routine. */
15934 
15935 /*       PLTSIZ = Plot size in inches.  A circular window in */
15936 /*                the projection plane is mapped to a circu- */
15937 /*                lar viewport with diameter equal to .88* */
15938 /*                PLTSIZ (leaving room for labels outside the */
15939 /*                viewport).  The viewport is centered on the */
15940 /*                8.5 by 11 inch page, and its boundary is */
15941 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
15942 
15943 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
15944 /*                   the center of projection E (the center */
15945 /*                   of the plot).  The projection plane is */
15946 /*                   the plane that contains the origin and */
15947 /*                   has E as unit normal.  In a rotated */
15948 /*                   coordinate system for which E is the */
15949 /*                   north pole, the projection plane con- */
15950 /*                   tains the equator, and only northern */
15951 /*                   hemisphere nodes are visible (from the */
15952 /*                   point at infinity in the direction E). */
15953 /*                   These are projected orthogonally onto */
15954 /*                   the projection plane (by zeroing the z- */
15955 /*                   component in the rotated coordinate */
15956 /*                   system).  ELAT and ELON must be in the */
15957 /*                   range -90 to 90 and -180 to 180, respec- */
15958 /*                   tively. */
15959 
15960 /*       A = Angular distance in degrees from E to the boun- */
15961 /*           dary of a circular window against which the */
15962 /*           triangulation is clipped.  The projected window */
15963 /*           is a disk of radius r = Sin(A) centered at the */
15964 /*           origin, and only visible nodes whose projections */
15965 /*           are within distance r of the origin are included */
15966 /*           in the plot.  Thus, if A = 90, the plot includes */
15967 /*           the entire hemisphere centered at E.  0 .LT. A */
15968 /*           .LE. 90. */
15969 
15970 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15971 
15972 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15973 /*               coordinates of the nodes (unit vectors). */
15974 
15975 /*       LIST,LPTR,LEND = Data structure defining the trian- */
15976 /*                        gulation.  Refer to Subroutine */
15977 /*                        TRMESH. */
15978 
15979 /*       TITLE = Type CHARACTER variable or constant contain- */
15980 /*               ing a string to be centered above the plot. */
15981 /*               The string must be enclosed in parentheses; */
15982 /*               i.e., the first and last characters must be */
15983 /*               '(' and ')', respectively, but these are not */
15984 /*               displayed.  TITLE may have at most 80 char- */
15985 /*               acters including the parentheses. */
15986 
15987 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
15988 /*               nodal indexes are plotted next to the nodes. */
15989 
15990 /* Input parameters are not altered by this routine. */
15991 
15992 /* On output: */
15993 
15994 /*       IER = Error indicator: */
15995 /*             IER = 0 if no errors were encountered. */
15996 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
15997 /*                     valid range. */
15998 /*             IER = 2 if ELAT, ELON, or A is outside its */
15999 /*                     valid range. */
16000 /*             IER = 3 if an error was encountered in writing */
16001 /*                     to unit LUN. */
16002 
16003 /*   The values in the data statement below may be altered */
16004 /* in order to modify various plotting options. */
16005 
16006 /* Module required by TRPLOT:  DRWARC */
16007 
16008 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
16009 /*                                          DBLE, NINT, SIN, */
16010 /*                                          SQRT */
16011 
16012 /* *********************************************************** */
16013 
16014 
16015     /* Parameter adjustments */
16016     --lend;
16017     --z__;
16018     --y;
16019     --x;
16020     --list;
16021     --lptr;
16022 
16023     /* Function Body */
16024 
16025 /* Local parameters: */
16026 
16027 /* ANNOT =     long int variable with value TRUE iff the plot */
16028 /*               is to be annotated with the values of ELAT, */
16029 /*               ELON, and A */
16030 /* CF =        Conversion factor for degrees to radians */
16031 /* CT =        Cos(ELAT) */
16032 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16033 /* FSIZN =     Font size in points for labeling nodes with */
16034 /*               their indexes if NUMBR = TRUE */
16035 /* FSIZT =     Font size in points for the title (and */
16036 /*               annotation if ANNOT = TRUE) */
16037 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16038 /*               left corner of the bounding box or viewport */
16039 /*               box */
16040 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16041 /*               right corner of the bounding box or viewport */
16042 /*               box */
16043 /* IR =        Half the width (height) of the bounding box or */
16044 /*               viewport box in points -- viewport radius */
16045 /* LP =        LIST index (pointer) */
16046 /* LPL =       Pointer to the last neighbor of N0 */
16047 /* N0 =        Index of a node whose incident arcs are to be */
16048 /*               drawn */
16049 /* N1 =        Neighbor of N0 */
16050 /* NSEG =      Number of line segments used by DRWARC in a */
16051 /*               polygonal approximation to a projected edge */
16052 /* P0 =        Coordinates of N0 in the rotated coordinate */
16053 /*               system or label location (first two */
16054 /*               components) */
16055 /* P1 =        Coordinates of N1 in the rotated coordinate */
16056 /*               system or intersection of edge N0-N1 with */
16057 /*               the equator (in the rotated coordinate */
16058 /*               system) */
16059 /* R11...R23 = Components of the first two rows of a rotation */
16060 /*               that maps E to the north pole (0,0,1) */
16061 /* SF =        Scale factor for mapping world coordinates */
16062 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16063 /*               to viewport coordinates in [IPX1,IPX2] X */
16064 /*               [IPY1,IPY2] */
16065 /* T =         Temporary variable */
16066 /* TOL =       Maximum distance in points between a projected */
16067 /*               triangulation edge and its approximation by */
16068 /*               a polygonal curve */
16069 /* TX,TY =     Translation vector for mapping world coordi- */
16070 /*               nates to viewport coordinates */
16071 /* WR =        Window radius r = Sin(A) */
16072 /* WRS =       WR**2 */
16073 
16074 
16075 /* Test for invalid parameters. */
16076 
16077     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16078         goto L11;
16079     }
16080     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16081         goto L12;
16082     }
16083 
16084 /* Compute a conversion factor CF for degrees to radians */
16085 /*   and compute the window radius WR. */
16086 
16087     cf = atan(1.) / 45.;
16088     wr = sin(cf * *a);
16089     wrs = wr * wr;
16090 
16091 /* Compute the lower left (IPX1,IPY1) and upper right */
16092 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16093 /*   The coordinates, specified in default user space units */
16094 /*   (points, at 72 points/inch with origin at the lower */
16095 /*   left corner of the page), are chosen to preserve the */
16096 /*   square aspect ratio, and to center the plot on the 8.5 */
16097 /*   by 11 inch page.  The center of the page is (306,396), */
16098 /*   and IR = PLTSIZ/2 in points. */
16099 
16100     d__1 = *pltsiz * 36.;
16101     ir = i_dnnt(&d__1);
16102     ipx1 = 306 - ir;
16103     ipx2 = ir + 306;
16104     ipy1 = 396 - ir;
16105     ipy2 = ir + 396;
16106 
16107 /* Output header comments. */
16108 
16109 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16110 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16111 /*     .        '%%BoundingBox:',4I4/ */
16112 /*     .        '%%Title:  Triangulation'/ */
16113 /*     .        '%%Creator:  STRIPACK'/ */
16114 /*     .        '%%EndComments') */
16115 
16116 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16117 /*   of a viewport box obtained by shrinking the bounding box */
16118 /*   by 12% in each dimension. */
16119 
16120     d__1 = (double) ir * .88;
16121     ir = i_dnnt(&d__1);
16122     ipx1 = 306 - ir;
16123     ipx2 = ir + 306;
16124     ipy1 = 396 - ir;
16125     ipy2 = ir + 396;
16126 
16127 /* Set the line thickness to 2 points, and draw the */
16128 /*   viewport boundary. */
16129 
16130     t = 2.;
16131 /*      WRITE (LUN,110,ERR=13) T */
16132 /*      WRITE (LUN,120,ERR=13) IR */
16133 /*      WRITE (LUN,130,ERR=13) */
16134 /*  110 FORMAT (F12.6,' setlinewidth') */
16135 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16136 /*  130 FORMAT ('stroke') */
16137 
16138 /* Set up an affine mapping from the window box [-WR,WR] X */
16139 /*   [-WR,WR] to the viewport box. */
16140 
16141     sf = (double) ir / wr;
16142     tx = ipx1 + sf * wr;
16143     ty = ipy1 + sf * wr;
16144 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16145 /*  140 FORMAT (2F12.6,' translate'/ */
16146 /*    .        2F12.6,' scale') */
16147 
16148 /* The line thickness must be changed to reflect the new */
16149 /*   scaling which is applied to all subsequent output. */
16150 /*   Set it to 1.0 point. */
16151 
16152     t = 1. / sf;
16153 /*      WRITE (LUN,110,ERR=13) T */
16154 
16155 /* Save the current graphics state, and set the clip path to */
16156 /*   the boundary of the window. */
16157 
16158 /*      WRITE (LUN,150,ERR=13) */
16159 /*      WRITE (LUN,160,ERR=13) WR */
16160 /*      WRITE (LUN,170,ERR=13) */
16161 /*  150 FORMAT ('gsave') */
16162 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16163 /*  170 FORMAT ('clip newpath') */
16164 
16165 /* Compute the Cartesian coordinates of E and the components */
16166 /*   of a rotation R which maps E to the north pole (0,0,1). */
16167 /*   R is taken to be a rotation about the z-axis (into the */
16168 /*   yz-plane) followed by a rotation about the x-axis chosen */
16169 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16170 /*   E is the north or south pole. */
16171 
16172 /*           ( R11  R12  0   ) */
16173 /*       R = ( R21  R22  R23 ) */
16174 /*           ( EX   EY   EZ  ) */
16175 
16176     t = cf * *elon;
16177     ct = cos(cf * *elat);
16178     ex = ct * cos(t);
16179     ey = ct * sin(t);
16180     ez = sin(cf * *elat);
16181     if (ct != 0.) {
16182         r11 = -ey / ct;
16183         r12 = ex / ct;
16184     } else {
16185         r11 = 0.;
16186         r12 = 1.;
16187     }
16188     r21 = -ez * r12;
16189     r22 = ez * r11;
16190     r23 = ct;
16191 
16192 /* Loop on visible nodes N0 that project to points */
16193 /*   (P0(1),P0(2)) in the window. */
16194 
16195     i__1 = *n;
16196     for (n0 = 1; n0 <= i__1; ++n0) {
16197         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16198         if (p0[2] < 0.) {
16199             goto L3;
16200         }
16201         p0[0] = r11 * x[n0] + r12 * y[n0];
16202         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16203         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16204             goto L3;
16205         }
16206         lpl = lend[n0];
16207         lp = lpl;
16208 
16209 /* Loop on neighbors N1 of N0.  LPL points to the last */
16210 /*   neighbor of N0.  Copy the components of N1 into P. */
16211 
16212 L1:
16213         lp = lptr[lp];
16214         n1 = (i__2 = list[lp], abs(i__2));
16215         p1[0] = r11 * x[n1] + r12 * y[n1];
16216         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16217         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16218         if (p1[2] < 0.) {
16219 
16220 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16221 /*     intersection of edge N0-N1 with the equator so that */
16222 /*     the edge is clipped properly.  P1(3) is set to 0. */
16223 
16224             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16225             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16226             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16227             p1[0] /= t;
16228             p1[1] /= t;
16229         }
16230 
16231 /*   If node N1 is in the window and N1 < N0, bypass edge */
16232 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16233 
16234         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16235             goto L2;
16236         }
16237 
16238 /*   Add the edge to the path.  (TOL is converted to world */
16239 /*     coordinates.) */
16240 
16241         if (p1[2] < 0.) {
16242             p1[2] = 0.;
16243         }
16244         d__1 = tol / sf;
16245         drwarc_(lun, p0, p1, &d__1, &nseg);
16246 
16247 /* Bottom of loops. */
16248 
16249 L2:
16250         if (lp != lpl) {
16251             goto L1;
16252         }
16253 L3:
16254         ;
16255     }
16256 
16257 /* Paint the path and restore the saved graphics state (with */
16258 /*   no clip path). */
16259 
16260 /*      WRITE (LUN,130,ERR=13) */
16261 /*      WRITE (LUN,190,ERR=13) */
16262 /*  190 FORMAT ('grestore') */
16263     if (*numbr) {
16264 
16265 /* Nodes in the window are to be labeled with their indexes. */
16266 /*   Convert FSIZN from points to world coordinates, and */
16267 /*   output the commands to select a font and scale it. */
16268 
16269         t = fsizn / sf;
16270 /*        WRITE (LUN,200,ERR=13) T */
16271 /*  200   FORMAT ('/Helvetica findfont'/ */
16272 /*     .          F12.6,' scalefont setfont') */
16273 
16274 /* Loop on visible nodes N0 that project to points */
16275 /*   P0 = (P0(1),P0(2)) in the window. */
16276 
16277         i__1 = *n;
16278         for (n0 = 1; n0 <= i__1; ++n0) {
16279             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16280                 goto L4;
16281             }
16282             p0[0] = r11 * x[n0] + r12 * y[n0];
16283             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16284             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16285                 goto L4;
16286             }
16287 
16288 /*   Move to P0 and draw the label N0.  The first character */
16289 /*     will will have its lower left corner about one */
16290 /*     character width to the right of the nodal position. */
16291 
16292 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16293 /*          WRITE (LUN,220,ERR=13) N0 */
16294 /*  210     FORMAT (2F12.6,' moveto') */
16295 /*  220     FORMAT ('(',I3,') show') */
16296 L4:
16297             ;
16298         }
16299     }
16300 
16301 /* Convert FSIZT from points to world coordinates, and output */
16302 /*   the commands to select a font and scale it. */
16303 
16304     t = fsizt / sf;
16305 /*      WRITE (LUN,200,ERR=13) T */
16306 
16307 /* Display TITLE centered above the plot: */
16308 
16309     p0[1] = wr + t * 3.;
16310 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16311 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16312 /*     .        ' moveto') */
16313 /*      WRITE (LUN,240,ERR=13) TITLE */
16314 /*  240 FORMAT (A80/'  show') */
16315     if (annot) {
16316 
16317 /* Display the window center and radius below the plot. */
16318 
16319         p0[0] = -wr;
16320         p0[1] = -wr - 50. / sf;
16321 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16322 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16323         p0[1] -= t * 2.;
16324 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16325 /*        WRITE (LUN,260,ERR=13) A */
16326 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16327 /*     .          ',  ELON = ',F8.2,') show') */
16328 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16329     }
16330 
16331 /* Paint the path and output the showpage command and */
16332 /*   end-of-file indicator. */
16333 
16334 /*      WRITE (LUN,270,ERR=13) */
16335 /*  270 FORMAT ('stroke'/ */
16336 /*     .        'showpage'/ */
16337 /*     .        '%%EOF') */
16338 
16339 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16340 /*   indicator (to eliminate a timeout error message): */
16341 /*   ASCII 4. */
16342 
16343 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16344 /*  280 FORMAT (A1) */
16345 
16346 /* No error encountered. */
16347 
16348     *ier = 0;
16349     return 0;
16350 
16351 /* Invalid input parameter LUN, PLTSIZ, or N. */
16352 
16353 L11:
16354     *ier = 1;
16355     return 0;
16356 
16357 /* Invalid input parameter ELAT, ELON, or A. */
16358 
16359 L12:
16360     *ier = 2;
16361     return 0;
16362 
16363 /* Error writing to unit LUN. */
16364 
16365 /* L13: */
16366     *ier = 3;
16367     return 0;
16368 } /* trplot_ */
16369 
16370 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16371         double *z__, int *iflag, int *list, int *lptr,
16372         int *lend, int *lout)
16373 {
16374     /* Initialized data */
16375 
16376     static int nmax = 9999;
16377     static int nlmax = 58;
16378 
16379     /* System generated locals */
16380     int i__1;
16381 
16382     /* Local variables */
16383     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16384             400];
16385 
16386 
16387 /* *********************************************************** */
16388 
16389 /*                                              From STRIPACK */
16390 /*                                            Robert J. Renka */
16391 /*                                  Dept. of Computer Science */
16392 /*                                       Univ. of North Texas */
16393 /*                                           renka@cs.unt.edu */
16394 /*                                                   07/25/98 */
16395 
16396 /*   This subroutine prints the triangulation adjacency lists */
16397 /* created by Subroutine TRMESH and, optionally, the nodal */
16398 /* coordinates (either latitude and longitude or Cartesian */
16399 /* coordinates) on long int unit LOUT.  The list of neighbors */
16400 /* of a boundary node is followed by index 0.  The numbers of */
16401 /* boundary nodes, triangles, and arcs are also printed. */
16402 
16403 
16404 /* On input: */
16405 
16406 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16407 /*           and N .LE. 9999. */
16408 
16409 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16410 /*               coordinates of the nodes if IFLAG = 0, or */
16411 /*               (X and Y only) arrays of length N containing */
16412 /*               longitude and latitude, respectively, if */
16413 /*               IFLAG > 0, or unused dummy parameters if */
16414 /*               IFLAG < 0. */
16415 
16416 /*       IFLAG = Nodal coordinate option indicator: */
16417 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16418 /*                         Cartesian coordinates) are to be */
16419 /*                         printed (to 6 decimal places). */
16420 /*               IFLAG > 0 if only X and Y (assumed to con- */
16421 /*                         tain longitude and latitude) are */
16422 /*                         to be printed (to 6 decimal */
16423 /*                         places). */
16424 /*               IFLAG < 0 if only the adjacency lists are to */
16425 /*                         be printed. */
16426 
16427 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16428 /*                        gulation.  Refer to Subroutine */
16429 /*                        TRMESH. */
16430 
16431 /*       LOUT = long int unit for output.  If LOUT is not in */
16432 /*              the range 0 to 99, output is written to */
16433 /*              long int unit 6. */
16434 
16435 /* Input parameters are not altered by this routine. */
16436 
16437 /* On output: */
16438 
16439 /*   The adjacency lists and nodal coordinates (as specified */
16440 /* by IFLAG) are written to unit LOUT. */
16441 
16442 /* Modules required by TRPRNT:  None */
16443 
16444 /* *********************************************************** */
16445 
16446     /* Parameter adjustments */
16447     --lend;
16448     --z__;
16449     --y;
16450     --x;
16451     --list;
16452     --lptr;
16453 
16454     /* Function Body */
16455 
16456 /* Local parameters: */
16457 
16458 /* I =     NABOR index (1 to K) */
16459 /* INC =   Increment for NL associated with an adjacency list */
16460 /* K =     Counter and number of neighbors of NODE */
16461 /* LP =    LIST pointer of a neighbor of NODE */
16462 /* LPL =   Pointer to the last neighbor of NODE */
16463 /* LUN =   long int unit for output (copy of LOUT) */
16464 /* NA =    Number of arcs in the triangulation */
16465 /* NABOR = Array containing the adjacency list associated */
16466 /*           with NODE, with zero appended if NODE is a */
16467 /*           boundary node */
16468 /* NB =    Number of boundary nodes encountered */
16469 /* ND =    Index of a neighbor of NODE (or negative index) */
16470 /* NL =    Number of lines that have been printed on the */
16471 /*           current page */
16472 /* NLMAX = Maximum number of print lines per page (except */
16473 /*           for the last page which may have two addi- */
16474 /*           tional lines) */
16475 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16476 /* NODE =  Index of a node and DO-loop index (1 to N) */
16477 /* NN =    Local copy of N */
16478 /* NT =    Number of triangles in the triangulation */
16479 
16480     nn = *n;
16481     lun = *lout;
16482     if (lun < 0 || lun > 99) {
16483         lun = 6;
16484     }
16485 
16486 /* Print a heading and test the range of N. */
16487 
16488 /*      WRITE (LUN,100) NN */
16489     if (nn < 3 || nn > nmax) {
16490 
16491 /* N is outside its valid range. */
16492 
16493 /*        WRITE (LUN,110) */
16494         return 0;
16495     }
16496 
16497 /* Initialize NL (the number of lines printed on the current */
16498 /*   page) and NB (the number of boundary nodes encountered). */
16499 
16500     nl = 6;
16501     nb = 0;
16502     if (*iflag < 0) {
16503 
16504 /* Print LIST only.  K is the number of neighbors of NODE */
16505 /*   that have been stored in NABOR. */
16506 
16507 /*        WRITE (LUN,101) */
16508         i__1 = nn;
16509         for (node = 1; node <= i__1; ++node) {
16510             lpl = lend[node];
16511             lp = lpl;
16512             k = 0;
16513 
16514 L1:
16515             ++k;
16516             lp = lptr[lp];
16517             nd = list[lp];
16518             nabor[k - 1] = nd;
16519             if (lp != lpl) {
16520                 goto L1;
16521             }
16522             if (nd <= 0) {
16523 
16524 /*   NODE is a boundary node.  Correct the sign of the last */
16525 /*     neighbor, add 0 to the end of the list, and increment */
16526 /*     NB. */
16527 
16528                 nabor[k - 1] = -nd;
16529                 ++k;
16530                 nabor[k - 1] = 0;
16531                 ++nb;
16532             }
16533 
16534 /*   Increment NL and print the list of neighbors. */
16535 
16536             inc = (k - 1) / 14 + 2;
16537             nl += inc;
16538             if (nl > nlmax) {
16539 /*            WRITE (LUN,108) */
16540                 nl = inc;
16541             }
16542 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16543 /*          IF (K .NE. 14) */
16544 /*           WRITE (LUN,107) */
16545 /* L2: */
16546         }
16547     } else if (*iflag > 0) {
16548 
16549 /* Print X (longitude), Y (latitude), and LIST. */
16550 
16551 /*        WRITE (LUN,102) */
16552         i__1 = nn;
16553         for (node = 1; node <= i__1; ++node) {
16554             lpl = lend[node];
16555             lp = lpl;
16556             k = 0;
16557 
16558 L3:
16559             ++k;
16560             lp = lptr[lp];
16561             nd = list[lp];
16562             nabor[k - 1] = nd;
16563             if (lp != lpl) {
16564                 goto L3;
16565             }
16566             if (nd <= 0) {
16567 
16568 /*   NODE is a boundary node. */
16569 
16570                 nabor[k - 1] = -nd;
16571                 ++k;
16572                 nabor[k - 1] = 0;
16573                 ++nb;
16574             }
16575 
16576 /*   Increment NL and print X, Y, and NABOR. */
16577 
16578             inc = (k - 1) / 8 + 2;
16579             nl += inc;
16580             if (nl > nlmax) {
16581 /*            WRITE (LUN,108) */
16582                 nl = inc;
16583             }
16584 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16585 /*          IF (K .NE. 8) */
16586 /*           PRINT *,K */
16587 /*           WRITE (LUN,107) */
16588 /* L4: */
16589         }
16590     } else {
16591 
16592 /* Print X, Y, Z, and LIST. */
16593 
16594 /*        WRITE (LUN,103) */
16595         i__1 = nn;
16596         for (node = 1; node <= i__1; ++node) {
16597             lpl = lend[node];
16598             lp = lpl;
16599             k = 0;
16600 
16601 L5:
16602             ++k;
16603             lp = lptr[lp];
16604             nd = list[lp];
16605             nabor[k - 1] = nd;
16606             if (lp != lpl) {
16607                 goto L5;
16608             }
16609             if (nd <= 0) {
16610 
16611 /*   NODE is a boundary node. */
16612 
16613                 nabor[k - 1] = -nd;
16614                 ++k;
16615                 nabor[k - 1] = 0;
16616                 ++nb;
16617             }
16618 
16619 /*   Increment NL and print X, Y, Z, and NABOR. */
16620 
16621             inc = (k - 1) / 5 + 2;
16622             nl += inc;
16623             if (nl > nlmax) {
16624 /*            WRITE (LUN,108) */
16625                 nl = inc;
16626             }
16627 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16628 /*          IF (K .NE. 5) */
16629 /*           print *,K */
16630 /*           WRITE (LUN,107) */
16631 /* L6: */
16632         }
16633     }
16634 
16635 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16636 /*   triangles). */
16637 
16638     if (nb != 0) {
16639         na = nn * 3 - nb - 3;
16640         nt = (nn << 1) - nb - 2;
16641     } else {
16642         na = nn * 3 - 6;
16643         nt = (nn << 1) - 4;
16644     }
16645 /*      WRITE (LUN,109) NB, NA, NT */
16646     return 0;
16647 
16648 /* Print formats: */
16649 
16650 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16651 /*     .        'Structure,  N = ',I5//) */
16652 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16653 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16654 /*     .        18X,'Neighbors of Node'//) */
16655 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16656 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16657 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16658 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16659 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16660 /*  107 FORMAT (1X) */
16661 /*  108 FORMAT (///) */
16662 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16663 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16664 /*     .        ' Triangles') */
16665 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16666 /*     .        ' range ***') */
16667 } /* trprnt_ */
16668 
16669 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16670         elat, double *elon, double *a, int *n, double *x,
16671         double *y, double *z__, int *nt, int *listc, int *
16672         lptr, int *lend, double *xc, double *yc, double *zc,
16673         char *, long int *numbr, int *ier, short)
16674 {
16675     /* Initialized data */
16676 
16677     static long int annot = TRUE_;
16678     static double fsizn = 10.;
16679     static double fsizt = 16.;
16680     static double tol = .5;
16681 
16682     /* System generated locals */
16683     int i__1;
16684     double d__1;
16685 
16686     /* Builtin functions */
16687     //double atan(double), sin(double);
16688     //int i_dnnt(double *);
16689     //double cos(double), sqrt(double);
16690 
16691     /* Local variables */
16692     static double t;
16693     static int n0;
16694     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16695             sf;
16696     static int ir, lp;
16697     static double ex, ey, ez, wr, tx, ty;
16698     static long int in1, in2;
16699     static int kv1, kv2, lpl;
16700     static double wrs;
16701     static int ipx1, ipx2, ipy1, ipy2, nseg;
16702     /* Subroutine */ int drwarc_(int *, double *, double *,
16703              double *, int *);
16704 
16705 
16706 /* *********************************************************** */
16707 
16708 /*                                              From STRIPACK */
16709 /*                                            Robert J. Renka */
16710 /*                                  Dept. of Computer Science */
16711 /*                                       Univ. of North Texas */
16712 /*                                           renka@cs.unt.edu */
16713 /*                                                   03/04/03 */
16714 
16715 /*   This subroutine creates a level-2 Encapsulated Post- */
16716 /* script (EPS) file containing a graphical depiction of a */
16717 /* Voronoi diagram of a set of nodes on the unit sphere. */
16718 /* The visible portion of the diagram is projected orthog- */
16719 /* onally onto the plane that contains the origin and has */
16720 /* normal defined by a user-specified eye-position. */
16721 
16722 /*   The parameters defining the Voronoi diagram may be com- */
16723 /* puted by Subroutine CRLIST. */
16724 
16725 
16726 /* On input: */
16727 
16728 /*       LUN = long int unit number in the range 0 to 99. */
16729 /*             The unit should be opened with an appropriate */
16730 /*             file name before the call to this routine. */
16731 
16732 /*       PLTSIZ = Plot size in inches.  A circular window in */
16733 /*                the projection plane is mapped to a circu- */
16734 /*                lar viewport with diameter equal to .88* */
16735 /*                PLTSIZ (leaving room for labels outside the */
16736 /*                viewport).  The viewport is centered on the */
16737 /*                8.5 by 11 inch page, and its boundary is */
16738 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16739 
16740 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16741 /*                   the center of projection E (the center */
16742 /*                   of the plot).  The projection plane is */
16743 /*                   the plane that contains the origin and */
16744 /*                   has E as unit normal.  In a rotated */
16745 /*                   coordinate system for which E is the */
16746 /*                   north pole, the projection plane con- */
16747 /*                   tains the equator, and only northern */
16748 /*                   hemisphere points are visible (from the */
16749 /*                   point at infinity in the direction E). */
16750 /*                   These are projected orthogonally onto */
16751 /*                   the projection plane (by zeroing the z- */
16752 /*                   component in the rotated coordinate */
16753 /*                   system).  ELAT and ELON must be in the */
16754 /*                   range -90 to 90 and -180 to 180, respec- */
16755 /*                   tively. */
16756 
16757 /*       A = Angular distance in degrees from E to the boun- */
16758 /*           dary of a circular window against which the */
16759 /*           Voronoi diagram is clipped.  The projected win- */
16760 /*           dow is a disk of radius r = Sin(A) centered at */
16761 /*           the origin, and only visible vertices whose */
16762 /*           projections are within distance r of the origin */
16763 /*           are included in the plot.  Thus, if A = 90, the */
16764 /*           plot includes the entire hemisphere centered at */
16765 /*           E.  0 .LT. A .LE. 90. */
16766 
16767 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16768 /*           regions.  N .GE. 3. */
16769 
16770 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16771 /*               coordinates of the nodes (unit vectors). */
16772 
16773 /*       NT = Number of Voronoi region vertices (triangles, */
16774 /*            including those in the extended triangulation */
16775 /*            if the number of boundary nodes NB is nonzero): */
16776 /*            NT = 2*N-4. */
16777 
16778 /*       LISTC = Array of length 3*NT containing triangle */
16779 /*               indexes (indexes to XC, YC, and ZC) stored */
16780 /*               in 1-1 correspondence with LIST/LPTR entries */
16781 /*               (or entries that would be stored in LIST for */
16782 /*               the extended triangulation):  the index of */
16783 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16784 /*               LISTC(L), and LISTC(M), where LIST(K), */
16785 /*               LIST(L), and LIST(M) are the indexes of N2 */
16786 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16787 /*               and N1 as a neighbor of N3.  The Voronoi */
16788 /*               region associated with a node is defined by */
16789 /*               the CCW-ordered sequence of circumcenters in */
16790 /*               one-to-one correspondence with its adjacency */
16791 /*               list (in the extended triangulation). */
16792 
16793 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16794 /*              set of pointers (LISTC indexes) in one-to-one */
16795 /*              correspondence with the elements of LISTC. */
16796 /*              LISTC(LPTR(I)) indexes the triangle which */
16797 /*              follows LISTC(I) in cyclical counterclockwise */
16798 /*              order (the first neighbor follows the last */
16799 /*              neighbor). */
16800 
16801 /*       LEND = Array of length N containing a set of */
16802 /*              pointers to triangle lists.  LP = LEND(K) */
16803 /*              points to a triangle (indexed by LISTC(LP)) */
16804 /*              containing node K for K = 1 to N. */
16805 
16806 /*       XC,YC,ZC = Arrays of length NT containing the */
16807 /*                  Cartesian coordinates of the triangle */
16808 /*                  circumcenters (Voronoi vertices). */
16809 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16810 
16811 /*       TITLE = Type CHARACTER variable or constant contain- */
16812 /*               ing a string to be centered above the plot. */
16813 /*               The string must be enclosed in parentheses; */
16814 /*               i.e., the first and last characters must be */
16815 /*               '(' and ')', respectively, but these are not */
16816 /*               displayed.  TITLE may have at most 80 char- */
16817 /*               acters including the parentheses. */
16818 
16819 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16820 /*               nodal indexes are plotted at the Voronoi */
16821 /*               region centers. */
16822 
16823 /* Input parameters are not altered by this routine. */
16824 
16825 /* On output: */
16826 
16827 /*       IER = Error indicator: */
16828 /*             IER = 0 if no errors were encountered. */
16829 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16830 /*                     its valid range. */
16831 /*             IER = 2 if ELAT, ELON, or A is outside its */
16832 /*                     valid range. */
16833 /*             IER = 3 if an error was encountered in writing */
16834 /*                     to unit LUN. */
16835 
16836 /* Module required by VRPLOT:  DRWARC */
16837 
16838 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16839 /*                                          DBLE, NINT, SIN, */
16840 /*                                          SQRT */
16841 
16842 /* *********************************************************** */
16843 
16844 
16845     /* Parameter adjustments */
16846     --lend;
16847     --z__;
16848     --y;
16849     --x;
16850     --zc;
16851     --yc;
16852     --xc;
16853     --listc;
16854     --lptr;
16855 
16856     /* Function Body */
16857 
16858 /* Local parameters: */
16859 
16860 /* ANNOT =     long int variable with value TRUE iff the plot */
16861 /*               is to be annotated with the values of ELAT, */
16862 /*               ELON, and A */
16863 /* CF =        Conversion factor for degrees to radians */
16864 /* CT =        Cos(ELAT) */
16865 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16866 /* FSIZN =     Font size in points for labeling nodes with */
16867 /*               their indexes if NUMBR = TRUE */
16868 /* FSIZT =     Font size in points for the title (and */
16869 /*               annotation if ANNOT = TRUE) */
16870 /* IN1,IN2 =   long int variables with value TRUE iff the */
16871 /*               projections of vertices KV1 and KV2, respec- */
16872 /*               tively, are inside the window */
16873 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16874 /*               left corner of the bounding box or viewport */
16875 /*               box */
16876 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16877 /*               right corner of the bounding box or viewport */
16878 /*               box */
16879 /* IR =        Half the width (height) of the bounding box or */
16880 /*               viewport box in points -- viewport radius */
16881 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
16882 /* LP =        LIST index (pointer) */
16883 /* LPL =       Pointer to the last neighbor of N0 */
16884 /* N0 =        Index of a node */
16885 /* NSEG =      Number of line segments used by DRWARC in a */
16886 /*               polygonal approximation to a projected edge */
16887 /* P1 =        Coordinates of vertex KV1 in the rotated */
16888 /*               coordinate system */
16889 /* P2 =        Coordinates of vertex KV2 in the rotated */
16890 /*               coordinate system or intersection of edge */
16891 /*               KV1-KV2 with the equator (in the rotated */
16892 /*               coordinate system) */
16893 /* R11...R23 = Components of the first two rows of a rotation */
16894 /*               that maps E to the north pole (0,0,1) */
16895 /* SF =        Scale factor for mapping world coordinates */
16896 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16897 /*               to viewport coordinates in [IPX1,IPX2] X */
16898 /*               [IPY1,IPY2] */
16899 /* T =         Temporary variable */
16900 /* TOL =       Maximum distance in points between a projected */
16901 /*               Voronoi edge and its approximation by a */
16902 /*               polygonal curve */
16903 /* TX,TY =     Translation vector for mapping world coordi- */
16904 /*               nates to viewport coordinates */
16905 /* WR =        Window radius r = Sin(A) */
16906 /* WRS =       WR**2 */
16907 /* X0,Y0 =     Projection plane coordinates of node N0 or */
16908 /*               label location */
16909 
16910 
16911 /* Test for invalid parameters. */
16912 
16913     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16914             nt != 2 * *n - 4) {
16915         goto L11;
16916     }
16917     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16918         goto L12;
16919     }
16920 
16921 /* Compute a conversion factor CF for degrees to radians */
16922 /*   and compute the window radius WR. */
16923 
16924     cf = atan(1.) / 45.;
16925     wr = sin(cf * *a);
16926     wrs = wr * wr;
16927 
16928 /* Compute the lower left (IPX1,IPY1) and upper right */
16929 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16930 /*   The coordinates, specified in default user space units */
16931 /*   (points, at 72 points/inch with origin at the lower */
16932 /*   left corner of the page), are chosen to preserve the */
16933 /*   square aspect ratio, and to center the plot on the 8.5 */
16934 /*   by 11 inch page.  The center of the page is (306,396), */
16935 /*   and IR = PLTSIZ/2 in points. */
16936 
16937     d__1 = *pltsiz * 36.;
16938     ir = i_dnnt(&d__1);
16939     ipx1 = 306 - ir;
16940     ipx2 = ir + 306;
16941     ipy1 = 396 - ir;
16942     ipy2 = ir + 396;
16943 
16944 /* Output header comments. */
16945 
16946 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16947 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16948 /*     .        '%%BoundingBox:',4I4/ */
16949 /*     .        '%%Title:  Voronoi diagram'/ */
16950 /*     .        '%%Creator:  STRIPACK'/ */
16951 /*     .        '%%EndComments') */
16952 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16953 /*   of a viewport box obtained by shrinking the bounding box */
16954 /*   by 12% in each dimension. */
16955 
16956     d__1 = (double) ir * .88;
16957     ir = i_dnnt(&d__1);
16958     ipx1 = 306 - ir;
16959     ipx2 = ir + 306;
16960     ipy1 = 396 - ir;
16961     ipy2 = ir + 396;
16962 
16963 /* Set the line thickness to 2 points, and draw the */
16964 /*   viewport boundary. */
16965 
16966     t = 2.;
16967 /*      WRITE (LUN,110,ERR=13) T */
16968 /*      WRITE (LUN,120,ERR=13) IR */
16969 /*      WRITE (LUN,130,ERR=13) */
16970 /*  110 FORMAT (F12.6,' setlinewidth') */
16971 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16972 /*  130 FORMAT ('stroke') */
16973 
16974 /* Set up an affine mapping from the window box [-WR,WR] X */
16975 /*   [-WR,WR] to the viewport box. */
16976 
16977     sf = (double) ir / wr;
16978     tx = ipx1 + sf * wr;
16979     ty = ipy1 + sf * wr;
16980 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16981 /*  140 FORMAT (2F12.6,' translate'/ */
16982 /*     .        2F12.6,' scale') */
16983 
16984 /* The line thickness must be changed to reflect the new */
16985 /*   scaling which is applied to all subsequent output. */
16986 /*   Set it to 1.0 point. */
16987 
16988     t = 1. / sf;
16989 /*      WRITE (LUN,110,ERR=13) T */
16990 
16991 /* Save the current graphics state, and set the clip path to */
16992 /*   the boundary of the window. */
16993 
16994 /*      WRITE (LUN,150,ERR=13) */
16995 /*      WRITE (LUN,160,ERR=13) WR */
16996 /*      WRITE (LUN,170,ERR=13) */
16997 /*  150 FORMAT ('gsave') */
16998 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16999 /*  170 FORMAT ('clip newpath') */
17000 
17001 /* Compute the Cartesian coordinates of E and the components */
17002 /*   of a rotation R which maps E to the north pole (0,0,1). */
17003 /*   R is taken to be a rotation about the z-axis (into the */
17004 /*   yz-plane) followed by a rotation about the x-axis chosen */
17005 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
17006 /*   E is the north or south pole. */
17007 
17008 /*           ( R11  R12  0   ) */
17009 /*       R = ( R21  R22  R23 ) */
17010 /*           ( EX   EY   EZ  ) */
17011 
17012     t = cf * *elon;
17013     ct = cos(cf * *elat);
17014     ex = ct * cos(t);
17015     ey = ct * sin(t);
17016     ez = sin(cf * *elat);
17017     if (ct != 0.) {
17018         r11 = -ey / ct;
17019         r12 = ex / ct;
17020     } else {
17021         r11 = 0.;
17022         r12 = 1.;
17023     }
17024     r21 = -ez * r12;
17025     r22 = ez * r11;
17026     r23 = ct;
17027 
17028 /* Loop on nodes (Voronoi centers) N0. */
17029 /*   LPL indexes the last neighbor of N0. */
17030 
17031     i__1 = *n;
17032     for (n0 = 1; n0 <= i__1; ++n0) {
17033         lpl = lend[n0];
17034 
17035 /* Set KV2 to the first (and last) vertex index and compute */
17036 /*   its coordinates P2 in the rotated coordinate system. */
17037 
17038         kv2 = listc[lpl];
17039         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17040         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17041         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17042 
17043 /*   IN2 = TRUE iff KV2 is in the window. */
17044 
17045         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17046 
17047 /* Loop on neighbors N1 of N0.  For each triangulation edge */
17048 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
17049 
17050         lp = lpl;
17051 L1:
17052         lp = lptr[lp];
17053         kv1 = kv2;
17054         p1[0] = p2[0];
17055         p1[1] = p2[1];
17056         p1[2] = p2[2];
17057         in1 = in2;
17058         kv2 = listc[lp];
17059 
17060 /*   Compute the new values of P2 and IN2. */
17061 
17062         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17063         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17064         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17065         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17066 
17067 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
17068 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
17069 /*   outside (so that the edge is drawn only once). */
17070 
17071         if (! in1 || (in2 && kv2 <= kv1)) {
17072             goto L2;
17073         }
17074         if (p2[2] < 0.) {
17075 
17076 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17077 /*     intersection of edge KV1-KV2 with the equator so that */
17078 /*     the edge is clipped properly.  P2(3) is set to 0. */
17079 
17080             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17081             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17082             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17083             p2[0] /= t;
17084             p2[1] /= t;
17085         }
17086 
17087 /*   Add the edge to the path.  (TOL is converted to world */
17088 /*     coordinates.) */
17089 
17090         if (p2[2] < 0.) {
17091             p2[2] = 0.f;
17092         }
17093         d__1 = tol / sf;
17094         drwarc_(lun, p1, p2, &d__1, &nseg);
17095 
17096 /* Bottom of loops. */
17097 
17098 L2:
17099         if (lp != lpl) {
17100             goto L1;
17101         }
17102 /* L3: */
17103     }
17104 
17105 /* Paint the path and restore the saved graphics state (with */
17106 /*   no clip path). */
17107 
17108 /*      WRITE (LUN,130,ERR=13) */
17109 /*      WRITE (LUN,190,ERR=13) */
17110 /*  190 FORMAT ('grestore') */
17111     if (*numbr) {
17112 
17113 /* Nodes in the window are to be labeled with their indexes. */
17114 /*   Convert FSIZN from points to world coordinates, and */
17115 /*   output the commands to select a font and scale it. */
17116 
17117         t = fsizn / sf;
17118 /*        WRITE (LUN,200,ERR=13) T */
17119 /*  200   FORMAT ('/Helvetica findfont'/ */
17120 /*     .          F12.6,' scalefont setfont') */
17121 
17122 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17123 /*   the window. */
17124 
17125         i__1 = *n;
17126         for (n0 = 1; n0 <= i__1; ++n0) {
17127             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17128                 goto L4;
17129             }
17130             x0 = r11 * x[n0] + r12 * y[n0];
17131             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17132             if (x0 * x0 + y0 * y0 > wrs) {
17133                 goto L4;
17134             }
17135 
17136 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17137 /*     of the first character at (X0,Y0). */
17138 
17139 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17140 /*          WRITE (LUN,220,ERR=13) N0 */
17141 /*  210     FORMAT (2F12.6,' moveto') */
17142 /*  220     FORMAT ('(',I3,') show') */
17143 L4:
17144             ;
17145         }
17146     }
17147 
17148 /* Convert FSIZT from points to world coordinates, and output */
17149 /*   the commands to select a font and scale it. */
17150 
17151     t = fsizt / sf;
17152 /*      WRITE (LUN,200,ERR=13) T */
17153 
17154 /* Display TITLE centered above the plot: */
17155 
17156     y0 = wr + t * 3.;
17157 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17158 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17159 /*     .        ' moveto') */
17160 /*      WRITE (LUN,240,ERR=13) TITLE */
17161 /*  240 FORMAT (A80/'  show') */
17162     if (annot) {
17163 
17164 /* Display the window center and radius below the plot. */
17165 
17166         x0 = -wr;
17167         y0 = -wr - 50. / sf;
17168 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17169 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17170         y0 -= t * 2.;
17171 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17172 /*        WRITE (LUN,260,ERR=13) A */
17173 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17174 /*     .          ',  ELON = ',F8.2,') show') */
17175 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17176     }
17177 
17178 /* Paint the path and output the showpage command and */
17179 /*   end-of-file indicator. */
17180 
17181 /*      WRITE (LUN,270,ERR=13) */
17182 /*  270 FORMAT ('stroke'/ */
17183 /*     .        'showpage'/ */
17184 /*     .        '%%EOF') */
17185 
17186 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17187 /*   indicator (to eliminate a timeout error message): */
17188 /*   ASCII 4. */
17189 
17190 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17191 /*  280 FORMAT (A1) */
17192 
17193 /* No error encountered. */
17194 
17195     *ier = 0;
17196     return 0;
17197 
17198 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17199 
17200 L11:
17201     *ier = 1;
17202     return 0;
17203 
17204 /* Invalid input parameter ELAT, ELON, or A. */
17205 
17206 L12:
17207     *ier = 2;
17208     return 0;
17209 
17210 /* Error writing to unit LUN. */
17211 
17212 /* L13: */
17213     *ier = 3;
17214     return 0;
17215 } /* vrplot_ */
17216 
17217 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17218         double *rannum)
17219 {
17220     static double x;
17221 
17222 
17223 /*   This routine returns pseudo-random numbers uniformly */
17224 /* distributed in the interval (0,1).  int seeds IX, IY, */
17225 /* and IZ should be initialized to values in the range 1 to */
17226 /* 30,000 before the first call to RANDOM, and should not */
17227 /* be altered between subsequent calls (unless a sequence */
17228 /* of random numbers is to be repeated by reinitializing the */
17229 /* seeds). */
17230 
17231 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17232 /*             and Portable Pseudo-random Number Generator, */
17233 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17234 /*             pp. 188-190. */
17235 
17236     *ix = *ix * 171 % 30269;
17237     *iy = *iy * 172 % 30307;
17238     *iz = *iz * 170 % 30323;
17239     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17240             double) (*iz) / 30323.;
17241     *rannum = x - (int) x;
17242     return 0;
17243 } /* random_ */
17244 
17245 #undef TRUE_
17246 #undef FALSE_
17247 #undef abs
17248 
17249 /*################################################################################################
17250 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17251 ######   You must link the resulting object file with the libraries: #############################
17252 ####################    -lf2c -lm   (in that order)   ############################################
17253 ################################################################################################*/
17254 
17255 
17256 
17257 EMData* Util::mult_scalar(EMData* img, float scalar)
17258 {
17259         ENTERFUNC;
17260         /* Exception Handle */
17261         if (!img) {
17262                 throw NullPointerException("NULL input image");
17263         }
17264         /* ============  output = scalar*input  ================== */
17265 
17266         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17267         size_t size = (size_t)nx*ny*nz;
17268         EMData * img2   = img->copy_head();
17269         float *img_ptr  = img->get_data();
17270         float *img2_ptr = img2->get_data();
17271         for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17272         img2->update();
17273 
17274         if(img->is_complex()) {
17275                 img2->set_complex(true);
17276                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17277         }
17278         EXITFUNC;
17279         return img2;
17280 }
17281 
17282 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17283 {
17284         ENTERFUNC;
17285         /* Exception Handle */
17286         if (!img) {
17287                 throw NullPointerException("NULL input image");
17288         }
17289         /* ==============   output = img + scalar*img1   ================ */
17290 
17291         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17292         size_t size = (size_t)nx*ny*nz;
17293         EMData * img2   = img->copy_head();
17294         float *img_ptr  = img->get_data();
17295         float *img2_ptr = img2->get_data();
17296         float *img1_ptr = img1->get_data();
17297         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17298         img2->update();
17299         if(img->is_complex()) {
17300                 img2->set_complex(true);
17301                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17302         }
17303 
17304         EXITFUNC;
17305         return img2;
17306 }
17307 
17308 EMData* Util::addn_img(EMData* img, EMData* img1)
17309 {
17310         ENTERFUNC;
17311         /* Exception Handle */
17312         if (!img) {
17313                 throw NullPointerException("NULL input image");
17314         }
17315         /* ==============   output = img + img1   ================ */
17316 
17317         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17318         size_t size = (size_t)nx*ny*nz;
17319         EMData * img2   = img->copy_head();
17320         float *img_ptr  = img->get_data();
17321         float *img2_ptr = img2->get_data();
17322         float *img1_ptr = img1->get_data();
17323         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17324         img2->update();
17325         if(img->is_complex()) {
17326                 img2->set_complex(true);
17327                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17328         }
17329 
17330         EXITFUNC;
17331         return img2;
17332 }
17333 
17334 EMData* Util::subn_img(EMData* img, EMData* img1)
17335 {
17336         ENTERFUNC;
17337         /* Exception Handle */
17338         if (!img) {
17339                 throw NullPointerException("NULL input image");
17340         }
17341         /* ==============   output = img - img1   ================ */
17342 
17343         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17344         size_t size = (size_t)nx*ny*nz;
17345         EMData * img2   = img->copy_head();
17346         float *img_ptr  = img->get_data();
17347         float *img2_ptr = img2->get_data();
17348         float *img1_ptr = img1->get_data();
17349         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17350         img2->update();
17351         if(img->is_complex()) {
17352                 img2->set_complex(true);
17353                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17354         }
17355 
17356         EXITFUNC;
17357         return img2;
17358 }
17359 
17360 EMData* Util::muln_img(EMData* img, EMData* img1)
17361 {
17362         ENTERFUNC;
17363         /* Exception Handle */
17364         if (!img) {
17365                 throw NullPointerException("NULL input image");
17366         }
17367         /* ==============   output = img * img1   ================ */
17368 
17369         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17370         size_t size = (size_t)nx*ny*nz;
17371         EMData * img2   = img->copy_head();
17372         float *img_ptr  = img->get_data();
17373         float *img2_ptr = img2->get_data();
17374         float *img1_ptr = img1->get_data();
17375         if(img->is_complex()) {
17376                 for (size_t i=0; i<size; i+=2) {
17377                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17378                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17379                 }
17380                 img2->set_complex(true);
17381                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17382         } else {
17383                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17384                 img2->update();
17385         }
17386 
17387         EXITFUNC;
17388         return img2;
17389 }
17390 
17391 EMData* Util::divn_img(EMData* img, EMData* img1)
17392 {
17393         ENTERFUNC;
17394         /* Exception Handle */
17395         if (!img) {
17396                 throw NullPointerException("NULL input image");
17397         }
17398         /* ==============   output = img / img1   ================ */
17399 
17400         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17401         size_t size = (size_t)nx*ny*nz;
17402         EMData * img2   = img->copy_head();
17403         float *img_ptr  = img->get_data();
17404         float *img2_ptr = img2->get_data();
17405         float *img1_ptr = img1->get_data();
17406         if(img->is_complex()) {
17407                 float  sq2;
17408                 for (size_t i=0; i<size; i+=2) {
17409                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17410                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17411                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17412                 }
17413                 img2->set_complex(true);
17414                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17415         } else {
17416                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17417                 img2->update();
17418         }
17419 
17420         EXITFUNC;
17421         return img2;
17422 }
17423 
17424 EMData* Util::divn_filter(EMData* img, EMData* img1)
17425 {
17426         ENTERFUNC;
17427         /* Exception Handle */
17428         if (!img) {
17429                 throw NullPointerException("NULL input image");
17430         }
17431         /* ========= img /= img1 ===================== */
17432 
17433         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17434         size_t size = (size_t)nx*ny*nz;
17435         EMData * img2   = img->copy_head();
17436         float *img_ptr  = img->get_data();
17437         float *img1_ptr = img1->get_data();
17438         float *img2_ptr = img2->get_data();
17439         if(img->is_complex()) {
17440                 for (size_t i=0; i<size; i+=2) {
17441                         if(img1_ptr[i] > 1.e-10f) {
17442                                 img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17443                                 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17444                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17445                 }
17446         } else  throw ImageFormatException("Only Fourier image allowed");
17447 
17448         img->update();
17449 
17450         EXITFUNC;
17451         return img2;
17452 }
17453 
17454 void Util::mul_scalar(EMData* img, float scalar)
17455 {
17456         ENTERFUNC;
17457         /* Exception Handle */
17458         if (!img) {
17459                 throw NullPointerException("NULL input image");
17460         }
17461         /* ============  output = scalar*input  ================== */
17462 
17463         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17464         size_t size = (size_t)nx*ny*nz;
17465         float *img_ptr  =img->get_data();
17466         for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17467         img->update();
17468 
17469         EXITFUNC;
17470 }
17471 
17472 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17473 {
17474         ENTERFUNC;
17475         /* Exception Handle */
17476         if (!img) {
17477                 throw NullPointerException("NULL input image");
17478         }
17479         /* ==============   img += scalar*img1   ================ */
17480 
17481         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17482         size_t size = (size_t)nx*ny*nz;
17483         float *img_ptr  = img->get_data();
17484         float *img1_ptr = img1->get_data();
17485         for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17486         img1->update();
17487 
17488         EXITFUNC;
17489 }
17490 
17491 void Util::add_img(EMData* img, EMData* img1)
17492 {
17493         ENTERFUNC;
17494         /* Exception Handle */
17495         if (!img || !img1) {
17496                 throw NullPointerException("NULL input image");
17497         }
17498         /* ========= img += img1 ===================== */
17499 
17500         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17501         size_t size = (size_t)nx*ny*nz;
17502         float *img_ptr  = img->get_data();
17503         float *img1_ptr = img1->get_data();
17504         for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17505         img->update();
17506 
17507         EXITFUNC;
17508 }
17509 
17510 void Util::add_img_abs(EMData* img, EMData* img1)
17511 {
17512         ENTERFUNC;
17513         /* Exception Handle */
17514         if (!img) {
17515                 throw NullPointerException("NULL input image");
17516         }
17517         /* ========= img += img1 ===================== */
17518 
17519         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17520         size_t size = (size_t)nx*ny*nz;
17521         float *img_ptr  = img->get_data();
17522         float *img1_ptr = img1->get_data();
17523         for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17524         img->update();
17525 
17526         EXITFUNC;
17527 }
17528 
17529 void Util::add_img2(EMData* img, EMData* img1)
17530 {
17531         ENTERFUNC;
17532         /* Exception Handle */
17533         if (!img) {
17534                 throw NullPointerException("NULL input image");
17535         }
17536         /* ========= img += img1**2 ===================== */
17537 
17538         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17539         size_t size = (size_t)nx*ny*nz;
17540         float *img_ptr  = img->get_data();
17541         float *img1_ptr = img1->get_data();
17542         if(img->is_complex()) {
17543                 for (size_t i=0; i<size; i+=2) img_ptr[i] += img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1] ;
17544         } else {
17545                 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17546         }
17547         img->update();
17548 
17549         EXITFUNC;
17550 }
17551 
17552 void Util::sub_img(EMData* img, EMData* img1)
17553 {
17554         ENTERFUNC;
17555         /* Exception Handle */
17556         if (!img) {
17557                 throw NullPointerException("NULL input image");
17558         }
17559         /* ========= img -= img1 ===================== */
17560 
17561         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17562         size_t size = (size_t)nx*ny*nz;
17563         float *img_ptr  = img->get_data();
17564         float *img1_ptr = img1->get_data();
17565         for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17566         img->update();
17567 
17568         EXITFUNC;
17569 }
17570 
17571 void Util::mul_img(EMData* img, EMData* img1)
17572 {
17573         ENTERFUNC;
17574         /* Exception Handle */
17575         if (!img) {
17576                 throw NullPointerException("NULL input image");
17577         }
17578         /* ========= img *= img1 ===================== */
17579 
17580         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17581         size_t size = (size_t)nx*ny*nz;
17582         float *img_ptr  = img->get_data();
17583         float *img1_ptr = img1->get_data();
17584         if(img->is_complex()) {
17585                 for (size_t i=0; i<size; i+=2) {
17586                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17587                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17588                         img_ptr[i]   = tmp;
17589 
17590                 }
17591         } else {
17592                 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17593         }
17594         img->update();
17595 
17596         EXITFUNC;
17597 }
17598 
17599 void Util::div_img(EMData* img, EMData* img1)
17600 {
17601         ENTERFUNC;
17602         /* Exception Handle */
17603         if (!img) {
17604                 throw NullPointerException("NULL input image");
17605         }
17606         /* ========= img /= img1 ===================== */
17607 
17608         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17609         size_t size = (size_t)nx*ny*nz;
17610         float *img_ptr  = img->get_data();
17611         float *img1_ptr = img1->get_data();
17612         if(img->is_complex()) {
17613                 float  sq2;
17614                 for (size_t i=0; i<size; i+=2) {
17615                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17616                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17617                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17618                         img_ptr[i]   = tmp;
17619                 }
17620         } else {
17621                 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17622         }
17623         img->update();
17624 
17625         EXITFUNC;
17626 }
17627 
17628 void Util::div_filter(EMData* img, EMData* img1)
17629 {
17630         ENTERFUNC;
17631         /* Exception Handle */
17632         if (!img) {
17633                 throw NullPointerException("NULL input image");
17634         }
17635         /* ========= img /= img1 ===================== */
17636 
17637         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17638         size_t size = (size_t)nx*ny*nz;
17639         float *img_ptr  = img->get_data();
17640         float *img1_ptr = img1->get_data();
17641         if(img->is_complex()) {
17642                 for (size_t i=0; i<size; i+=2) {
17643                         if(img1_ptr[i] > 1.e-10f) {
17644                                 img_ptr[i]   /= img1_ptr[i];
17645                                 img_ptr[i+1] /= img1_ptr[i];
17646                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17647                 }
17648         } else throw ImageFormatException("Only Fourier image allowed");
17649 
17650         img->update();
17651 
17652         EXITFUNC;
17653 }
17654 
17655 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17656 
17657 EMData* Util::pack_complex_to_real(EMData* img)
17658 {
17659         ENTERFUNC;
17660         /* Exception Handle */
17661         if (!img) {
17662                 throw NullPointerException("NULL input image");
17663         }
17664         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17665                               output is img packed into real image with Friedel part added,   ================ */
17666 
17667         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17668         int nx = nxo - 2 + img->is_fftodd();
17669         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17670         int nyt, nzt;
17671         int nx2 = nx/2;
17672         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17673         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17674         int nx2p = nx2+nx%2;
17675         int ny2p = ny2+ny%2;
17676         int nz2p = nz2+nz%2;
17677         EMData& power = *(new EMData()); // output image
17678         power.set_size(nx, ny, nz);
17679         power.set_array_offsets(-nx2,-ny2,-nz2);
17680         //img->set_array_offsets(1,1,1);
17681         float *img_ptr  = img->get_data();
17682         for (int iz = 1; iz <= nz; iz++) {
17683                 int jz=iz-1;
17684                 if(jz>=nz2p) jz=jz-nzt;
17685                 for (int iy = 1; iy <= ny; iy++) {
17686                         int jy=iy-1;
17687                         if(jy>=ny2p) jy=jy-nyt;
17688                         for (int ix = 1; ix <= lsd2; ix++) {
17689                                 int jx=ix-1;
17690                                 if(jx>=nx2p) jx=jx-nx;
17691                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17692                         }
17693                 }
17694         }
17695 //  Create the Friedel related half
17696         int  nzb, nze, nyb, nye, nxb, nxe;
17697         nxb =-nx2+(nx+1)%2;
17698         nxe = nx2-(nx+1)%2;
17699         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17700         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17701         for (int iz = nzb; iz <= nze; iz++) {
17702                 for (int iy = nyb; iy <= nye; iy++) {
17703                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17704                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17705                         }
17706                 }
17707         }
17708         if(ny2 != 0)  {
17709                 if(nz2 != 0)  {
17710                         if(nz%2 == 0) {  //if nz even, fix the first slice
17711                                 for (int iy = nyb; iy <= nye; iy++) {
17712                                         for (int ix = nxb; ix <= -1; ix++) {
17713                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17714                                         }
17715                                 }
17716                                 if(ny%2 == 0) {  //if ny even, fix the first line
17717                                         for (int ix = nxb; ix <= -1; ix++) {
17718                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17719                                         }
17720                                 }
17721                         }
17722                 }
17723                 if(ny%2 == 0) {  //if ny even, fix the first column
17724                         for (int iz = nzb; iz <= nze; iz++) {
17725                                 for (int ix = nxb; ix <= -1; ix++) {
17726                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17727                                 }
17728                         }
17729                 }
17730 
17731         }
17732         power.update();
17733         power.set_array_offsets(0,0,0);
17734         return &power;
17735 }
17736 #undef  img_ptr
17737 
17738 float Util::ang_n(float peakp, string mode, int maxrin)
17739 {
17740     if (mode == "f" || mode == "F")
17741         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17742     else
17743         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17744 }
17745 
17746 
17747 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17748 {
17749     float* data = ring->get_data();
17750     float av=0.0;
17751     float sq=0.0;
17752     float nn=0.0;
17753     int nring = numr.size()/3;
17754     for( int i=0; i < nring; ++i )
17755     {
17756         int numr3i = numr[3*i+2];
17757         int numr2i = numr[3*i+1]-1;
17758         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17759         for( int j=0; j < numr3i; ++j )
17760         {
17761             int jc = numr2i+j;
17762             av += data[jc] * w;
17763             sq += data[jc] * data[jc] * w;
17764             nn += w;
17765         }
17766     }
17767 
17768     float avg = av/nn;
17769     float sgm = sqrt( (sq-av*av/nn)/nn );
17770     size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17771     for( size_t i=0; i < n; ++i )
17772     {
17773         data[i] -= avg;
17774         data[i] /= sgm;
17775     }
17776 
17777     ring->update();
17778 }
17779 
17780 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17781                 float xrng, float yrng, float step, string mode,
17782                 vector<int>numr, float cnx, float cny) {
17783 
17784     // Manually extract.
17785 /*    vector< EMAN::EMData* > crefim;
17786     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17787     crefim.reserve(crefim_len);
17788 
17789     for(std::size_t i=0;i<crefim_len;i++) {
17790         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17791         crefim.push_back(proxy());
17792     }
17793 */
17794 
17795         size_t crefim_len = crefim.size();
17796 
17797         int   ky = int(2*yrng/step+0.5)/2;
17798         int   kx = int(2*xrng/step+0.5)/2;
17799         int   iref, nref=0, mirror=0;
17800         float iy, ix, sx=0, sy=0;
17801         float peak = -1.0E23f;
17802         float ang=0.0f;
17803         for (int i = -ky; i <= ky; i++) {
17804                 iy = i * step ;
17805                 for (int j = -kx; j <= kx; j++) {
17806                         ix = j*step ;
17807                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17808 
17809                         Normalize_ring( cimage, numr );
17810 
17811                         Frngs(cimage, numr);
17812                         //  compare with all reference images
17813                         // for iref in xrange(len(crefim)):
17814                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17815                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17816                                 double qn = retvals["qn"];
17817                                 double qm = retvals["qm"];
17818                                 if(qn >= peak || qm >= peak) {
17819                                         sx = -ix;
17820                                         sy = -iy;
17821                                         nref = iref;
17822                                         if (qn >= qm) {
17823                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17824                                                 peak = static_cast<float>(qn);
17825                                                 mirror = 0;
17826                                         } else {
17827                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17828                                                 peak = static_cast<float>(qm);
17829                                                 mirror = 1;
17830                                         }
17831                                 }
17832                         }  delete cimage; cimage = 0;
17833                 }
17834         }
17835         float co, so, sxs, sys;
17836         co = static_cast<float>( cos(ang*pi/180.0) );
17837         so = static_cast<float>( -sin(ang*pi/180.0) );
17838         sxs = sx*co - sy*so;
17839         sys = sx*so + sy*co;
17840         vector<float> res;
17841         res.push_back(ang);
17842         res.push_back(sxs);
17843         res.push_back(sys);
17844         res.push_back(static_cast<float>(mirror));
17845         res.push_back(static_cast<float>(nref));
17846         res.push_back(peak);
17847         return res;
17848 }
17849 
17850 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17851                 float xrng, float yrng, float step, string mode,
17852                 vector<int>numr, float cnx, float cny) {
17853 
17854         size_t crefim_len = crefim.size();
17855 
17856         int   ky = int(2*yrng/step+0.5)/2;
17857         int   kx = int(2*xrng/step+0.5)/2;
17858         float iy, ix;
17859         vector<float> peak(crefim_len*5, -1.0e23f);
17860         for (int i = -ky; i <= ky; i++) {
17861                 iy = i * step ;
17862                 for (int j = -kx; j <= kx; j++) {
17863                         ix = j*step ;
17864                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17865                         Normalize_ring( cimage, numr );
17866                         Frngs(cimage, numr);
17867                         for (int iref = 0; iref < (int)crefim_len; iref++) {
17868                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17869                                 double qn = retvals["qn"];
17870                                 double qm = retvals["qm"];
17871                                 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17872                                         if (qn >= qm) {
17873                                                 peak[iref*5] = static_cast<float>(qn);
17874                                                 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17875                                                 peak[iref*5+2] = -ix;
17876                                                 peak[iref*5+3] = -iy;
17877                                                 peak[iref*5+4] = 0;
17878                                         } else {
17879                                                 peak[iref*5] = static_cast<float>(qm);
17880                                                 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17881                                                 peak[iref*5+2] = -ix;
17882                                                 peak[iref*5+3] = -iy;
17883                                                 peak[iref*5+4] = 1;
17884                                         }
17885                                 }
17886                         }  delete cimage; cimage = 0;
17887                 }
17888         }
17889         for (int iref = 0; iref < (int)crefim_len; iref++) {
17890                 float ang = peak[iref*5+1];
17891                 float sx = peak[iref*5+2];
17892                 float sy = peak[iref*5+3];
17893                 float co =  cos(ang*pi/180.0);
17894                 float so = -sin(ang*pi/180.0);
17895                 float sxs = sx*co - sy*so;
17896                 float sys = sx*so + sy*co;
17897                 peak[iref*5+2] = sxs;
17898                 peak[iref*5+3] = sys;
17899         }
17900         return peak;
17901 }
17902 
17903 struct assign_groups_comparator {
17904         const float * values;
17905         bool operator() (int i,int j) { return (values[i] > values[j]); }
17906         assign_groups_comparator(const float * v) : values(v) {}
17907 };
17908 
17909 vector<int> Util::assign_groups(std::string matrix_address, int nref, int nima)
17910 {
17911         const float * matrix;
17912         { // convert memory address sent as string to pointer to float
17913                 size_t addr = 0;
17914                 for ( std::string::const_iterator i = matrix_address.begin();  i != matrix_address.end();  ++i ) {
17915                         int digit = *i - '0';
17916                         addr *= 10;
17917                         addr += digit;
17918                 }
17919                 matrix = reinterpret_cast<float*>(addr);
17920         }
17921 
17922         int kt = nref;
17923         unsigned int maxasi = nima/nref;
17924         vector< vector<int> > id_list(nref);
17925         int group, ima;
17926 
17927         {
17928                 int begin = 0;
17929                 // allocate and sort vector of indexes
17930                 std::vector<int> dd(nref*nima);
17931                 for (int i=0; i<nref*nima; i++)  {
17932                         dd[i] = i;
17933                 }
17934                 assign_groups_comparator comparator(matrix);
17935                 sort(dd.begin(), dd.end(), comparator);
17936                 // main loop
17937                 std::vector<bool> del_row(nref, false);
17938                 std::vector<bool> del_column(nima, false);
17939                 while (kt > 0) {
17940                         bool flag = true;
17941                         while (flag) {
17942                                 int l = dd[begin];
17943                                 group = l/nima;
17944                                 ima = l%nima;
17945                                 if (del_column[ima] || del_row[group]) begin++;
17946                                 else flag = false;
17947                         }
17948 
17949                         id_list[group].push_back(ima);
17950                         if (kt > 1) {
17951                                 if (id_list[group].size() < maxasi) group = -1;
17952                                 else kt -= 1;
17953                         } else {
17954                                 if (id_list[group].size() < maxasi+nima%nref) group = -1;
17955                                 else kt -= 1;
17956                         }
17957                         del_column[ima] = true;
17958                         if (group != -1) {
17959                                 del_row[group] = true;
17960                         }
17961                 }
17962         }
17963 
17964         vector<int> id_list_1; 
17965         for (int iref=0; iref<nref; iref++)
17966                 for (unsigned int im=0; im<maxasi; im++)
17967                         id_list_1.push_back(id_list[iref][im]);
17968         for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
17969                         id_list_1.push_back(id_list[group][im]);
17970         id_list_1.push_back(group);
17971 
17972         return id_list_1;
17973 }
17974 
17975 int Util::nearest_ang(const vector<float>& vecref, float x, float y, float z) {
17976         float best_v = -1.0f;
17977         int best_i = -1;
17978         
17979         for (unsigned int i=0; i<vecref.size()/3; i++) {
17980                 float v = abs(vecref[i*3]*x+vecref[i*3+1]*y+vecref[i*3+2]*z);
17981                 if (v > best_v) {
17982                         best_v = v;
17983                         best_i = i;
17984                 }
17985         }
17986         return best_i;
17987 }
17988 
17989 struct d_ang {
17990         float d;
17991         int i;
17992         int mirror;
17993         d_ang(float _d, int _i, int _m):d(_d), i(_i), mirror(_m) {}
17994         bool operator<(const d_ang& a) const { return d < a.d || (d == a.d && i < a.i); }
17995 };
17996 
17997 vector<int> Util::assign_projangles(const vector<float>& projangles, const vector<float>& refangles) {
17998         int nref = refangles.size()/2;
17999         int nproj = projangles.size()/2;
18000         vector<int> asg(nproj);
18001         vector<float> vecref(nref*3);
18002         for (int i=0; i<nref; i++)
18003                 getvec(refangles[i*2], refangles[i*2+1], vecref[i*3], vecref[i*3+1], vecref[i*3+2]);
18004         for (int i=0; i<nproj; i++) {
18005                 float x, y, z;
18006                 getvec(projangles[i*2], projangles[i*2+1], x, y, z);
18007                 asg[i] = nearest_ang(vecref, x, y, z);
18008         }
18009         return asg;
18010 }
18011 
18012 
18013 vector<int> Util::nearestk_to_refdir(const vector<float>& projangles, const vector<float>& refangles, const int howmany) {
18014         int nref = refangles.size()/2;
18015         int nproj = projangles.size()/2;
18016         vector<int> asg(howmany*nref);
18017         vector<float> vecproj(nproj*3);
18018         for (int i=0; i<nproj; i++)
18019                 getvec(projangles[i*2], projangles[i*2+1], vecproj[i*3], vecproj[i*3+1], vecproj[i*3+2]);
18020 
18021 
18022         vector<bool> taken(nproj);
18023         for (int k=0; k<nref; k++) {
18024                 for (int i=0; i<nproj; i++)  taken[i] = true;
18025                 float x, y, z;
18026                 getvec(refangles[k*2], refangles[k*2+1], x, y, z);
18027                 for (int h=0; h<howmany; h++) {
18028                         float best_v = -1.0f;
18029                         int best_i = -1;
18030                         for (int i=0; i<nproj; i++) {
18031                                 if( taken[i] ) {
18032                                         float v = abs(vecproj[i*3]*x+vecproj[i*3+1]*y+vecproj[i*3+2]*z);
18033                                         if (v > best_v) {
18034                                                 best_v = v;
18035                                                 best_i = i;
18036                                         }
18037                                 }
18038                         }
18039                         asg[k*howmany + h] = best_i;
18040                         taken[best_i] = false;
18041                 }
18042         }
18043         return asg;
18044 }
18045 
18046 
18047 vector<int> Util::group_proj_by_phitheta(const vector<float>& projangles, const vector<float>& ref_ang, const int img_per_grp) {
18048         float c = 100.0;
18049         int L = max(100, img_per_grp);
18050         int N = projangles.size()/2;
18051 
18052         int sz = ref_ang.size();
18053         int nref1 = ref_ang[sz-4];
18054         int nref2 = ref_ang[sz-3];
18055         int nref3 = ref_ang[sz-2];
18056         int nref4 = ref_ang[sz-1];
18057         int nref;
18058 
18059         set<int> pt;
18060         for (int i=0; i<N; i++) pt.insert(i);
18061         vector<float> v(N*3, 0.0f);
18062         for (int i=0; i<N; i++) 
18063                 getvec(projangles[i*2], projangles[i*2+1], v[i*3], v[i*3+1], v[i*3+2], 1);
18064 
18065         int previous_group = -1;
18066         int previous_zone = 5;
18067         int max_group = 0;
18068         vector<float> ref_ang_list;
18069         vector<float> diff_table;
18070         map<int, int> diff_table_index;
18071         vector<int> proj_list;
18072         vector<int> sg;
18073         vector<int> remain_index;
18074         vector<int> asg;
18075         int mirror;
18076         for (int grp=0; grp<N/img_per_grp; grp++) {
18077                 int N_remain = N-grp*img_per_grp;
18078                 assert(N_remain == static_cast<int>(pt.size()));
18079                 if (N_remain >= nref4*L) {
18080                         if (previous_zone > 4) {
18081                                 ref_ang_list.resize(nref4*2);
18082                                 for (int i=0; i<nref4*2; i++)  ref_ang_list[i] = ref_ang[(nref1+nref2+nref3)*2+i];
18083                                 nref = nref4;
18084                                 previous_group = -1;
18085                                 previous_zone = 4;
18086                         }
18087                 } else if (N_remain >= nref3*L) {
18088                         if (previous_zone > 3) {
18089                                 ref_ang_list.resize(nref3*2);
18090                                 for (int i=0; i<nref3*2; i++)  ref_ang_list[i] = ref_ang[(nref1+nref2)*2+i];
18091                                 nref = nref3;
18092                                 previous_group = -1;
18093                                 previous_zone = 3;
18094                         }
18095                 } else if (N_remain >= nref2*L) {
18096                         if (previous_zone > 2) {
18097                                 ref_ang_list.resize(nref2*2);
18098                                 for (int i=0; i<nref2*2; i++)  ref_ang_list[i] = ref_ang[nref1*2+i];
18099                                 nref = nref2;
18100                                 previous_group = -1;
18101                                 previous_zone = 2;
18102                         }
18103                 } else if (N_remain >= nref1*L) {
18104                         if (previous_zone > 1) {
18105                                 ref_ang_list.resize(nref1*2);
18106                                 for (int i=0; i<nref1*2; i++)  ref_ang_list[i] = ref_ang[i];
18107                                 nref = nref1;
18108                                 previous_group = -1;
18109                                 previous_zone = 1;
18110                         }
18111                 } else if (previous_zone > 0) {
18112                         previous_group = -1;
18113                         previous_zone = 0;
18114                 }
18115 
18116                 vector<int> index;
18117                 if (N_remain >=  nref1*L) {
18118                         if (previous_group == -1) { // which means it just changed zone
18119                                 vector<float> proj_ang_list(N_remain*2, 0.0f);
18120                                 remain_index.resize(N_remain);
18121                                 int l = 0;
18122                                 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18123                                         remain_index[l] = (*si);
18124                                         proj_ang_list[l*2] = projangles[(*si)*2];
18125                                         proj_ang_list[l*2+1] = projangles[(*si)*2+1];
18126                                         l++; 
18127                                 }
18128                                 assert(N_remain == l);
18129                                 asg = assign_projangles(proj_ang_list, ref_ang_list);
18130                                 sg.resize(nref);
18131                                 for (int i=0; i<nref; i++) sg[i] = 0;
18132                                 for (int i=0; i<N_remain; i++)  sg[asg[i]]++;
18133                         }
18134                         int max_group_size = 0;
18135                         for (int i=0; i<nref; i++)
18136                                 if (sg[i] > max_group_size)     { max_group_size = sg[i]; max_group = i; }
18137                         for (unsigned int i=0; i<remain_index.size(); i++)
18138                                 if (asg[i] == max_group)  index.push_back(remain_index[i]);
18139                 } else {
18140                         for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) 
18141                                 index.push_back(*si);
18142                         max_group = 0;
18143                 }
18144                         
18145                 int Nn = index.size();
18146                 if (max_group != previous_group) {
18147                         diff_table.resize(Nn*Nn);
18148                         diff_table_index.clear();
18149                         for (int i=0; i<Nn-1; i++)
18150                                 for (int j=i+1; j<Nn; j++) {
18151                                         float diff = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2], v[index[j]*3], v[index[j]*3+1], v[index[j]*3+2], mirror);
18152                                         float q = exp(-c*pow(diff/180.0f*static_cast<float>(M_PI), 2.0f));
18153                                         diff_table[i*Nn+j] = q;
18154                                         diff_table[j*Nn+i] = q;
18155                                 }
18156                         for (int i=0; i<Nn; i++)  {
18157                                 diff_table[i*Nn+i] = 0.0f;      // diagonal values
18158                                 diff_table_index[index[i]] = i; 
18159                         }
18160                         previous_group = max_group;
18161                 } 
18162 
18163                 int diff_table_size = static_cast<int>(sqrt((float)diff_table.size())+0.5f);
18164                 float max_density = -1;
18165                 int max_density_i = -1;
18166                 for (int i=0; i<Nn; i++) {
18167                         float s = 0.0f;
18168                         int z = diff_table_index[index[i]];
18169                         for (int j=0; j<diff_table_size; j++)  s += diff_table[z*diff_table_size+j];
18170                         if (s > max_density) {
18171                                 max_density = s;
18172                                 max_density_i = i;
18173                         }
18174                 }
18175 
18176                 vector<d_ang> dang(Nn, d_ang(0.0, 0, 0));
18177                 for (int i=0; i<Nn; i++) {
18178                         dang[i].d = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2], 
18179                           v[index[max_density_i]*3], v[index[max_density_i]*3+1], v[index[max_density_i]*3+2], mirror);
18180                         dang[i].mirror = mirror;
18181                         dang[i].i = i;
18182                 }
18183                 dang[max_density_i].d = -1;
18184                 sort(dang.begin(), dang.end());         
18185 
18186                 for (int i=0; i<img_per_grp; i++) {
18187                         int idd = index[dang[i].i];
18188                         mirror = dang[i].mirror;
18189                         for (unsigned int j=0; j<remain_index.size(); j++)
18190                                 if (idd == remain_index[j]) asg[j] = -1;
18191                         for (int j=0; j<diff_table_size; j++) {
18192                                 diff_table[diff_table_index[idd]*diff_table_size+j] = 0.0f;
18193                                 diff_table[diff_table_index[idd]+diff_table_size*j] = 0.0f;
18194                         }
18195                         proj_list.push_back(mirror*idd);
18196                         pt.erase(idd);
18197                 }
18198                 sg[max_group] -= img_per_grp;
18199         }
18200         for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18201                 proj_list.push_back(*si);
18202         }
18203         return proj_list;
18204 }
18205 
18206 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18207                 float xrng, float yrng, float step, string mode,
18208                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18209 
18210     // Manually extract.
18211 /*    vector< EMAN::EMData* > crefim;
18212     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18213     crefim.reserve(crefim_len);
18214 
18215     for(std::size_t i=0;i<crefim_len;i++) {
18216         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18217         crefim.push_back(proxy());
18218     }
18219 */
18220 
18221         size_t crefim_len = crefim.size();
18222 
18223         int   ky = int(2*yrng/step+0.5)/2;
18224         int   kx = int(2*xrng/step+0.5)/2;
18225         int   iref, nref=0, mirror=0;
18226         float iy, ix, sx=0, sy=0;
18227         float peak = -1.0E23f;
18228         float ang=0.0f;
18229         for (int i = -ky; i <= ky; i++) {
18230                 iy = i * step ;
18231                 for (int j = -kx; j <= kx; j++) {
18232                         ix = j*step ;
18233                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18234 
18235                         Normalize_ring( cimage, numr );
18236 
18237                         Frngs(cimage, numr);
18238                         //  compare with all reference images
18239                         // for iref in xrange(len(crefim)):
18240                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18241                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18242                                 double qn = retvals["qn"];
18243                                 double qm = retvals["qm"];
18244                                 if(qn >= peak || qm >= peak) {
18245                                         sx = -ix;
18246                                         sy = -iy;
18247                                         nref = iref;
18248                                         if (qn >= qm) {
18249                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18250                                                 peak = static_cast<float>(qn);
18251                                                 mirror = 0;
18252                                         } else {
18253                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18254                                                 peak = static_cast<float>(qm);
18255                                                 mirror = 1;
18256                                         }
18257                                 }
18258                         }  delete cimage; cimage = 0;
18259                 }
18260         }
18261         float co, so, sxs, sys;
18262         co = static_cast<float>( cos(ang*pi/180.0) );
18263         so = static_cast<float>( -sin(ang*pi/180.0) );
18264         sxs = sx*co - sy*so;
18265         sys = sx*so + sy*co;
18266         vector<float> res;
18267         res.push_back(ang);
18268         res.push_back(sxs);
18269         res.push_back(sys);
18270         res.push_back(static_cast<float>(mirror));
18271         res.push_back(static_cast<float>(nref));
18272         res.push_back(peak);
18273         return res;
18274 }
18275 
18276 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18277                 float xrng, float yrng, float step, string mode,
18278                 vector< int >numr, float cnx, float cny) {
18279 
18280     // Manually extract.
18281 /*    vector< EMAN::EMData* > crefim;
18282     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18283     crefim.reserve(crefim_len);
18284 
18285     for(std::size_t i=0;i<crefim_len;i++) {
18286         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18287         crefim.push_back(proxy());
18288     }
18289 */
18290         size_t crefim_len = crefim.size();
18291 
18292         int   ky = int(2*yrng/step+0.5)/2;
18293         int   kx = int(2*xrng/step+0.5)/2;
18294         int   iref, nref=0;
18295         float iy, ix, sx=0, sy=0;
18296         float peak = -1.0E23f;
18297         float ang=0.0f;
18298         for (int i = -ky; i <= ky; i++) {
18299                 iy = i * step ;
18300                 for (int j = -kx; j <= kx; j++) {
18301                         ix = j*step ;
18302                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18303                         Frngs(cimage, numr);
18304                         //  compare with all reference images
18305                         // for iref in xrange(len(crefim)):
18306                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18307                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18308                                 double qn = retvals["qn"];
18309                                 if(qn >= peak) {
18310                                         sx = -ix;
18311                                         sy = -iy;
18312                                         nref = iref;
18313                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18314                                         peak = static_cast<float>(qn);
18315                                 }
18316                         }  delete cimage; cimage = 0;
18317                 }
18318         }
18319         float co, so, sxs, sys;
18320         co = static_cast<float>( cos(ang*pi/180.0) );
18321         so = static_cast<float>( -sin(ang*pi/180.0) );
18322         sxs = sx*co - sy*so;
18323         sys = sx*so + sy*co;
18324         vector<float> res;
18325         res.push_back(ang);
18326         res.push_back(sxs);
18327         res.push_back(sys);
18328         res.push_back(static_cast<float>(nref));
18329         res.push_back(peak);
18330         return res;
18331 }
18332 
18333 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18334                 float xrng, float yrng, float step, float ant, string mode,
18335                 vector<int>numr, float cnx, float cny) {
18336 
18337     // Manually extract.
18338 /*    vector< EMAN::EMData* > crefim;
18339     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18340     crefim.reserve(crefim_len);
18341 
18342     for(std::size_t i=0;i<crefim_len;i++) {
18343         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18344         crefim.push_back(proxy());
18345     }
18346 */
18347         size_t crefim_len = crefim.size();
18348         const float qv = static_cast<float>( pi/180.0 );
18349 
18350         Transform * t = image->get_attr("xform.projection");
18351         Dict d = t->get_params("spider");
18352         if(t) {delete t; t=0;}
18353         float phi = d["phi"];
18354         float theta = d["theta"];
18355         int   ky = int(2*yrng/step+0.5)/2;
18356         int   kx = int(2*xrng/step+0.5)/2;
18357         int   iref, nref=0, mirror=0;
18358         float iy, ix, sx=0, sy=0;
18359         float peak = -1.0E23f;
18360         float ang=0.0f;
18361         float imn1 = sin(theta*qv)*cos(phi*qv);
18362         float imn2 = sin(theta*qv)*sin(phi*qv);
18363         float imn3 = cos(theta*qv);
18364         vector<float> n1(crefim_len);
18365         vector<float> n2(crefim_len);
18366         vector<float> n3(crefim_len);
18367         for ( iref = 0; iref < (int)crefim_len; iref++) {
18368                         n1[iref] = crefim[iref]->get_attr("n1");
18369                         n2[iref] = crefim[iref]->get_attr("n2");
18370                         n3[iref] = crefim[iref]->get_attr("n3");
18371         }
18372         for (int i = -ky; i <= ky; i++) {
18373             iy = i * step ;
18374             for (int j = -kx; j <= kx; j++) {
18375                 ix = j*step;
18376                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18377                 Normalize_ring( cimage, numr );
18378                 Frngs(cimage, numr);
18379                 //  compare with all reference images
18380                 // for iref in xrange(len(crefim)):
18381                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18382                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18383                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18384                                 double qn = retvals["qn"];
18385                                 double qm = retvals["qm"];
18386                                 if(qn >= peak || qm >= peak) {
18387                                         sx = -ix;
18388                                         sy = -iy;
18389                                         nref = iref;
18390                                         if (qn >= qm) {
18391                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18392                                                 peak = static_cast<float>( qn );
18393                                                 mirror = 0;
18394                                         } else {
18395                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18396                                                 peak = static_cast<float>( qm );
18397                                                 mirror = 1;
18398                                         }
18399                                 }
18400                         }
18401                 }  delete cimage; cimage = 0;
18402             }
18403         }
18404         float co, so, sxs, sys;
18405         if(peak == -1.0E23) {
18406                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18407                 nref = -1;
18408         } else {
18409                 co =  cos(ang*qv);
18410                 so = -sin(ang*qv);
18411                 sxs = sx*co - sy*so;
18412                 sys = sx*so + sy*co;
18413         }
18414         vector<float> res;
18415         res.push_back(ang);
18416         res.push_back(sxs);
18417         res.push_back(sys);
18418         res.push_back(static_cast<float>(mirror));
18419         res.push_back(static_cast<float>(nref));
18420         res.push_back(peak);
18421         return res;
18422 }
18423 
18424 vector<float> Util::hans(EMData* image, const vector< EMData* >& crefim,
18425                 float xrng, float yrng, float step, float ant, string mode,
18426                 vector<int>numr, float cnx, float cny) {
18427 
18428     // Manually extract.
18429 /*    vector< EMAN::EMData* > crefim;
18430     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18431     crefim.reserve(crefim_len);
18432 
18433     for(std::size_t i=0;i<crefim_len;i++) {
18434         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18435         crefim.push_back(proxy());
18436     }
18437 */
18438         size_t crefim_len = crefim.size();
18439         const float qv = static_cast<float>( pi/180.0 );
18440 
18441         //Transform * t = image->get_attr("xform.projection");
18442         float previousmax = image->get_attr("previousmax");
18443         //Dict d = t->get_params("spider");
18444         //if(t) {delete t; t=0;}
18445         //float phi   = d["phi"];
18446         //float theta = d["theta"];
18447         int   ky = int(2*yrng/step+0.5)/2;
18448         int   kx = int(2*xrng/step+0.5)/2;
18449         int   iref, nref=0, mirror=0;
18450         float iy, ix, sx=0, sy=0;
18451         float peak = previousmax;
18452         float ang=0.0f;
18453         
18454         vector< vector<EMData*> > cimages( 2*ky+1, vector<EMData*>(2*kx+1) );
18455         
18456         for (int i = -ky; i <= ky; i++) {
18457             iy = i * step ;
18458             for (int j = -kx; j <= kx; j++) {
18459                         ix = j*step;
18460                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18461                         Normalize_ring( cimage, numr );
18462                         Frngs(cimage, numr);
18463                         cimages[i+ky][j+kx] = cimage;                   
18464                 }
18465         }
18466         
18467         vector<unsigned> listr(crefim_len);
18468         for (unsigned i = 0; i < crefim_len; ++i) listr[i] = i;
18469         for (unsigned i = 0; i < crefim_len; ++i) {
18470                 unsigned r = Util::get_irand(0,crefim_len-1);
18471                 swap( listr[r], listr[i] );
18472         }
18473 
18474         bool found_better = false;
18475         for ( int tiref = 0;  (tiref < (int)crefim_len) && (! found_better); tiref++) {
18476                 iref = listr[tiref];
18477                 float best_for_ref = -1.0E23f;
18478                 for (int i = -ky; i <= ky; i++) {
18479                         iy = i * step ;
18480                         for (int j = -kx; j <= kx; j++) {
18481                                 ix = j*step;
18482                                 EMData* cimage = cimages[i+ky][j+kx];
18483                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18484                                 double qn = retvals["qn"];
18485                                 double qm = retvals["qm"];
18486                                 if (qn >= best_for_ref || qm >= best_for_ref) {
18487                                         sx = -ix;
18488                                         sy = -iy;
18489                                         nref = iref;
18490                                         if (qn >= qm) {
18491                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18492                                                 peak = static_cast<float>( qn );
18493                                                 mirror = 0;
18494                                         } else {
18495                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18496                                                 peak = static_cast<float>( qm );
18497                                                 mirror = 1;
18498                                         }
18499                                         best_for_ref = peak;
18500                                         //cout <<"  iref "<<iref<<"  tiref "<<tiref<<"   "<<previousmax<<"   "<<qn<<"   "<<qm<<endl;
18501                                 }
18502                         }
18503                 }
18504                 found_better = (best_for_ref >= previousmax);
18505         }
18506         
18507         for (unsigned i = 0; i < cimages.size(); ++i) {
18508                 for (unsigned j = 0; j < cimages[i].size(); ++j) {
18509                         delete cimages[i][j];
18510                         cimages[i][j] = NULL;
18511                 }
18512         }
18513         
18514         float sxs, sys;
18515         if (found_better) {
18516                 float co =  cos(ang*qv);
18517                 float so = -sin(ang*qv);
18518                 sxs = sx*co - sy*so;
18519                 sys = sx*so + sy*co;
18520                 image->set_attr("previousmax",peak);
18521         } else {
18522                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18523                 nref = -1;
18524                 peak = previousmax;
18525         }
18526         vector<float> res;
18527         res.push_back(ang);
18528         res.push_back(sxs);
18529         res.push_back(sys);
18530         res.push_back(static_cast<float>(mirror));
18531         res.push_back(static_cast<float>(nref));
18532         res.push_back(peak);
18533         return res;
18534 }
18535 
18536 
18537 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18538                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18539                 vector<int>numr, float cnx, float cny) {
18540 
18541     // Manually extract.
18542 /*    vector< EMAN::EMData* > crefim;
18543     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18544     crefim.reserve(crefim_len);
18545 
18546     for(std::size_t i=0;i<crefim_len;i++) {
18547         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18548         crefim.push_back(proxy());
18549     }
18550 */
18551         size_t crefim_len = crefim.size();
18552         const float qv = static_cast<float>(pi/180.0);
18553 
18554         Transform* t = image->get_attr("xform.projection");
18555         Dict d = t->get_params("spider");
18556         if(t) {delete t; t=0;}
18557         float phi = d["phi"];
18558         float theta = d["theta"];
18559         float psi = d["psi"];
18560         int ky = int(2*yrng/step+0.5)/2;
18561         int kx = int(2*xrng/step+0.5)/2;
18562         int iref, nref = 0, mirror = 0;
18563         float iy, ix, sx = 0, sy = 0;
18564         float peak = -1.0E23f;
18565         float ang = 0.0f;
18566         float imn1 = sin(theta*qv)*cos(phi*qv);
18567         float imn2 = sin(theta*qv)*sin(phi*qv);
18568         float imn3 = cos(theta*qv);
18569         vector<float> n1(crefim_len);
18570         vector<float> n2(crefim_len);
18571         vector<float> n3(crefim_len);
18572         for (iref = 0; iref < (int)crefim_len; iref++) {
18573                         n1[iref] = crefim[iref]->get_attr("n1");
18574                         n2[iref] = crefim[iref]->get_attr("n2");
18575                         n3[iref] = crefim[iref]->get_attr("n3");
18576         }
18577         bool nomirror = (theta<90.0) || (theta==90.0);
18578         if (!nomirror) {
18579                 phi = fmod(phi+540.0f, 360.0f);
18580                 theta = 180-theta;
18581                 psi = fmod(540.0f-psi, 360.0f);
18582         } else { psi = fmod(360.0f-psi, 360.0f); }
18583         for (int i = -ky; i <= ky; i++) {
18584             iy = i * step ;
18585             for (int j = -kx; j <= kx; j++) {
18586                 ix = j*step;
18587                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18588 
18589                 Normalize_ring(cimage, numr);
18590 
18591                 Frngs(cimage, numr);
18592                 //  compare with all reference images
18593                 // for iref in xrange(len(crefim)):
18594                 for (iref = 0; iref < (int)crefim_len; iref++) {
18595                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18596                                 float refpsi = crefim[iref]->get_attr("psi");
18597                                 if (nomirror) {
18598                                 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi+refpsi, 360.0), 0, psi_max);
18599                                 double qn = retvals["qn"];
18600                                 if (qn >= peak) {
18601                                                 sx = -ix;
18602                                                 sy = -iy;
18603                                                 nref = iref;
18604                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18605                                                 peak = static_cast<float>(qn);
18606                                                 mirror = 0;
18607                                         }
18608                                 } else {
18609                                 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi-refpsi, 360.0), 1, psi_max);
18610                                 double qn = retvals["qn"];
18611                                 if (qn >= peak) {
18612                                                 sx = -ix;
18613                                                 sy = -iy;
18614                                                 nref = iref;
18615                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18616                                                 peak = static_cast<float>(qn);
18617                                                 mirror = 1;
18618                                         }
18619                                 }
18620                     }
18621                 }  delete cimage; cimage = 0;
18622             }
18623         }
18624         float co, so, sxs, sys;
18625         if(peak == -1.0E23) {
18626                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18627                 nref = -1;
18628         } else {
18629                 co =  cos(ang*qv);
18630                 so = -sin(ang*qv);
18631                 sxs = sx*co - sy*so;
18632                 sys = sx*so + sy*co;
18633         }
18634         vector<float> res;
18635         res.push_back(ang);
18636         res.push_back(sxs);
18637         res.push_back(sys);
18638         res.push_back(static_cast<float>(mirror));
18639         res.push_back(static_cast<float>(nref));
18640         res.push_back(peak);
18641         return res;
18642 }
18643 
18644 
18645 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18646                 float xrng, float yrng, float step, float psi_max, string mode,
18647                 vector<int>numr, float cnx, float cny, int ynumber) {
18648         
18649         size_t crefim_len = crefim.size();
18650 
18651         int   iref, nref=0, mirror=0;
18652         float iy, ix, sx=0, sy=0;
18653         float peak = -1.0E23f;
18654         float ang=0.0f;
18655         int   kx = int(2*xrng/step+0.5)/2;
18656         //if ynumber==-1, use the old code which process x and y direction equally.
18657         //if ynumber is given, it should be even. We need to check whether it is zero
18658 
18659         int ky;
18660         float stepy;
18661         int kystart;
18662         
18663         if (ynumber == -1) {
18664             ky = int(2*yrng/step+0.5)/2;
18665             stepy = step;
18666             kystart = -ky;
18667         } else if(ynumber == 0) {
18668              ky = 0;
18669                  stepy = 0.0f;
18670                  kystart = ky;
18671         } else {
18672             ky = int(ynumber/2);                
18673                 stepy=2*yrng/ynumber;
18674                 kystart = -ky + 1;    
18675         }
18676         //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18677         for (int i = kystart; i <= ky; i++) {
18678                 iy = i * stepy ;
18679                 for (int j = -kx; j <= kx; j++) {
18680                         ix = j*step ;
18681                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18682 
18683                         Normalize_ring( cimage, numr );
18684 
18685                         Frngs(cimage, numr);
18686                         //  compare with all reference images
18687                         // for iref in xrange(len(crefim)):
18688                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18689                                 Dict retvals_0   = Crosrng_psi(crefim[iref], cimage, numr,   0, psi_max);
18690                                 Dict retvals_180 = Crosrng_psi(crefim[iref], cimage, numr, 180, psi_max);
18691                                 double qn_0   = retvals_0["qn"];
18692                                 double qn_180 = retvals_180["qn"];
18693                                 double qm_0   = retvals_0["qm"];
18694                                 double qm_180 = retvals_180["qm"];
18695                                 double qn;
18696                                 double qm;
18697                                 bool   qn_is_zero = false;
18698                                 bool   qm_is_zero = false;
18699 
18700                                 if (qn_0 >= qn_180) {
18701                                         qn = qn_0;
18702                                         qn_is_zero = true;
18703                                 } else {
18704                                         qn = qn_180;
18705                                         qn_is_zero = false; 
18706                                 }
18707 
18708                                 if (qm_0 >= qm_180) {
18709                                         qm = qm_0;
18710                                         qm_is_zero = true;
18711                                 } else {
18712                                         qm = qm_180;
18713                                         qm_is_zero = false; 
18714                                 }
18715 
18716                                 if(qn >= peak || qm >= peak) {
18717                                         sx = -ix;
18718                                         sy = -iy;
18719                                         nref = iref;
18720                                         if (qn >= qm) {
18721                                                 if (qn_is_zero) ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18722                                                 else            ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18723                                                 peak = static_cast<float>(qn);
18724                                                 mirror = 0;
18725                                         } else {
18726                                                 if (qm_is_zero) ang = ang_n(retvals_0["tmt"], mode, numr[numr.size()-1]);
18727                                                 else            ang = ang_n(retvals_180["tmt"], mode, numr[numr.size()-1]);
18728                                                 peak = static_cast<float>(qm);
18729                                                 mirror = 1;
18730                                         }
18731                                 }
18732                         }
18733                         delete cimage; cimage = 0;
18734                 }
18735         }
18736         float co, so, sxs, sys;
18737         co = static_cast<float>( cos(ang*pi/180.0) );
18738         so = static_cast<float>( -sin(ang*pi/180.0) );
18739         sxs = sx*co - sy*so;
18740         sys = sx*so + sy*co;
18741         vector<float> res;
18742         res.push_back(ang);
18743         res.push_back(sxs);
18744         res.push_back(sys);
18745         res.push_back(static_cast<float>(mirror));
18746         res.push_back(static_cast<float>(nref));
18747         res.push_back(peak);
18748         return res;
18749 }
18750 
18751 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18752                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18753                 vector<int>numr, float cnx, float cny, int ynumber, float yrnglocal) {
18754 
18755         size_t crefim_len = crefim.size();
18756 
18757         int   iref, nref=-1, mirror=0;
18758         float iy, ix, sx=0, sy=0;
18759         float peak = -1.0E23f;
18760         float ang=0.0f;
18761         const float qv = static_cast<float>( pi/180.0 );
18762         Transform * t = image->get_attr("xform.projection");
18763         Dict d = t->get_params("spider");
18764         if(t) {delete t; t=0;}
18765         float phi       = d["phi"];
18766         float theta     = d["theta"];
18767         float psi       = d["psi"];
18768         float imn1 = sin(theta*qv)*cos(phi*qv);
18769         float imn2 = sin(theta*qv)*sin(phi*qv);
18770         float imn3 = cos(theta*qv);
18771         vector<float> n1(crefim_len);
18772         vector<float> n2(crefim_len);
18773         vector<float> n3(crefim_len);
18774         for ( iref = 0; iref < (int)crefim_len; iref++) {
18775                 n1[iref] = crefim[iref]->get_attr("n1");
18776                 n2[iref] = crefim[iref]->get_attr("n2");
18777                 n3[iref] = crefim[iref]->get_attr("n3");                
18778         }
18779         float nbrinp;
18780         float nbrinp_mirror;
18781         bool  use_ref;
18782         bool  use_ref_mirror;
18783         int   kx = int(2*xrng/step+0.5)/2;
18784         int ky;
18785         float stepy;
18786         
18787         if (ynumber == 0) {
18788                 ky = 0;
18789         }
18790         else { 
18791         
18792                 if (ynumber > 0) stepy=2*yrng/ynumber;
18793                 else if (ynumber == -1) stepy = step;
18794                 
18795                 if (yrnglocal >= 0.0) {
18796                         ky = int(yrnglocal/stepy);
18797                 }
18798                 else { // search range is not restricted
18799                         if (ynumber > 0) {
18800                                 ky = int(ynumber/2);    
18801                         }
18802                         else{
18803                                 ky = int(2*yrng/stepy+0.5)/2;   
18804                         }       
18805                                         
18806                 }
18807         }
18808         for (int i = -ky; i <= ky; i++) {
18809                         iy = i * stepy ;
18810                         for (int j = -kx; j <= kx; j++)  {
18811                                 ix = j*step ;
18812                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18813                                 Normalize_ring( cimage, numr );
18814                                 
18815                                 Frngs(cimage, numr);
18816                                 //  Compare with All reference images within neighborhood ant
18817                                 // for iref in xrange(len(crefim)):
18818                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18819                                 
18820                                         use_ref = false;
18821                                         use_ref_mirror = false;
18822                                         
18823                                         // inner product of iref's Eulerian angles with that of the data
18824                                         nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18825                                         if (nbrinp >= ant)   use_ref = true;
18826                                         
18827                                         // inner product of the mirror of iref's Eulerian angles with that of the data
18828                                         nbrinp_mirror = n3[iref]*imn3 - n1[iref]*imn1 - n2[iref]*imn2;
18829                                         if (nbrinp_mirror >= ant)  use_ref_mirror = true;
18830                                         if(use_ref || use_ref_mirror){
18831                                                 
18832                                                 Dict retvals;
18833                                                 Dict retvals_mirror;
18834                                                 if (use_ref_mirror == true) {
18835                                                     if ((psi-90.0f) < 90.0f) retvals_mirror = Crosrng_sm_psi(crefim[iref], cimage, numr,   0, 1, psi_max);
18836                                                     else                     retvals_mirror = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
18837                                                 } 
18838                                                 if (use_ref == true) {
18839                                                     if ((psi-90.0f) < 90.0f) retvals = Crosrng_sm_psi(crefim[iref], cimage, numr,   0, 0, psi_max);
18840                                                     else                     retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18841                                                 }
18842                                                 double qn = retvals["qn"];
18843                                                 double qm = retvals_mirror["qn"];
18844                                                 
18845                                                 if(qn >= peak || qm >= peak) {
18846                                                         sx = -ix;
18847                                                         sy = -iy;
18848                                                         nref = iref;
18849                                                         if (qn >= qm){
18850                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18851                                                                 mirror = 0;
18852                                                                 peak = static_cast<float>(qn);
18853                                                         }
18854                                                         else{
18855                                                                 ang = ang_n(retvals_mirror["tot"], mode, numr[numr.size()-1]);
18856                                                                 mirror = 1;
18857                                                                 peak = static_cast<float>(qm);
18858                                                         }
18859                                                         
18860                                                 }
18861                                         }
18862                                 }
18863                                 delete cimage; cimage = 0;
18864                         }
18865         }
18866                                                 
18867         float co, so, sxs, sys;
18868         co = static_cast<float>( cos(ang*pi/180.0) );
18869         so = static_cast<float>( -sin(ang*pi/180.0) );
18870         sxs = sx*co - sy*so;
18871         sys = sx*so + sy*co;
18872         vector<float> res;
18873         res.push_back(ang);
18874         res.push_back(sxs);
18875         res.push_back(sys);
18876         res.push_back(static_cast<float>(mirror));
18877         res.push_back(static_cast<float>(nref));
18878         res.push_back(peak);
18879         return res;
18880 }
18881 
18882 
18883 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
18884                 float xrng, float yrng, float step, float psi_max, string mode,
18885                 vector<int>numr, float cnx, float cny, int ynumber) {
18886 
18887         size_t crefim_len = crefim.size();
18888 
18889         int   iref, nref=0, mirror=0;
18890         float iy, ix, sx=0, sy=0;
18891         float peak = -1.0E23f;
18892         float ang=0.0f;
18893         int   kx = int(2*xrng/step+0.5)/2;
18894         //if ynumber==-1, use the old code which process x and y direction equally.
18895         
18896         int ky;
18897         float stepy;
18898         int kystart;
18899         
18900         if (ynumber == -1) {
18901             ky = int(2*yrng/step+0.5)/2;
18902             stepy = step;
18903             kystart = -ky;
18904         } else if(ynumber == 0) {
18905              ky = 0;
18906                  stepy = 0.0f;
18907                  kystart = ky;
18908         } else {
18909             ky = int(ynumber/2);                
18910                 stepy=2*yrng/ynumber;
18911                 kystart = -ky + 1;    
18912         }
18913 
18914         //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18915         for (int i = kystart; i <= ky; i++) {
18916                 iy = i * stepy ;
18917                 for (int j = -kx; j <= kx; j++) {
18918                         ix = j*step ;
18919                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18920 
18921                         Normalize_ring( cimage, numr );
18922 
18923                         Frngs(cimage, numr);
18924                         //  compare with all reference images
18925                         // for iref in xrange(len(crefim)):
18926                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18927                                 Dict retvals_0   = Crosrng_sm_psi(crefim[iref], cimage, numr,   0, 0, psi_max);
18928                                 Dict retvals_180 = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18929                                 double qn_0   = retvals_0["qn"];
18930                                 double qn_180 = retvals_180["qn"];
18931                                 double qn;
18932                                 bool qn_is_zero = false;
18933                                 
18934                                 if (qn_0 >= qn_180){
18935                                         qn = qn_0;
18936                                         qn_is_zero = true;
18937                                 }
18938                                 else{
18939                                         qn = qn_180;
18940                                         qn_is_zero = false; 
18941                                 }
18942                                         
18943                                 if(qn >= peak) {
18944                                         sx = -ix;
18945                                         sy = -iy;
18946                                         nref = iref;
18947                                         
18948                                         if (qn_is_zero){
18949                                                 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18950                                         }
18951                                         else{
18952                                                 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18953                                         }
18954                                         peak = static_cast<float>(qn);
18955                                         mirror = 0;
18956                                          
18957                                 }
18958                         }
18959                         delete cimage; cimage = 0;
18960                 }
18961         }       
18962         float co, so, sxs, sys;
18963         co = static_cast<float>( cos(ang*pi/180.0) );
18964         so = static_cast<float>( -sin(ang*pi/180.0) );
18965         sxs = sx*co - sy*so;
18966         sys = sx*so + sy*co;
18967         vector<float> res;
18968         res.push_back(ang);
18969         res.push_back(sxs);
18970         res.push_back(sys);
18971         res.push_back(static_cast<float>(mirror));
18972         res.push_back(static_cast<float>(nref));
18973         res.push_back(peak);
18974         return res;
18975 }
18976 
18977 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
18978                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18979                 vector<int>numr, float cnx, float cny, int ynumber, float yrnglocal) {
18980 
18981         size_t crefim_len = crefim.size();
18982         const float qv = static_cast<float>( pi/180.0 );
18983         Transform * t = image->get_attr("xform.projection");
18984         Dict d = t->get_params("spider");
18985         if(t) {delete t; t=0;}
18986         float phi   = d["phi"];
18987         float theta = d["theta"];
18988         float psi       = d["psi"];
18989         float imn1 = sin(theta*qv)*cos(phi*qv);
18990         float imn2 = sin(theta*qv)*sin(phi*qv);
18991         float imn3 = cos(theta*qv);
18992         vector<float> n1(crefim_len);
18993         vector<float> n2(crefim_len);
18994         vector<float> n3(crefim_len);
18995         int   iref, nref=-1, mirror=0;
18996         float iy, ix, sx=0, sy=0;
18997         float peak = -1.0E23f;
18998         float ang  = 0.0f;
18999         int   kx   = int(2*xrng/step+0.5)/2;
19000 
19001         for ( iref = 0; iref < (int)crefim_len; iref++) {
19002                 n1[iref] = crefim[iref]->get_attr("n1");
19003                 n2[iref] = crefim[iref]->get_attr("n2");
19004                 n3[iref] = crefim[iref]->get_attr("n3");
19005         }
19006         
19007         float stepy;
19008         int ky;
19009         
19010         if (ynumber == 0) {
19011                 ky = 0;
19012         }
19013         else { 
19014         
19015                 if (ynumber > 0) stepy=2*yrng/ynumber;
19016                 else if (ynumber == -1) stepy = step;
19017                 
19018                 if (yrnglocal >= 0.0) {
19019                         ky = int(yrnglocal/stepy);
19020                 }
19021                 else { // search range is not restricted
19022                         if (ynumber > 0) {
19023                                 ky = int(ynumber/2);    
19024                         }
19025                         else{
19026                                 ky = int(2*yrng/stepy+0.5)/2;   
19027                         }       
19028                                         
19029                 }
19030         }
19031         
19032         for (int i = -ky; i <= ky; i++) {
19033                 iy = i * stepy ;
19034                 for (int j = -kx; j <= kx; j++)  {
19035                         ix = j*step ;
19036                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19037 
19038                         Normalize_ring( cimage, numr );
19039 
19040                         Frngs(cimage, numr);
19041                         //  compare with all reference images
19042                         // for iref in xrange(len(crefim)):
19043                         for ( iref = 0; iref < (int)crefim_len; iref++) {
19044                                 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19045                                         Dict retvals; 
19046                                         if ((psi-90.0f) < 90.0f) retvals = Crosrng_sm_psi(crefim[iref], cimage, numr,   0, 0, psi_max);
19047                                         else                     retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
19048 
19049                                         double qn = retvals["qn"];
19050                                         if( qn >= peak) {
19051                                                 sx = -ix;
19052                                                 sy = -iy;
19053                                                 nref = iref;
19054                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19055                                                 peak = static_cast<float>(qn);
19056                                                 mirror = 0;
19057                                         }
19058                                 }
19059                         }  
19060                         delete cimage; cimage = 0;
19061                 }
19062         }
19063         
19064         float co, so, sxs, sys;
19065         co = static_cast<float>( cos(ang*pi/180.0) );
19066         so = static_cast<float>( -sin(ang*pi/180.0) );
19067         sxs = sx*co - sy*so;
19068         sys = sx*so + sy*co;
19069         vector<float> res;
19070         res.push_back(ang);
19071         res.push_back(sxs);
19072         res.push_back(sys);
19073         res.push_back(static_cast<float>(mirror));
19074         res.push_back(static_cast<float>(nref));
19075         res.push_back(peak);
19076         return res;
19077 }
19078 
19079 
19080 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
19081                         float xrng, float yrng, float step, string mode,
19082                         vector< int >numr, float cnx, float cny,
19083                         EMData *peaks, EMData *peakm) {
19084 
19085         int   maxrin = numr[numr.size()-1];
19086 
19087         int   ky = int(2*yrng/step+0.5)/2;
19088         int   kx = int(2*xrng/step+0.5)/2;
19089 
19090         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19091         float *p_ccf1ds = peaks->get_data();
19092 
19093         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19094         float *p_ccf1dm = peakm->get_data();
19095 
19096         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19097                 p_ccf1ds[i] = -1.e20f;
19098                 p_ccf1dm[i] = -1.e20f;
19099         }
19100 
19101         for (int i = -ky; i <= ky; i++) {
19102                 float iy = i * step;
19103                 for (int j = -kx; j <= kx; j++) {
19104                         float ix = j*step;
19105                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19106                         Frngs(cimage, numr);
19107                         Crosrng_msg_vec(crefim, cimage, numr,
19108                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19109                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19110                         delete cimage; cimage = 0;
19111                 }
19112         }
19113         return;
19114 }
19115 
19116 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19117      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19118      EMData *peaks_compress, EMData *peakm_compress) {
19119 
19120         int   maxrin = numr[numr.size()-1];
19121 
19122         int   ky = int(2*yrng/step+0.5)/2;
19123         int   kx = int(2*xrng/step+0.5)/2;
19124 
19125         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19126         float *p_ccf1ds = peaks->get_data();
19127 
19128         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19129         float *p_ccf1dm = peakm->get_data();
19130 
19131         peaks_compress->set_size(maxrin, 1, 1);
19132         float *p_ccf1ds_compress = peaks_compress->get_data();
19133 
19134         peakm_compress->set_size(maxrin, 1, 1);
19135         float *p_ccf1dm_compress = peakm_compress->get_data();
19136 
19137         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19138                 p_ccf1ds[i] = -1.e20f;
19139                 p_ccf1dm[i] = -1.e20f;
19140         }
19141 
19142         for (int i = -ky; i <= ky; i++) {
19143                 float iy = i * step;
19144                 for (int j = -kx; j <= kx; j++) {
19145                         float ix = j*step;
19146                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19147                         Frngs(cimage, numr);
19148                         Crosrng_msg_vec(crefim, cimage, numr,
19149                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19150                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19151                         delete cimage; cimage = 0;
19152                 }
19153         }
19154         for (int x=0; x<maxrin; x++) {
19155                 float maxs = -1.0e22f;
19156                 float maxm = -1.0e22f;
19157                 for (int i=1; i<=2*ky+1; i++) {
19158                         for (int j=1; j<=2*kx+1; j++) {
19159                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19160                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19161                         }
19162                 }
19163                 p_ccf1ds_compress[x] = maxs;
19164                 p_ccf1dm_compress[x] = maxm;
19165         }
19166         return;
19167 }
19168 
19169 struct ccf_point
19170 {
19171     float value;
19172     int i;
19173     int j;
19174     int k;
19175     int mirror;
19176 };
19177 
19178 
19179 struct ccf_value
19180 {
19181     bool operator()( const ccf_point& a, const ccf_point& b )
19182     {
19183         return a.value > b.value;
19184     }
19185 };
19186 
19187 
19188 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19189                         float xrng, float yrng, float step, string mode,
19190                         vector< int >numr, float cnx, float cny, double T) {
19191 
19192         int   maxrin = numr[numr.size()-1];
19193 
19194         int   ky = int(2*yrng/step+0.5)/2;
19195         int   kx = int(2*xrng/step+0.5)/2;
19196 
19197         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19198         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19199         int vol = maxrin*(2*kx+1)*(2*ky+1);
19200         vector<ccf_point> ccf(2*vol);
19201         ccf_point temp;
19202 
19203         int index = 0;
19204         for (int i = -ky; i <= ky; i++) {
19205                 float iy = i * step;
19206                 for (int j = -kx; j <= kx; j++) {
19207                         float ix = j*step;
19208                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19209                         Frngs(cimage, numr);
19210                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19211                         for (int k=0; k<maxrin; k++) {
19212                                 temp.value = p_ccf1ds[k];
19213                                 temp.i = k;
19214                                 temp.j = j;
19215                                 temp.k = i;
19216                                 temp.mirror = 0;
19217                                 ccf[index] = temp;
19218                                 index++;
19219                                 temp.value = p_ccf1dm[k];
19220                                 temp.mirror = 1;
19221                                 ccf[index] = temp;
19222                                 index++;
19223                         }
19224                         delete cimage; cimage = 0;
19225                 }
19226         }
19227 
19228         delete p_ccf1ds;
19229         delete p_ccf1dm;
19230         std::sort(ccf.begin(), ccf.end(), ccf_value());
19231 
19232         double qt = (double)ccf[0].value;
19233         vector <double> p(2*vol), cp(2*vol);
19234 
19235         double sump = 0.0;
19236         for (int i=0; i<2*vol; i++) {
19237                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19238                 sump += p[i];
19239         }
19240         for (int i=0; i<2*vol; i++) {
19241                 p[i] /= sump;
19242         }
19243         for (int i=1; i<2*vol; i++) {
19244                 p[i] += p[i-1];
19245         }
19246         p[2*vol-1] = 2.0;
19247 
19248         float t = get_frand(0.0f, 1.0f);
19249         int select = 0;
19250         while (p[select] < t)   select += 1;
19251 
19252         vector<float> a(6);
19253         a[0] = ccf[select].value;
19254         a[1] = (float)ccf[select].i;
19255         a[2] = (float)ccf[select].j;
19256         a[3] = (float)ccf[select].k;
19257         a[4] = (float)ccf[select].mirror;
19258         a[5] = (float)select;
19259         return a;
19260 }
19261 
19262 
19263 /*
19264 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
19265                         float xrng, float yrng, float step, string mode,
19266                         vector< int >numr, float cnx, float cny,
19267                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
19268 
19269 // formerly known as apmq
19270     // Determine shift and rotation between image and many reference
19271     // images (crefim, weights have to be applied) quadratic
19272     // interpolation
19273 
19274 
19275     // Manually extract.
19276 *//*    vector< EMAN::EMData* > crefim;
19277     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
19278     crefim.reserve(crefim_len);
19279 
19280     for(std::size_t i=0;i<crefim_len;i++) {
19281         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
19282         crefim.push_back(proxy());
19283     }
19284 */
19285 /*
19286         int   maxrin = numr[numr.size()-1];
19287 
19288         size_t crefim_len = crefim.size();
19289 
19290         int   iref;
19291         int   ky = int(2*yrng/step+0.5)/2;
19292         int   kx = int(2*xrng/step+0.5)/2;
19293         int   tkx = 2*kx+3;
19294         int   tky = 2*ky+3;
19295 
19296         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
19297         float *p_ccf1ds = peaks->get_data();
19298 
19299 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
19300 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
19301         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
19302         float *p_ccf1dm = peakm->get_data();
19303 
19304         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
19305                 p_ccf1ds[i] = -1.e20f;
19306                 p_ccf1dm[i] = -1.e20f;
19307         }
19308 
19309         float  iy, ix;
19310         for (int i = -ky; i <= ky; i++) {
19311                 iy = i * step ;
19312                 for (int j = -kx; j <= kx; j++) {
19313                         ix = j*step ;
19314                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19315                         Frngs(cimage, numr);
19316                         //  compare with all reference images
19317                         // for iref in xrange(len(crefim)):
19318                         for ( iref = 0; iref < (int)crefim_len; iref++) {
19319                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
19320                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
19321                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
19322                         }
19323                         delete cimage; cimage = 0;
19324                 }
19325         }
19326         return;
19327 }
19328 */
19329 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19330 
19331         EMData *rot;
19332 
19333         const int nmax=3, mmax=3;
19334         char task[60], csave[60];
19335         long int lsave[4];
19336         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19337         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];
19338         long int SIXTY=60;
19339 
19340         //     We wish to have no output.
19341         iprint = -1;
19342 
19343         //c     We specify the tolerances in the stopping criteria.
19344         factr=1.0e1;
19345         pgtol=1.0e-5;
19346 
19347         //     We specify the dimension n of the sample problem and the number
19348         //        m of limited memory corrections stored.  (n and m should not
19349         //        exceed the limits nmax and mmax respectively.)
19350         n=3;
19351         m=3;
19352 
19353         //     We now provide nbd which defines the bounds on the variables:
19354         //                    l   specifies the lower bounds,
19355         //                    u   specifies the upper bounds.
19356         //                    x   specifies the initial guess
19357         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19358         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19359         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19360 
19361 
19362         //     We start the iteration by initializing task.
19363         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19364         strcpy(task,"START");
19365         for (int i=5;i<60;i++)  task[i]=' ';
19366 
19367         //     This is the call to the L-BFGS-B code.
19368         // (* call the L-BFGS-B routine with task='START' once before loop *)
19369         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19370         //int step = 1;
19371 
19372         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19373         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19374 
19375                 if (strncmp(task,"FG",2)==0) {
19376                 //   the minimization routine has returned to request the
19377                 //   function f and gradient g values at the current x
19378 
19379                 //        Compute function value f for the sample problem.
19380                 rot = new EMData();
19381                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19382                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19383                 //f = -f;
19384                 delete rot;
19385 
19386                 //        Compute gradient g for the sample problem.
19387                 float dt = 1.0e-3f;
19388                 rot = new EMData();
19389                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19390                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19391                 //f1 = -f1;
19392                 g[0] = (f1-f)/dt;
19393                 delete rot;
19394 
19395                 dt = 1.0e-2f;
19396                 rot = new EMData();
19397                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19398                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19399                 //f2 = -f2;
19400                 g[1] = (f2-f)/dt;
19401                 delete rot;
19402 
19403                 rot = new EMData();
19404                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19405                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19406                 //f3 = -f3;
19407                 g[2] = (f3-f)/dt;
19408                 delete rot;
19409                 }
19410 
19411                 //c          go back to the minimization routine.
19412                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19413                 //step++;
19414         }
19415 
19416         //printf("Total step is %d\n", step);
19417         vector<float> res;
19418         res.push_back(static_cast<float>(x[0]));
19419         res.push_back(static_cast<float>(x[1]));
19420         res.push_back(static_cast<float>(x[2]));
19421         //res.push_back(step);
19422         return res;
19423 }
19424 
19425 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19426 
19427         EMData *rot;
19428 
19429         const int nmax=3, mmax=3;
19430         char task[60], csave[60];
19431         long int lsave[4];
19432         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19433         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];
19434         long int SIXTY=60;
19435 
19436         //     We wish to have no output.
19437         iprint = -1;
19438 
19439         //c     We specify the tolerances in the stopping criteria.
19440         factr=1.0e1;
19441         pgtol=1.0e-5;
19442 
19443         //     We specify the dimension n of the sample problem and the number
19444         //        m of limited memory corrections stored.  (n and m should not
19445         //        exceed the limits nmax and mmax respectively.)
19446         n=3;
19447         m=3;
19448 
19449         //     We now provide nbd which defines the bounds on the variables:
19450         //                    l   specifies the lower bounds,
19451         //                    u   specifies the upper bounds.
19452         //                    x   specifies the initial guess
19453         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19454         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19455         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19456 
19457 
19458         //     We start the iteration by initializing task.
19459         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19460         strcpy(task,"START");
19461         for (int i=5;i<60;i++)  task[i]=' ';
19462 
19463         //     This is the call to the L-BFGS-B code.
19464         // (* call the L-BFGS-B routine with task='START' once before loop *)
19465         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19466         //int step = 1;
19467 
19468         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19469         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19470 
19471                 if (strncmp(task,"FG",2)==0) {
19472                 //   the minimization routine has returned to request the
19473                 //   function f and gradient g values at the current x
19474 
19475                 //        Compute function value f for the sample problem.
19476                 rot = new EMData();
19477                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19478                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19479                 //f = -f;
19480                 delete rot;
19481 
19482                 //        Compute gradient g for the sample problem.
19483                 float dt = 1.0e-3f;
19484                 rot = new EMData();
19485                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19486                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19487                 //f1 = -f1;
19488                 g[0] = (f1-f)/dt;
19489                 delete rot;
19490 
19491                 rot = new EMData();
19492                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19493                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19494                 //f2 = -f2;
19495                 g[1] = (f2-f)/dt;
19496                 delete rot;
19497 
19498                 rot = new EMData();
19499                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19500                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19501                 //f3 = -f3;
19502                 g[2] = (f3-f)/dt;
19503                 delete rot;
19504                 }
19505 
19506                 //c          go back to the minimization routine.
19507                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19508                 //step++;
19509         }
19510 
19511         //printf("Total step is %d\n", step);
19512         vector<float> res;
19513         res.push_back(static_cast<float>(x[0]));
19514         res.push_back(static_cast<float>(x[1]));
19515         res.push_back(static_cast<float>(x[2]));
19516         //res.push_back(step);
19517         return res;
19518 }
19519 
19520 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) {
19521 
19522         EMData *proj, *proj2;
19523 
19524         const int nmax=5, mmax=5;
19525         char task[60], csave[60];
19526         long int lsave[4];
19527         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19528         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];
19529         long int SIXTY=60;
19530 
19531         //     We wish to have no output.
19532         iprint = -1;
19533 
19534         //c     We specify the tolerances in the stopping criteria.
19535         factr=1.0e1;
19536         pgtol=1.0e-5;
19537 
19538         //     We specify the dimension n of the sample problem and the number
19539         //        m of limited memory corrections stored.  (n and m should not
19540         //        exceed the limits nmax and mmax respectively.)
19541         n=5;
19542         m=5;
19543 
19544         //     We now provide nbd which defines the bounds on the variables:
19545         //                    l   specifies the lower bounds,
19546         //                    u   specifies the upper bounds.
19547         //                    x   specifies the initial guess
19548         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
19549         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
19550         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
19551         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
19552         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
19553 
19554 
19555         //     We start the iteration by initializing task.
19556         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19557         strcpy(task,"START");
19558         for (int i=5;i<60;i++)  task[i]=' ';
19559 
19560         //     This is the call to the L-BFGS-B code.
19561         // (* call the L-BFGS-B routine with task='START' once before loop *)
19562         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19563         int step = 1;
19564 
19565         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19566         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19567 
19568                 if (strncmp(task,"FG",2)==0) {
19569                 //   the minimization routine has returned to request the
19570                 //   function f and gradient g values at the current x
19571 
19572                 //        Compute function value f for the sample problem.
19573                 proj = new EMData();
19574                 proj2 = new EMData();
19575                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19576                 proj->fft_shuffle();
19577                 proj->center_origin_fft();
19578                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19579                 proj->do_ift_inplace();
19580                 int M = proj->get_ysize()/2;
19581                 proj2 = proj->window_center(M);
19582                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19583                 //f = -f;
19584                 delete proj;
19585                 delete proj2;
19586 
19587                 //        Compute gradient g for the sample problem.
19588                 float dt = 1.0e-3f;
19589                 proj = new EMData();
19590                 proj2 = new EMData();
19591                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19592                 proj->fft_shuffle();
19593                 proj->center_origin_fft();
19594                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19595                 proj->do_ift_inplace();
19596                 proj2 = proj->window_center(M);
19597                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19598                 //ft = -ft;
19599                 delete proj;
19600                 delete proj2;
19601                 g[0] = (ft-f)/dt;
19602 
19603                 proj = new EMData();
19604                 proj2 = new EMData();
19605                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19606                 proj->fft_shuffle();
19607                 proj->center_origin_fft();
19608                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19609                 proj->do_ift_inplace();
19610                 proj2 = proj->window_center(M);
19611                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19612                 //ft = -ft;
19613                 delete proj;
19614                 delete proj2;
19615                 g[1] = (ft-f)/dt;
19616 
19617                 proj = new EMData();
19618                 proj2 = new EMData();
19619                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19620                 proj->fft_shuffle();
19621                 proj->center_origin_fft();
19622                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19623                 proj->do_ift_inplace();
19624                 proj2 = proj->window_center(M);
19625                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19626                 //ft = -ft;
19627                 delete proj;
19628                 delete proj2;
19629                 g[2] = (ft-f)/dt;
19630 
19631                 proj = new EMData();
19632                 proj2 = new EMData();
19633                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19634                 proj->fft_shuffle();
19635                 proj->center_origin_fft();
19636                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19637                 proj->do_ift_inplace();
19638                 proj2 = proj->window_center(M);
19639                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19640                 //ft = -ft;
19641                 delete proj;
19642                 delete proj2;
19643                 g[3] = (ft-f)/dt;
19644 
19645                 proj = new EMData();
19646                 proj2 = new EMData();
19647                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19648                 proj->fft_shuffle();
19649                 proj->center_origin_fft();
19650                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19651                 proj->do_ift_inplace();
19652                 proj2 = proj->window_center(M);
19653                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19654                 //ft = -ft;
19655                 delete proj;
19656                 delete proj2;
19657                 g[4] = (ft-f)/dt;
19658                 }
19659 
19660                 //c          go back to the minimization routine.
19661                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19662                 step++;
19663         }
19664 
19665         //printf("Total step is %d\n", step);
19666         vector<float> res;
19667         res.push_back(static_cast<float>(x[0]));
19668         res.push_back(static_cast<float>(x[1]));
19669         res.push_back(static_cast<float>(x[2]));
19670         res.push_back(static_cast<float>(x[3]));
19671         res.push_back(static_cast<float>(x[4]));
19672         //res.push_back(step);
19673         return res;
19674 }
19675 
19676 
19677 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19678 
19679         double  x[4];
19680         int n;
19681         int l = 3;
19682         int m = 200;
19683         double e = 1e-9;
19684         double step = 0.01;
19685         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19686 
19687         x[1] = ang;
19688         x[2] = sxs;
19689         x[3] = sys;
19690 
19691         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
19692         //printf("Took %d steps\n", n);
19693 
19694         vector<float> res;
19695         res.push_back(static_cast<float>(x[1]));
19696         res.push_back(static_cast<float>(x[2]));
19697         res.push_back(static_cast<float>(x[3]));
19698         res.push_back(static_cast<float>(n));
19699         return res;
19700 }
19701 
19702 /* Parameters:
19703  * args - parameters of (L-1) G transformation (G_1, G_2, ..., G_(L-1)), saved as alpha_1, sx_1, sy_1, alpha_2, sx_2, sy_2, ... (transformation G_L is always set to I)
19704  * all_ali_params - parameters of (L*N) T transformation, saved as flat list: <parameters of N images for G_1 alignment>, <parameters of N images for G_2 alignment>, ...
19705  *                  where image's parameters consists of four values: alpha, sx, sy, mirror
19706  * d - particle diameter (2R)
19707  */
19708 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19709         
19710         const int nmax=args.size(), mmax=nmax;
19711         char task[60], csave[60];
19712         long int lsave[4];
19713         long int n, m, iprint, isave[44];
19714         long int* nbd = new long int[nmax];
19715         long int* iwa = new long int[3*nmax];
19716         double f, factr, pgtol;
19717         double* x = new double[nmax];
19718         double* l = new double[nmax];
19719         double* u = new double[nmax];
19720         double* g = new double[nmax];
19721         double dsave[29];
19722         double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19723         long int SIXTY=60;
19724 
19725         int num_ali = nmax/3+1;
19726         int nima = all_ali_params.size()/(num_ali*4);
19727         
19728         //     We wish to have no output.
19729         iprint = -1;
19730 
19731         //c     We specify the tolerances in the stopping criteria.
19732         factr=1.0e1;
19733         pgtol=1.0e-9;
19734 
19735         //     We specify the dimension n of the sample problem and the number
19736         //        m of limited memory corrections stored.  (n and m should not
19737         //        exceed the limits nmax and mmax respectively.)
19738         n=nmax;
19739         m=mmax;
19740 
19741         //     We now provide nbd which defines the bounds on the variables:
19742         //                    l   specifies the lower bounds,
19743         //                    u   specifies the upper bounds.
19744         //                    x   specifies the initial guess
19745         for (int i=0; i<nmax; i++) {
19746                 x[i] = args[i]; 
19747                 nbd[i] = 0;
19748         }
19749 
19750         //     We start the iteration by initializing task.
19751         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19752         strcpy(task,"START");
19753         for (int i=5;i<60;i++)  task[i]=' ';
19754 
19755         //     This is the call to the L-BFGS-B code.
19756         // (* call the L-BFGS-B routine with task='START' once before loop *)
19757         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19758         int step = 1;
19759 
19760         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19761         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19762 
19763                 if (strncmp(task,"FG",2)==0) {
19764                 //   the minimization routine has returned to request the
19765                 //   function f and gradient g values at the current x
19766 
19767                 //        Compute function value f for the sample problem.
19768                 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
19769 
19770                 //        Compute gradient g for the sample problem.
19771                 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
19772 
19773                 }
19774                 //c          go back to the minimization routine.
19775                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19776                 step++;
19777         }
19778 
19779         //printf("Total step is %d\n", step);
19780         vector<float> res;
19781         for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19782         res.push_back(static_cast<float>(f));
19783 
19784         delete[] nbd;
19785         delete[] iwa;
19786         delete[] x;
19787         delete[] l;
19788         delete[] u;
19789         delete[] g;
19790         delete[] wa;
19791 
19792         return res;
19793 
19794 }
19795 
19796 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d)
19797 {
19798         vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
19799         double sum_sqr_pixel_error = 0.0;
19800         for (int i=0; i<nima; i++) {
19801                 sum_sqr_pixel_error += sqr_pixel_error[i];
19802         }
19803         return ( sum_sqr_pixel_error / nima );
19804 }
19805 
19806 
19807 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d)
19808 {
19809         double* args = new double[num_ali*3];
19810         for (int i=0; i<3*num_ali-3; i++)   args[i] = x[i];
19811         args[3*num_ali-3] = 0.0;
19812         args[3*num_ali-2] = 0.0;
19813         args[3*num_ali-1] = 0.0;
19814         double* cosa = new double[num_ali];
19815         double* sina = new double[num_ali];
19816         for (int i=0; i<num_ali; i++) {
19817                 cosa[i] = cos(args[i*3]*M_PI/180.0);
19818                 sina[i] = sin(args[i*3]*M_PI/180.0);
19819         }
19820         double* sx = new double[num_ali];
19821         double* sy = new double[num_ali];
19822         
19823         vector<double> sqr_pixel_error(nima);
19824 
19825         for (int i=0; i<nima; i++) {
19826                 double sum_cosa = 0.0;
19827                 double sum_sina = 0.0;
19828                 for (int j=0; j<num_ali; j++) {
19829                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19830                                 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19831                                 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19832                                 sx[j] = args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] + ali_params[j*nima*4+i*4+2]*sina[j];
19833                                 sy[j] = args[j*3+2] - ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19834                         } else {
19835                                 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19836                                 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19837                                 sx[j] = -args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] - ali_params[j*nima*4+i*4+2]*sina[j];
19838                                 sy[j] =  args[j*3+2] + ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19839                         }
19840                 }
19841                 double sqrtP = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19842                 sqr_pixel_error[i] = d*d/4.0*(1.0-sqrtP/num_ali)+var(sx, num_ali)+var(sy, num_ali);
19843         }
19844         
19845         delete[] args;
19846         delete[] cosa;
19847         delete[] sina;
19848         delete[] sx;
19849         delete[] sy;
19850         
19851         return sqr_pixel_error;
19852 }
19853 
19854 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
19855 
19856         for (int i=0; i<num_ali*3-3; i++)    g[i] = 0.0;
19857 
19858         double* args = new double[num_ali*3];
19859         for (int i=0; i<3*num_ali-3; i++)   args[i] = x[i];
19860         args[3*num_ali-3] = 0.0;
19861         args[3*num_ali-2] = 0.0;
19862         args[3*num_ali-1] = 0.0;
19863         double* cosa = new double[num_ali];
19864         double* sina = new double[num_ali];
19865         for (int i=0; i<num_ali; i++) {
19866                 cosa[i] = cos(args[i*3]*M_PI/180.0);
19867                 sina[i] = sin(args[i*3]*M_PI/180.0);
19868         }
19869         double* sx = new double[num_ali];
19870         double* sy = new double[num_ali];
19871         
19872         vector<float> sqr_pixel_error(nima);
19873 
19874         for (int i=0; i<nima; i++) {
19875                 double sum_cosa = 0.0;
19876                 double sum_sina = 0.0;
19877                 for (int j=0; j<num_ali; j++) {
19878                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19879                                 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19880                                 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19881                                 sx[j] = args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] + ali_params[j*nima*4+i*4+2]*sina[j];
19882                                 sy[j] = args[j*3+2] - ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19883                         } else {
19884                                 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19885                                 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19886                                 sx[j] = -args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] - ali_params[j*nima*4+i*4+2]*sina[j];
19887                                 sy[j] =  args[j*3+2] + ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19888                         }
19889                 }
19890                 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19891                 sum_cosa /= P;
19892                 sum_sina /= P;
19893                 for (int j=0; j<num_ali-1; j++) {
19894                         double dx = 2.0*(sx[j]-mean(sx, num_ali));
19895                         double dy = 2.0*(sy[j]-mean(sy, num_ali));
19896                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19897                                 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
19898                                                     sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19899                                                     dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19900                                                     dy*(-ali_params[j*nima*4+i*4+1]*cosa[j]-ali_params[j*nima*4+i*4+2]*sina[j]))*M_PI/180.0;
19901                                 g[j*3+1] += dx;
19902                                 g[j*3+2] += dy;
19903                         } else {
19904                                 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
19905                                                      sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19906                                                     dx*( ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19907                                                     dy*(-ali_params[j*nima*4+i*4+1]*cosa[j]+ali_params[j*nima*4+i*4+2]*sina[j]))*M_PI/180.0;
19908                                 g[j*3+1] += -dx;
19909                                 g[j*3+2] += dy;
19910                         }
19911                 }
19912         }
19913         
19914         for (int i=0; i<3*num_ali-3; i++)  g[i] /= (num_ali*nima);
19915         
19916         delete[] args;
19917         delete[] cosa;
19918         delete[] sina;
19919         delete[] sx;
19920         delete[] sy;
19921 }
19922 
19923 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19924 
19925         EMData *rot= new EMData();
19926         float ccc;
19927 
19928         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19929         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19930         delete rot;
19931         return ccc;
19932 }
19933 
19934 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19935 
19936         double  x[4];
19937         int n;
19938         int l = 3;
19939         int m = 200;
19940         double e = 1e-9;
19941         double step = 0.001;
19942         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19943 
19944         x[1] = ang;
19945         x[2] = sxs;
19946         x[3] = sys;
19947 
19948         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
19949         //printf("Took %d steps\n", n);
19950 
19951         vector<float> res;
19952         res.push_back(static_cast<float>(x[1]));
19953         res.push_back(static_cast<float>(x[2]));
19954         res.push_back(static_cast<float>(x[3]));
19955         res.push_back(static_cast<float>(n));
19956         return res;
19957 }
19958 
19959 
19960 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19961 
19962         EMData *rot= new EMData();
19963         float ccc;
19964 
19965         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19966         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19967         delete rot;
19968         return ccc;
19969 }
19970 
19971 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*(size_t)nx]
19972 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19973 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19974 {
19975         ENTERFUNC;
19976         /* Exception Handle */
19977         if (!img) {
19978                 throw NullPointerException("NULL input image");
19979         }
19980         cout <<"  VERSION  05/20/2013  3:00pm"<<endl;
19981         int newx, newy, newz;
19982         bool  keep_going;
19983         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19984         //int size = nx*ny*nz;
19985         EMData * img2 = new EMData();
19986         img2->set_size(nx,ny,nz);
19987         img2->to_zero();
19988         float *img_ptr  =img->get_data();
19989         float *img2_ptr = img2->get_data();
19990         int r2 = ro*ro;
19991         int r3 = r2*ro;
19992         int ri2 = ri*ri;
19993         int ri3 = ri2*ri;
19994 
19995         int n2 = nx/2;
19996 
19997         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
19998                 float z2 = static_cast<float>(k*k);
19999                 for (int j=-n2; j<=n2; j++) {
20000                         float y2 = z2 + j*j;
20001                         if(y2 <= r2) {
20002                                                                                         //cout << "  j  "<<j <<endl;
20003 
20004                                 for (int i=-n2; i<=n2; i++) {
20005                                         float x2 = y2 + i*i;
20006                                         if(x2 <= r3) {
20007                                                                                         //cout << "  i  "<<i <<endl;
20008                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
20009                                                 if(x2 >= ri3) {
20010                                                         //  this is the outer shell, here points can only vanish
20011                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
20012                                                                 //cout << "  1  "<<ib <<endl;
20013                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
20014                                                                         img2_ptr(ib,jb,kb) = 0.0f;
20015                                                                         keep_going = true;
20016                                                                 //cout << "  try  "<<ib <<endl;
20017                                                                         while(keep_going) {
20018                                                                                 newx = Util::get_irand(-ro,ro);
20019                                                                                 newy = Util::get_irand(-ro,ro);
20020                                                                                 newz = Util::get_irand(-ro,ro);
20021                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
20022                                                                                         newx += n2; newy += n2; newz += n2;
20023                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
20024                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
20025                                                                                                 keep_going = false; }
20026                                                                                 }
20027                                                                         }
20028                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
20029                                                         }
20030                                                 }  else  {
20031                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
20032                                                         if(img_ptr(ib,jb,kb) == 1.0) {
20033                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
20034                                                                         //  find out the number of neighbors
20035                                                                         float  numn = -1.0f;  // we already know the central one is 1
20036                                                                         for (newz = -1; newz <= 1; newz++)
20037                                                                                 for (newy = -1; newy <= 1; newy++)
20038                                                                                         for (newx = -1; newx <= 1; newx++)
20039                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
20040                                                                         img2_ptr(ib,jb,kb) = 0.0;
20041                                                                         if(numn == 26.0f) {
20042                                                                                 //  all neighbors exist, it has to vanish
20043                                                                                 keep_going = true;
20044                                                                                 while(keep_going) {
20045                                                                                         newx = Util::get_irand(-ro,ro);
20046                                                                                         newy = Util::get_irand(-ro,ro);
20047                                                                                         newz = Util::get_irand(-ro,ro);
20048                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
20049                                                                                                 newx += n2; newy += n2; newz += n2;
20050                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
20051                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
20052                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
20053                                                                                                                         newx += n2; newy += n2; newz += n2;
20054                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
20055                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
20056                                                                                                                                 keep_going = false; }
20057                                                                                                                 }
20058                                                                                                         }
20059                                                                                                 }
20060                                                                                         }
20061                                                                                 }
20062                                                                         }  else if(numn == 25.0f) {
20063                                                                                 // there is only one empty neighbor, move there
20064                                                                                 for (newz = -1; newz <= 1; newz++) {
20065                                                                                         for (newy = -1; newy <= 1; newy++) {
20066                                                                                                 for (newx = -1; newx <= 1; newx++) {
20067                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
20068                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
20069                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
20070                                                                                                                         }
20071                                                                                                         }
20072                                                                                                 }
20073                                                                                         }
20074                                                                                 }
20075                                                                         }  else {
20076                                                                                 //  more than one neighbor is zero, select randomly one and move there
20077                                                                                 keep_going = true;
20078                                                                                 while(keep_going) {
20079                                                                                         newx = Util::get_irand(-1,1);
20080                                                                                         newy = Util::get_irand(-1,1);
20081                                                                                         newz = Util::get_irand(-1,1);
20082                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
20083                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
20084                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
20085                                                                                                         keep_going = false;
20086                                                                                                 }
20087                                                                                         }
20088                                                                                 }
20089                                                                         }
20090                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
20091                                                         }
20092                                                 }
20093                                         }
20094                                 }
20095                         }
20096                 }
20097         }
20098         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
20099         img2->update();
20100 
20101         EXITFUNC;
20102         return img2;
20103 }
20104 #undef img_ptr
20105 #undef img2_ptr
20106 
20107 struct point3d_t
20108 {
20109         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20110 
20111         int x;
20112         int y;
20113         int z;
20114 };
20115 
20116 
20117 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20118 {
20119         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20120         int noff = 6;
20121 
20122         int nx = visited->get_xsize();
20123         int ny = visited->get_ysize();
20124         int nz = visited->get_zsize();
20125 
20126         vector< point3d_t > pts;
20127         pts.push_back( point3d_t(ix, iy, iz) );
20128         visited->set_value_at( ix, iy, iz, (float)grpid );
20129 
20130         int start = 0;
20131         int end = pts.size();
20132 
20133         while( end > start ) {
20134                 for(int i=start; i < end; ++i ) {
20135                         int ix = pts[i].x;
20136                         int iy = pts[i].y;
20137                         int iz = pts[i].z;
20138 
20139                         for( int j=0; j < noff; ++j ) {
20140                                 int jx = ix + offs[j][0];
20141                                 int jy = iy + offs[j][1];
20142                                 int jz = iz + offs[j][2];
20143 
20144                                 if( jx < 0 || jx >= nx ) continue;
20145                                 if( jy < 0 || jy >= ny ) continue;
20146                                 if( jz < 0 || jz >= nz ) continue;
20147 
20148 
20149                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20150                                     pts.push_back( point3d_t(jx, jy, jz) );
20151                                     visited->set_value_at( jx, jy, jz, (float)grpid );
20152                                 }
20153 
20154                         }
20155                 }
20156 
20157                 start = end;
20158                 end = pts.size();
20159         }
20160         return pts.size();
20161 }
20162 
20163 
20164 EMData* Util::get_biggest_cluster( EMData* mg )
20165 {
20166         int nx = mg->get_xsize();
20167         int ny = mg->get_ysize();
20168         int nz = mg->get_zsize();
20169 
20170         EMData* visited = new EMData();
20171         visited->set_size( nx, ny, nz );
20172         visited->to_zero();
20173         int grpid = 0;
20174         int maxgrp = 0;
20175         int maxsize = 0;
20176         for( int iz=0; iz < nz; ++iz ) {
20177                 for( int iy=0; iy < ny; ++iy ) {
20178                         for( int ix=0; ix < nx; ++ix ) {
20179                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20180 
20181                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
20182                                         // visited before, must be in other group.
20183                                         continue;
20184                                 }
20185 
20186                                 grpid++;
20187                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20188                                 if( grpsize > maxsize ) {
20189                                         maxsize = grpsize;
20190                                         maxgrp = grpid;
20191                                 }
20192                         }
20193                 }
20194         }
20195 
20196         Assert( maxgrp > 0 );
20197 
20198         int npoint = 0;
20199         EMData* result = new EMData();
20200         result->set_size( nx, ny, nz );
20201         result->to_zero();
20202 
20203         for( int iz=0; iz < nz; ++iz ) {
20204                 for( int iy=0; iy < ny; ++iy ) {
20205                         for( int ix=0; ix < nx; ++ix ) {
20206                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
20207                                         (*result)(ix,iy,iz) = 1.0;
20208                                         npoint++;
20209                                 }
20210                         }
20211                 }
20212         }
20213 
20214         Assert( npoint==maxsize );
20215         delete visited;
20216         return result;
20217 
20218 }
20219 
20220 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)
20221 {
20222         int    ix, iy, iz;
20223         int    i,  j, k;
20224         int    nr2, nl2;
20225         float  ak;
20226         float  scx, scy, scz;
20227         int    offset = 2 - nx%2;
20228         int    lsm = nx + offset;
20229         EMData* ctf_img1 = new EMData();
20230         ctf_img1->set_size(lsm, ny, nz);
20231         float freq = 1.0f/(2.0f*ps);
20232         scx = 2.0f/float(nx);
20233         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20234         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20235         nr2 = ny/2 ;
20236         nl2 = nz/2 ;
20237         for ( k=0; k<nz;k++) {
20238                 iz = k;  if(k>nl2) iz=k-nz;
20239                 float oz2 = iz*scz*iz*scz;
20240                 for ( j=0; j<ny;j++) {
20241                         iy = j;  if(j>nr2) iy=j - ny;
20242                         float oy = iy*scy;
20243                         float oy2 = oy*oy;
20244                         for ( i=0; i<lsm/2; i++) {
20245                                 ix=i;
20246                                 if( dza == 0.0f) {
20247                                         ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20248                                         (*ctf_img1) (i*2,j,k)   = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20249                                 } else {
20250                                         float ox = ix*scx;
20251                                         ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20252                                         float dzz = dz - dza/2.0f*sin(2*(atan2(oy, ox)+azz*M_PI/180.0f));
20253                                         (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20254                                 }
20255                                 //(*ctf_img1) (i*2+1,j,k) = 0.0f;  PAP  I assumed new EMData sets to zero
20256                         }
20257                 }
20258         }
20259         ctf_img1->update();
20260         ctf_img1->set_complex(true);
20261         ctf_img1->set_ri(true);
20262         //ctf_img1->attr_dict["is_complex"] = 1;
20263         //ctf_img1->attr_dict["is_ri"] = 1;
20264         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20265         return ctf_img1;
20266 }
20267 
20268 
20269 
20270 EMData* Util::ctf_rimg(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)
20271 {
20272         int    ix, iy, iz;
20273         int    i,  j, k;
20274         float  ak;
20275         float  scx, scy, scz;
20276         EMData* ctf_img1 = new EMData();
20277         ctf_img1->set_size(nx, ny, nz);
20278         float freq = 1.0f/(2.0f*ps);
20279         scx = 2.0f/float(nx);
20280         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20281         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20282         int ns2 = nx/2 ;
20283         int nr2 = ny/2 ;
20284         int nl2 = nz/2 ;
20285         int nod = nx%2 ;
20286         int nok = ny%2 ;
20287         int noz = nz%2 ;
20288         for ( k=0; k<nz;k++) {
20289                 iz = k - nl2;
20290                 int kz = (nz - k - noz)%nz;
20291                 float oz2 = iz*scz*iz*scz;
20292                 for ( j=0; j<ny;j++) {
20293                         iy = j - nr2;
20294                         int jy = (ny - j - nok)%ny;
20295                         float oy = iy*scy;
20296                         float oy2 = oy*oy;
20297                         for ( i=0; i<=ns2; i++) {
20298                                 ix = i - ns2;
20299                                 if( dza == 0.0f) {
20300                                         ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20301                                         (*ctf_img1) (i,j,k)   = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20302                                 } else {
20303                                         float ox = ix*scx;
20304                                         ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20305                                         float dzz = dz - dza/2.0f*sin(2*(atan2(oy, ox)+azz*M_PI/180.0f));
20306                                         (*ctf_img1) (i,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20307                                 }
20308                                 ix = nx - i - nod;
20309                                 if(ix<nx)  (*ctf_img1) (ix,jy,kz) = (*ctf_img1) (i,j,k);
20310                         }
20311                 }
20312         }
20313         ctf_img1->update();
20314         return ctf_img1;
20315 }
20316 
20317 
20318 
20319 
20320 EMData* Util::ctf2_rimg(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)
20321 {
20322         int    ix, iy, iz;
20323         int    i,  j, k;
20324         float  ak;
20325         float  scx, scy, scz;
20326         EMData* ctf_img1 = new EMData();
20327         ctf_img1->set_size(nx, ny, nz);
20328         float freq = 1.0f/(2.0f*ps);
20329         scx = 2.0f/float(nx);
20330         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20331         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20332         int ns2 = nx/2 ;
20333         int nr2 = ny/2 ;
20334         int nl2 = nz/2 ;
20335         int nod = nx%2 ;
20336         int nok = ny%2 ;
20337         int noz = nz%2 ;
20338         for ( k=0; k<nz;k++) {
20339                 iz = k - nl2;
20340                 int kz = (nz - k - noz)%nz;
20341                 float oz2 = iz*scz*iz*scz;
20342                 for ( j=0; j<ny;j++) {
20343                         iy = j - nr2;
20344                         int jy = (ny - j - nok)%ny;
20345                         float oy = iy*scy;
20346                         float oy2 = oy*oy;
20347                         for ( i=0; i<=ns2; i++) {
20348                                 ix = i - ns2;
20349                                 if( dza == 0.0f) {
20350                                         ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20351                                         (*ctf_img1) (i,j,k)   = pow(Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign),2);
20352                                 } else {
20353                                         float ox = ix*scx;
20354                                         ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20355                                         //az = atan2(oy, ox);
20356                                         //float dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f-pihalf));
20357                                         float dzz = dz - dza/2.0f*sin(2*(atan2(oy, ox)+azz*M_PI/180.0f));
20358                                         (*ctf_img1) (i,j,k)   = pow(Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign),2);
20359                                 }
20360                                 ix = nx - i - nod;
20361                                 if(ix<nx)  (*ctf_img1) (ix,jy,kz) = (*ctf_img1) (i,j,k);
20362                         }
20363                 }
20364         }
20365         ctf_img1->update();
20366         return ctf_img1;
20367 }
20368 
20369 
20370 
20371 /*
20372 #define  cent(i)     out[i+N]
20373 #define  assign(i)   out[i]
20374 vector<float> Util::cluster_pairwise(EMData* d, int K) {
20375 
20376         int nx = d->get_xsize();
20377         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20378         vector<float> out(N+K+2);
20379         if(N*(N-1)/2 != nx) {
20380                 //print  "  incorrect dimension"
20381                 return out;}
20382         //  assign random objects as centers
20383         for(int i=0; i<N; i++) assign(i) = float(i);
20384         // shuffle
20385         for(int i=0; i<N; i++) {
20386                 int j = Util::get_irand(0,N-1);
20387                 float temp = assign(i);
20388                 assign(i) = assign(j);
20389                 assign(j) = temp;
20390         }
20391         for(int k=0; k<K; k++) cent(k) = float(assign(k));
20392         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
20393         //
20394         for(int i=0; i<N; i++) assign(i) = 0.0f;
20395         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20396         bool change = true;
20397         int it = -1;
20398         while(change && disp < dispold) {
20399                 change = false;
20400                 dispold = disp;
20401                 it++;
20402                 //cout<<"Iteration:  "<<it<<endl;
20403                 // dispersion is a sum of distance from objects to object center
20404                 disp = 0.0f;
20405                 for(int i=0; i<N; i++) {
20406                         qm = 1.0e23f;
20407                         for(int k=0; k<K; k++) {
20408                                 if(float(i) == cent(k)) {
20409                                         qm = 0.0f;
20410                                         na = (float)k;
20411                                 } else {
20412                                         float dt = (*d)(mono(i,int(cent(k))));
20413                                         if(dt < qm) {
20414                                                 qm = dt;
20415                                                 na = (float)k;
20416                                         }
20417                                 }
20418                         }
20419                         disp += qm;
20420                         if(na != assign(i)) {
20421                                 assign(i) = na;
20422                                 change = true;
20423                         }
20424                 }
20425         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
20426                 //print disp
20427                 //print  assign
20428                 // find centers
20429                 for(int k=0; k<K; k++) {
20430                         qm = 1.0e23f;
20431                         for(int i=0; i<N; i++) {
20432                                 if(assign(i) == float(k)) {
20433                                         float q = 0.0;
20434                                         for(int j=0; j<N; j++) {
20435                                                 if(assign(j) == float(k)) {
20436                                                                 //it cannot be the same object
20437                                                         if(i != j)  q += (*d)(mono(i,j));
20438                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
20439                                                 }
20440                                         }
20441                                         if(q < qm) {
20442                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
20443                                                 qm = q;
20444                                                 cent(k) = float(i);
20445                                         }
20446                                 }
20447                         }
20448                 }
20449         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
20450         }
20451         out[N+K] = disp;
20452         out[N+K+1] = float(it);
20453         return  out;
20454 }
20455 #undef  cent
20456 #undef  assign
20457 */
20458 #define  cent(i)     out[i+N]
20459 #define  assign(i)   out[i]
20460 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20461         int nx = d->get_xsize();
20462         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20463         vector<float> out(N+K+2);
20464         if(N*(N-1)/2 != nx) {
20465                 //print  "  incorrect dimension"
20466                 return out;}
20467         //  assign random objects as centers
20468         for(int i=0; i<N; i++) assign(i) = float(i);
20469         // shuffle
20470         for(int i=0; i<N; i++) {
20471                 int j = Util::get_irand(0,N-1);
20472                 float temp = assign(i);
20473                 assign(i) = assign(j);
20474                 assign(j) = temp;
20475         }
20476         for(int k=0; k<K; k++) cent(k) = float(assign(k));
20477         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
20478         //
20479         for(int i=0; i<N; i++) assign(i) = 0.0f;
20480         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20481         bool change = true;
20482         int it = -1;
20483         int ct = -1;
20484         while ((change && disp < dispold) || ct > 0) {
20485 
20486                 change = false;
20487                 dispold = disp;
20488                 it++;
20489 
20490                 // dispersion is a sum of distance from objects to object center
20491                 disp = 0.0f;
20492                 ct = 0;
20493                 for(int i=0; i<N; i++) {
20494                         qm = 1.0e23f;
20495                         for(int k=0; k<K; k++) {
20496                                 if(float(i) == cent(k)) {
20497                                         qm = 0.0f;
20498                                         na = (float)k;
20499                                 } else {
20500                                         float dt = (*d)(mono(i,int(cent(k))));
20501                                         if(dt < qm) {
20502                                                 qm = dt;
20503                                                 na = (float)k;
20504                                         }
20505                                 }
20506                         }
20507 
20508 
20509                         // Simulated annealing
20510                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20511                             na = (float)(Util::get_irand(0, K));
20512                             qm = (*d)(mono(i,int(na)));
20513                             ct++;
20514                         }
20515 
20516                         disp += qm;
20517 
20518                         if(na != assign(i)) {
20519                                 assign(i) = na;
20520                                 change = true;
20521                         }
20522                 }
20523 
20524                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
20525                 T = T*F;
20526 
20527         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
20528                 //print disp
20529                 //print  assign
20530                 // find centers
20531                 for(int k=0; k<K; k++) {
20532                         qm = 1.0e23f;
20533                         for(int i=0; i<N; i++) {
20534                                 if(assign(i) == float(k)) {
20535                                         float q = 0.0;
20536                                         for(int j=0; j<N; j++) {
20537                                                 if(assign(j) == float(k)) {
20538                                                                 //it cannot be the same object
20539                                                         if(i != j)  q += (*d)(mono(i,j));
20540                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
20541                                                 }
20542                                         }
20543                                         if(q < qm) {
20544                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
20545                                                 qm = q;
20546                                                 cent(k) = float(i);
20547                                         }
20548                                 }
20549                         }
20550                 }
20551         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
20552         }
20553         out[N+K] = disp;
20554         out[N+K+1] = float(it);
20555         return  out;
20556 }
20557 #undef  cent
20558 #undef  assign
20559 /*
20560 #define  groupping(i,k)   group[i + k*m]
20561 vector<float> Util::cluster_equalsize(EMData* d, int m) {
20562         int nx = d->get_xsize();
20563         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20564         int K = N/m;
20565         //cout<<"  K  "<<K<<endl;
20566         vector<float> group(N+1);
20567         if(N*(N-1)/2 != nx) {
20568                 //print  "  incorrect dimension"
20569                 return group;}
20570         bool active[N];
20571         for(int i=0; i<N; i++) active[i] = true;
20572 
20573         float dm, qd;
20574         int   ppi, ppj;
20575         for(int k=0; k<K; k++) {
20576                 // find two most similiar objects among active
20577                 cout<<"  k  "<<k<<endl;
20578                 dm = 1.0e23;
20579                 for(int i=1; i<N; i++) {
20580                         if(active[i]) {
20581                                 for(int j=0; j<i; j++) {
20582                                         if(active[j]) {
20583                                                 qd = (*d)(mono(i,j));
20584                                                 if(qd < dm) {
20585                                                         dm = qd;
20586                                                         ppi = i;
20587                                                         ppj = j;
20588                                                 }
20589                                         }
20590                                 }
20591                         }
20592                 }
20593                 groupping(0,k) = float(ppi);
20594                 groupping(1,k) = float(ppj);
20595                 active[ppi] = false;
20596                 active[ppj] = false;
20597 
20598                 // find progressively objects most similar to those in the current list
20599                 for(int l=2; l<m; l++) {
20600                         //cout<<"  l  "<<l<<endl;
20601                         dm = 1.0e23;
20602                         for(int i=0; i<N; i++) {
20603                                 if(active[i]) {
20604                                         qd = 0.0;
20605                                         for(int j=0; j<l; j++) { //j in groupping[k]:
20606                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
20607                                                 int jj = int(groupping(j,k));
20608                         //cout<<"   "<<jj<<endl;
20609                                                 qd += (*d)(mono(i,jj));
20610                                         }
20611                                         if(qd < dm) {
20612                                                 dm = qd;
20613                                                 ppi = i;
20614                                         }
20615                                 }
20616                         }
20617                         groupping(l,k) = float(ppi);
20618                         active[ppi] = false;
20619                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
20620                 }
20621                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
20622         }
20623         // there might be remaining objects when N is not divisible by m, simply put them in one group
20624         if(N%m != 0) {
20625                 int j = K*m;
20626                 K++;
20627                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
20628                 for(int i=0; i<N; i++) {
20629                         if(active[i]) {
20630                                 group[j] = float(i);
20631                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
20632                                 j++;
20633                         }
20634                 }
20635         }
20636 
20637         int  cent[K];
20638          // find centers
20639         for(int k=0; k<K; k++) {
20640                 float qm = 1.0e23f;
20641                 for(int i=0; i<N; i++) {
20642                         if(group[i] == float(k)) {
20643                                 qd = 0.0;
20644                                 for(int j=0; j<N; j++) {
20645                                         if(group[j] == float(k)) {
20646                                                 //it cannot be the same object
20647                                                 if(i != j)  qd += (*d)(mono(i,j));
20648                                         }
20649                                 }
20650                                 if(qd < qm) {
20651                                         qm = qd;
20652                                         cent[k] = i;
20653                                 }
20654                         }
20655                 }
20656         }
20657         // dispersion is a sum of distances from objects to object center
20658         float disp = 0.0f;
20659         for(int i=0; i<N; i++) {
20660                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
20661         }
20662         group[N] = disp;
20663         return  group;
20664 }
20665 #undef  groupping
20666 */
20667 
20668 vector<float> Util::cluster_equalsize(EMData* d) {
20669         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
20670         int nx = d->get_xsize();
20671         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20672         int K = N/2;
20673         vector<float> group(N);
20674         if(N*(N-1)/2 != nx) {
20675                 //print  "  incorrect dimension"
20676                 return group;}
20677         //bool active[N];       //this does not compile in VS2005. --Grant Tang
20678         bool * active = new bool[N];
20679         for(int i=0; i<N; i++) active[i] = true;
20680 
20681         float dm, qd;
20682         int   ppi = 0, ppj = 0;
20683         for(int k=0; k<K; k++) {
20684                 // find pairs of most similiar objects among active
20685                 //cout<<"  k  "<<k<<endl;
20686                 dm = 1.0e23f;
20687                 for(int i=1; i<N; i++) {
20688                         if(active[i]) {
20689                                 for(int j=0; j<i; j++) {
20690                                         if(active[j]) {
20691                                                 qd = (*d)(i*(i - 1)/2 + j);
20692                                                 if(qd < dm) {
20693                                                         dm = qd;
20694                                                         ppi = i;
20695                                                         ppj = j;
20696                                                 }
20697                                         }
20698                                 }
20699                         }
20700                 }
20701                 group[2*k] = float(ppi);
20702                 group[1+2*k] = float(ppj);
20703                 active[ppi] = false;
20704                 active[ppj] = false;
20705         }
20706 
20707         delete [] active;
20708         active = NULL;
20709         return  group;
20710 }
20711 /*
20712 #define son(i,j)=i*(i-1)/2+j
20713 vector<float> Util::cluster_equalsize(EMData* d) {
20714         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
20715         int nx = d->get_xsize();
20716         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20717         int K = N/2;
20718         vector<float> group(N);
20719         if(N*(N-1)/2 != nx) {
20720                 //print  "  incorrect dimension"
20721                 return group;}
20722         //bool active[N];
20723         int  active[N];
20724         for(int i=0; i<N; i++) active[i] = i;
20725 
20726         float dm, qd;
20727         int   ppi = 0, ppj = 0, ln = N;
20728         for(int k=0; k<K; k++) {
20729                 // find pairs of most similiar objects among active
20730                 //cout<<"  k:  "<<k<<endl;
20731                 dm = 1.0e23;
20732                 for(int i=1; i<ln; i++) {
20733                         for(int j=0; j<i; j++) {
20734                                 //qd = (*d)(mono(active[i],active[j]));
20735                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
20736                                 if(qd < dm) {
20737                                         dm = qd;
20738                                         ppi = i;
20739                                         ppj = j;
20740                                 }
20741                         }
20742                 }
20743                 group[2*k]   = float(active[ppi]);
20744                 group[1+2*k] = float(active[ppj]);
20745                 //  Shorten the list
20746                 if(ppi > ln-3 || ppj > ln - 3) {
20747                         if(ppi > ln-3 && ppj > ln - 3) {
20748                         } else if(ppi > ln-3) {
20749                                 if(ppi == ln -1) active[ppj] = active[ln-2];
20750                                 else             active[ppj] = active[ln-1];
20751                         } else { // ppj>ln-3
20752                                 if(ppj == ln -1) active[ppi] = active[ln-2];
20753                                 else             active[ppi] = active[ln-1];
20754                         }
20755                 } else {
20756                         active[ppi] = active[ln-1];
20757                         active[ppj] = active[ln-2];
20758                 }
20759                 ln = ln - 2;
20760         }
20761         return  group;
20762 }
20763 
20764 */
20765 #define data(i,j) group[i*ny+j]
20766 vector<float> Util::vareas(EMData* d) {
20767         const float step=0.001f;
20768         int ny = d->get_ysize();
20769         //  input emdata should have size 2xN, where N is number of points
20770         //  output vector should be 2xN, first element is the number of elements
20771         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
20772         vector<float> group(2*ny);
20773         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20774         int K = int(1.0f/step) +1;
20775         int hit = 0;
20776         for(int kx=0; kx<=K; kx++) {
20777                 float tx = kx*step;
20778                 for(int ky=0; ky<=K; ky++) {
20779                         float ty = ky*step;
20780                         float dm = 1.0e23f;
20781                         for(int i=0; i<ny; i++) {
20782                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20783                                 if( qd < dm) {
20784                                         dm = qd;
20785                                         hit = i;
20786                                 }
20787                         }
20788                         data(0,hit) += 1.0f;
20789                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
20790                 }
20791         }
20792         return  group;
20793 }
20794 #undef data
20795 
20796 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20797 
20798         int nx = vol->get_xsize();
20799         int ny = vol->get_ysize();
20800         int nz = vol->get_zsize();
20801         float *vol_data = vol->get_data();
20802         int new_nx, new_ny;
20803 
20804         if (nz == 1)
20805                 throw ImageDimensionException("Error: Input must be a 3-D object");
20806         if ((dim < 1) || (dim > 3))
20807                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20808         if (((dim == 1) && (index < 0 || index > nx-1)) ||
20809           ((dim == 1) && (index < 0 || index > nx-1)) ||
20810           ((dim == 1) && (index < 0 || index > nx-1)))
20811                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20812 
20813         if (dim == 1) {
20814                 new_nx = ny;
20815                 new_ny = nz;
20816         } else if (dim == 2) {
20817                 new_nx = nx;
20818                 new_ny = nz;
20819         } else {
20820                 new_nx = nx;
20821                 new_ny = ny;
20822         }
20823 
20824         EMData *slice = new EMData();
20825         slice->set_size(new_nx, new_ny, 1);
20826         float *slice_data = slice->get_data();
20827 
20828         if (dim == 1) {
20829                 for (int x=0; x<new_nx; x++)
20830                         for (int y=0; y<new_ny; y++)
20831                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20832         } else if (dim == 2) {
20833                 for (int x=0; x<new_nx; x++)
20834                         for (int y=0; y<new_ny; y++)
20835                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20836         } else {
20837                 for (int x=0; x<new_nx; x++)
20838                         for (int y=0; y<new_ny; y++)
20839                                 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20840         }
20841 
20842         return slice;
20843 }
20844 
20845 void Util::image_mutation(EMData *img, float mutation_rate) {
20846         int nx = img->get_xsize();
20847         float min = img->get_attr("minimum");
20848         float max = img->get_attr("maximum");
20849         float* img_data = img->get_data();
20850         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20851         return;
20852 }
20853 
20854 
20855 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20856 
20857         if (is_mirror != 0) {
20858                 for (int i=0; i<len_list; i++) {
20859                         int r = rand()%10000;
20860                         float f = r/10000.0f;
20861                         if (f < mutation_rate) list[i] = 1-list[i];
20862                 }
20863         } else {
20864                 map<int, vector<int> >  graycode;
20865                 map<vector<int>, int> rev_graycode;
20866                 vector <int> gray;
20867 
20868                 int K=1;
20869                 for (int i=0; i<L; i++) K*=2;
20870 
20871                 for (int k=0; k<K; k++) {
20872                         int shift = 0;
20873                         vector <int> gray;
20874                         for (int i=L-1; i>-1; i--) {
20875                                 int t = ((k>>i)%2-shift)%2;
20876                                 gray.push_back(t);
20877                                 shift += t-2;
20878                         }
20879                         graycode[k] = gray;
20880                         rev_graycode[gray] = k;
20881                 }
20882 
20883                 float gap = (K-1)/(max_val-min_val);
20884                 for (int i=0; i<len_list; i++) {
20885                         float val = list[i];
20886                         if (val < min_val) { val = min_val; }
20887                         else if  (val > max_val) { val = max_val; }
20888                         int k = int((val-min_val)*gap+0.5);
20889                         vector<int> gray = graycode[k];
20890                         bool changed = false;
20891                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20892                                 int r = rand()%10000;
20893                                 float f = r/10000.0f;
20894                                 if (f < mutation_rate) {
20895                                         *p = 1-*p;
20896                                         changed = true;
20897                                 }
20898                         }
20899                         if (changed) {
20900                                 k = rev_graycode[gray];
20901                                 list[i] = k/gap+min_val;
20902                         }
20903                 }
20904         }
20905 
20906 }
20907 
20908 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20909 
20910         if (is_mirror != 0) {
20911                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20912                         int r = rand()%10000;
20913                         float f = r/10000.0f;
20914                         if (f < mutation_rate) *q = 1-*q;
20915                 }
20916         } else {
20917                 map<int, vector<int> >  graycode;
20918                 map<vector<int>, int> rev_graycode;
20919                 vector <int> gray;
20920 
20921                 int K=1;
20922                 for (int i=0; i<L; i++) K*=2;
20923 
20924                 for (int k=0; k<K; k++) {
20925                         int shift = 0;
20926                         vector <int> gray;
20927                         for (int i=L-1; i>-1; i--) {
20928                                 int t = ((k>>i)%2-shift)%2;
20929                                 gray.push_back(t);
20930                                 shift += t-2;
20931                         }
20932                         graycode[k] = gray;
20933                         rev_graycode[gray] = k;
20934                 }
20935 
20936                 float gap = (K-1)/(max_val-min_val);
20937                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20938                         float val = *q;
20939                         if (val < min_val) { val = min_val; }
20940                         else if  (val > max_val) { val = max_val; }
20941                         int k = int((val-min_val)*gap+0.5);
20942                         vector<int> gray = graycode[k];
20943                         bool changed = false;
20944                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20945                                 int r = rand()%10000;
20946                                 float f = r/10000.0f;
20947                                 if (f < mutation_rate) {
20948                                         *p = 1-*p;
20949                                         changed = true;
20950                                 }
20951                         }
20952                         if (changed) {
20953                                 k = rev_graycode[gray];
20954                                 *q = k/gap+min_val;
20955                         }
20956                 }
20957         }
20958         return list;
20959 }
20960 
20961 
20962 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20963         //cout<<"sanitycheck called\n";
20964         int total_cost = *output;
20965         int num_matches = *(output+1);
20966 
20967         int cost=0;
20968         int* intx;
20969         int intx_size;
20970         int* intx_next(0);
20971         int intx_next_size = 0;
20972         int curclass;
20973         int curclass_size;
20974         //cout<<"cost by match: [";
20975         for(int i = 0; i < num_matches; i++){
20976                 curclass = *(output+2+ i*nParts);
20977                 // check feasibility
20978                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20979                 *(argParts + Indices[curclass]+1) = -5;
20980                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
20981                 curclass_size = *(dimClasses+curclass)-2;
20982                 intx = new int[curclass_size];
20983                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20984                 intx_size = curclass_size;
20985 
20986                 for (int j=1; j < nParts; j++){
20987                       curclass = *(output+2+ i*nParts+j);
20988                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20989                       *(argParts + Indices[j*K+curclass]+1)=-5;
20990                       // compute the intersection of intx and class curclass of partition j of the i-th match
20991                       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);
20992                       intx_next = new int[intx_next_size];
20993                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20994                       delete[] intx;
20995                       intx=intx_next;
20996                       intx_size= intx_next_size;
20997                 }
20998                 delete[] intx_next;
20999 
21000                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
21001                 //cout <<intx_next_size<<",";
21002                 cost = cost + intx_next_size;
21003         }
21004         //cout<<"]\n";
21005         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
21006 
21007         return 1;
21008 
21009 }
21010 
21011 
21012 // Given J, returns the J matches with the largest weight
21013 // matchlist has room for J matches
21014 // costlist has J elements to record cost of the J largest matches
21015 
21016 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
21017         
21018         // some temp variables
21019         bool flag = 0;
21020         int nintx;
21021         int* dummy(0);
21022         //int* ret;
21023         int* curbranch = new int[nParts];
21024         
21025         //initialize costlist to all 0
21026         for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
21027         
21028         
21029         for(int a=0; a<K; a++)
21030         {
21031         
21032                 // check that class a of partition 0 is active and has greater than T elements. If not the case, then skip to the next class
21033                 if (*(argParts + Indices[a] + 1) < 1) continue;
21034                 if (*(dimClasses + a)-2 <= T) continue;
21035 
21036                 // 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
21037 
21038                 for( int i=1; i < nParts; i++){
21039                         flag = 0; // if flag stays 0 then no class in this partition has more than T objects in common with a, which implies no feasible match (> T) with class a of part 0 is possible.
21040                         for(int j=0; j < K; j++){
21041                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
21042                                 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);
21043                                 if (nintx > T) flag=1;
21044                                 else *(argParts + Indices[i*K+j] + 1) =-4;
21045                         }
21046                         if (flag==0) {break;}
21047                 }
21048 
21049                 // explore determines J matchs with the largest weight greater than T where class in partition 0 is class a
21050                 *curbranch = a;
21051 
21052                 if (flag > 0) // Each partition has one or more active class
21053                         Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
21054                         *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
21055                         
21056                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
21057                 for( int i=1; i < nParts; i++){
21058                         for(int j=0; j < K; j++){
21059                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
21060 
21061                         }
21062                 }
21063         }
21064         
21065         delete[] curbranch;
21066 }
21067 
21068 // returns J largest matches
21069 void Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* curintx, int size_curintx, int* next, int size_next, int depth, int J, int* matchlist, int*costlist, int* curbranch){
21070 
21071 // depth is the level which is going to be explored in the current iteration
21072         int* curintx2(0);
21073         int nintx = size_curintx;
21074         
21075         
21076         // 2. take the intx of next and cur. Prune if <= T
21077         if (depth >0){
21078                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
21079                 if (nintx <= T) return; //prune!
21080         }
21081 
21082         // 1. we're at a leaf with weight > T, so determine if there is any empty space. If so, put it in. If not, determine if current cost is larger than any of the cost in matchlist, if so, replace the  smallest one in matchlist
21083         if (depth == (nParts-1)) {
21084                 
21085                 int replace = 0;
21086                 int ind_smallest = -1;
21087                 int smallest_cost = -1;
21088                 
21089                 for (int jit = 0; jit < J; jit++){
21090                         if (*(costlist+jit) < nintx){
21091                                 replace = 1;
21092                                 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21093                                 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21094                         }       
21095                 }
21096                 
21097                 if (replace > 0){
21098                         // replace the smallest cost in matchlist with the current stuff
21099                         *(costlist + ind_smallest) = nintx;
21100                         for (int xit = 0; xit < nParts; xit++)
21101                                 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
21102                                 
21103                 }
21104                 
21105                 return; 
21106         }
21107         
21108 
21109         // 3. have not yet reached a leaf, and current weight is still greather than T, so keep on going.
21110 
21111         if (depth > 0){
21112                 curintx2 = new int[nintx]; // put the intersection set in here
21113                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
21114         }
21115 
21116         if (depth == 0){
21117                 // set curintx2 to curintx
21118                 curintx2 = new int[size_curintx];
21119                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
21120         }
21121 
21122 
21123         // recursion (non-leaf case)
21124         depth=depth+1;
21125         // we now consider each of the classes in partition depth and recurse upon each of them
21126         for (int i=0; i < K; i++){
21127 
21128                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
21129                 size_next = (*(dimClasses + depth*K+i ))-2;
21130                 if (size_next <= T) continue;
21131                 *(curbranch+depth) = i;
21132                 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
21133                         costlist, curbranch);
21134                 
21135         }
21136 
21137         delete[] curintx2;
21138 }
21139 
21140 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
21141         //cout<<"initial_prune\n";
21142         // simple initial pruning. For class indClass of partition indPart:
21143         // 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
21144         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
21145 
21146         // 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
21147 
21148         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
21149         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
21150 
21151         int* dummy(0);
21152         int* cref;
21153         int cref_size;
21154         int* ccomp;
21155         int ccomp_size;
21156         int nintx;
21157         for (int i=0; i < nParts; i++){
21158                 for (int j =0; j < K; j++){
21159 
21160                         // consider class Parts[i][j]
21161                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
21162                         cref_size = dimClasses[i*K+cref[0]]-2;
21163 
21164 
21165                         if (cref_size <= T){
21166                                 cref[0] = -1;
21167                                 continue;
21168                         }
21169                         bool done = 0;
21170                         for (int a = 0; a < nParts; a++){
21171                                 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
21172                                 bool hasActive=0;
21173                                 for (unsigned int b=0; b < Parts[a].size(); b++){
21174                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
21175                                         // remember first element of each class is the index of the class
21176                                         ccomp = Parts[a][b];
21177                                         ccomp_size= dimClasses[a*K+ccomp[0]]-2;
21178                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
21179 
21180 
21181                                         if (nintx <= T)
21182                                                 ccomp[1] = 0; // class Parts[a][b] is 'inactive' for cref
21183                                         else{
21184                                                 ccomp[1] = 1; // class Parts[a][b] is 'active' for cref
21185                                                 hasActive=1;
21186                                         }
21187                                 }
21188                                 // see if partition a has at least one active class.if not then we're done with cref
21189                                 if (hasActive < 1){
21190                                    done=1;
21191                                    break;
21192                                 }
21193 
21194                         }
21195 
21196                         if (done > 0){
21197                                 // remove class j from partition i
21198 
21199                                 cref[0] = -1; // mark for deletion later
21200                                 continue; // move on to class Parts[i][j+1]
21201                         }
21202 
21203                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
21204                         // 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.
21205 
21206                         // (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.
21207                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
21208                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
21209 
21210                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
21211                         //bool found = 1;
21212                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21213 
21214                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
21215                                 // Parts[i].erase(Parts[i].begin()+j);
21216                                 cref[0] = -1;
21217                         }
21218                 }
21219 
21220                 // Erase from Parts[i] all the classes that's being designated for erasure
21221 
21222                 for (int d = K-1; d > -1; d--){
21223                         if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21224                 }
21225 
21226         }
21227         //cout <<"number of classes left in each partition after initial prune\n";      
21228         // Print out how many classes are left in each partition
21229         //for (int i =0; i < nParts; i++)
21230         //      cout << Parts[i].size()<<", ";
21231         //cout << "\n";
21232 }
21233 
21234 
21235 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) {
21236 
21237 
21238         if (size_next <= T) return 0;
21239 
21240         // take the intx of next and cur
21241         int* curintx2(0);
21242         int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21243         if (nintx <= T) return 0;
21244 
21245         int old_depth=depth;
21246         if (depth == partref) depth = depth + 1; // we skip classes in partref
21247         if (depth == nParts &&  old_depth>0) return 1;
21248 
21249         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
21250 
21251         curintx2 = new int[nintx]; // put the intersection set in here
21252         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21253 
21254         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
21255 
21256         // we now consider each of the classes in partition (depth+1) in turn
21257         bool gt_thresh;
21258         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
21259 
21260         for (int i=0; i < num_classes; i++){
21261                 if (Parts[depth][i][1] < 1) continue; // class is not active so move on
21262                 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21263                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21264                 if (gt_thresh) { delete[] curintx2; return 1; }
21265         }
21266         delete[] curintx2;
21267         return 0;
21268 }
21269 
21270 
21271 
21272 
21273 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21274 int max_branching, float stmult, int branchfunc, int LIM) {
21275         
21276         
21277         // 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
21278         // 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
21279         // Make a vector of nParts vectors of K int* each
21280          int* Indices = new int[nParts*K];
21281          int ind_c = 0;
21282          for (int i=0; i < nParts; i++){
21283                  for(int j = 0; j < K; j++){
21284                          Indices[i*K + j] = ind_c;
21285                          ind_c = ind_c + dimClasses[i*K + j];
21286                  }
21287          }
21288 
21289         // do initial pruning on argParts and return the pruned partitions
21290 
21291         // Make a vector of nParts vectors of K int* each
21292         vector <vector <int*> > Parts(nParts,vector<int*>(K));
21293         ind_c = 0;
21294         int argParts_size=0;
21295         for (int i=0; i < nParts; i++){
21296                 for(int j = 0; j < K; j++){
21297                         Parts[i][j] = argParts + ind_c;
21298                         ind_c = ind_c + dimClasses[i*K + j];
21299                         argParts_size = argParts_size + dimClasses[i*K + j];
21300                 }
21301         }
21302 
21303         // in the following we call initial_prune with Parts which is a vector. This is not the most
21304         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
21305         // 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.....
21306 
21307         // comment out for testing
21308         Util::initial_prune(Parts, dimClasses, nParts, K, T);
21309         for(int i = 0; i < nParts; i++){
21310                 for(int j=0; j < K; j++){
21311                         argParts[Indices[i*K + j]+1] = -1;
21312                 }
21313         }
21314 
21315         int num_classes;
21316         int old_index;
21317         for(int i=0; i<nParts; i++){
21318                 num_classes = Parts[i].size();// number of classes in partition i after pruning
21319                 for (int j=0; j < num_classes; j++){
21320                         old_index = Parts[i][j][0];
21321                         //cout << "old_index: " << old_index<<"\n";
21322                         argParts[Indices[i*K + old_index]+1] = 1;
21323                 }
21324         }
21325 
21326 
21327         // if we're not doing mpi then keep going and call branchMPI and return the output
21328         //cout <<"begin partition matching\n";
21329         //int* dummy(0);
21330         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21331         
21332         //cout<<"total cost: "<<*output<<"\n";
21333         //cout<<"number of matches: "<<*(output+1)<<"\n";
21334         // 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
21335         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21336 
21337         delete[] Indices;
21338 
21339         // something is wrong with output of branchMPI!
21340         if (correct < 1){
21341                 cout << "something is wrong with output of branchMPI!\n";
21342                 vector<int> ret(1);
21343                 ret[0] = -1;
21344                 if (output != 0)  { delete[] output; output = 0; }
21345                 return ret;
21346         }
21347 
21348         // output is not nonsense, so now put it into a single dimension vector and return
21349         // 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
21350         // and the rest is the list of matches. output is one dimensional
21351 
21352         int output_size = 2 + output[1] * nParts;
21353         vector<int> ret(output_size);
21354         for (int i = 0; i < output_size; i++) {
21355                 ret[i]= output[i];
21356         }
21357         if (output != 0) { delete[] output; output = 0; }
21358         return ret;
21359 
21360 }
21361 
21362 
21363 int branch_all=0;
21364 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21365 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21366 
21367 //*************************************
21368 //testing search2
21369 if (1 == 0){
21370 cout <<"begin test search2\n";
21371 int* matchlist = new int[J*nParts];
21372 int* costlist = new int[J];
21373 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21374 Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
21375 
21376 for (int jit = 0; jit < J; jit++) {
21377   cout << *(costlist +jit)<<": ";
21378   for (int yit = 0; yit < nParts; yit++)
21379         cout << *(matchlist + jit*nParts + yit)<<",";
21380   cout <<"\n";  
21381 
21382 }
21383 cout <<"end test search2\n";
21384 int* output = new int[1];
21385 output[0] = 1;
21386 delete [] matchlist;
21387 delete [] costlist;
21388 return output;
21389 }
21390 //**************************************
21391 
21392         // Base Case: we're at a leaf, no more feasible matches possible
21393         if (curlevel > K -1){
21394                 int* output = new int[2];
21395                 output[0] = 0;
21396                 output[1] = 0;
21397                 return output;
21398         }
21399 
21400         // branch dynamically depending on results of search 2!
21401         
21402         int* matchlist = new int[J*nParts];
21403         int* costlist = new int[J];
21404         Util::search2(argParts, Indices, dimClasses, nParts, K,  T, matchlist, costlist, J);
21405         
21406         
21407         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
21408         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
21409 
21410         // if there are no feasible matches with cost gt T, then return 0
21411         for (int jit = 0; jit < J ; jit++){
21412         
21413                 if (costlist[jit] > T) break;
21414                 if (jit == J-1){
21415                         int* output = new int[2];
21416                         output[0] = 0;
21417                         output[1] = 0;
21418                         delete[] matchlist;
21419                         delete[] costlist;
21420                         return output;
21421                 }
21422         }
21423         
21424 
21425         
21426         // note that costlist and matchlist are NOT sorted by weight, and branch factor takes care of that...
21427         if (curlevel==0) branch_all = 0;
21428         
21429         int nBranches = -1;
21430 
21431         if (branchfunc == 0)
21432                 nBranches = branch_factor_0(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
21433 
21434         if (branchfunc == 2)
21435                 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
21436 
21437         if (branchfunc == 3)
21438                 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
21439 
21440         if (branchfunc == 4)
21441                 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult); // branch based on distribution of top J (weighted) matches  with cost > T
21442 
21443         int* newcostlist= new int[nBranches];
21444         int* newmatchlist = new int[nBranches*nParts];
21445         for (int i=0; i<nBranches; i++){
21446                 newcostlist[i] = costlist[i];
21447                 for (int j=0; j< nParts; j++)
21448                         newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21449         }
21450 
21451         delete[] costlist;
21452         delete[] matchlist;
21453         
21454         //int* output = new int[2];//initialize to placeholder
21455         int* output = new int[2+K*nParts];//initialize to placeholder
21456         output[0] = 0;
21457         output[1] = 0;
21458         // some temporary variables
21459         int old_index;
21460         int totalcost;
21461         int nmatches;
21462         //int offset;
21463 
21464         for(int i=0; i < nBranches ; i++){
21465 
21466                 // consider the i-th match returned by findTopLargest
21467                 //if (newcostlist[i] <= T) continue;
21468 
21469                 // 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.
21470                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
21471 
21472                 for(int j=0; j < nParts; j++){
21473                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
21474                         old_index = newmatchlist[i*nParts + j];
21475                         argParts[Indices[j*K+old_index] + 1] = -2;
21476                 }
21477 
21478                 
21479                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21480                 J, max_branching, stmult,branchfunc, LIM);
21481                 
21482                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
21483                 totalcost = newcostlist[i] + ret[0];
21484 
21485                 //if (curlevel == 0) {
21486                 //      cout <<"totalcost*****************************************************************: "<<totalcost<<", costlist["<<i<<"]="<<newcostlist[i]<<", *ret="<<*ret<<", level: "<<curlevel<<"\n";
21487                         
21488                 //}
21489                 if (totalcost > output[0]) // option 1
21490                 {
21491                         nmatches = 1 + ret[1];
21492                         //delete[] output; // get rid of the old maxreturn
21493                         //output = new int[2+nmatches*nParts];
21494                         output[0] = totalcost;
21495                         output[1] = nmatches;
21496                         int nret = 2+(nmatches-1)*nParts;
21497                         for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21498                         for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21499                 }
21500 
21501 
21502                 delete[] ret;
21503 
21504                 // unmark the marked classes in preparation for the next iteration
21505 
21506                 for(int j=0; j < nParts; j++){
21507                         old_index = newmatchlist[i*nParts + j];
21508                         argParts[Indices[j*K+old_index] + 1] = 1;
21509                 }
21510 
21511         }
21512 
21513         delete[] newmatchlist;
21514         delete[] newcostlist;
21515         
21516         return output;
21517 }
21518 
21519 int* costlist_global;
21520 // make global costlist
21521 bool jiafunc(int i, int j){
21522         return (costlist_global[j] < costlist_global[i]) ;
21523 
21524 }
21525 // Given J matches, branch always on the first one (i.e., the one with the largest weight, so the worst case we just end up doing greedy).
21526 // Branch on the second one only if it is INFEASIBLE with the first, so you know it will never occur in any branching beginning with the first.
21527 // Branch on subsequent ones only if its infeasible with ALL the ones which we have previously decided to branch on.
21528 // The other option is to use LIM - so we branch on a match if its infeasible with at least LIM matches which we have previously decoded to branch on.
21529 // For now, LIM is defaulted to -1, which means we branch on a match only if it is infeasible to ALL matches we have previously decided to branch on.
21530 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21531         
21532         int ntot=0;
21533         for (int jit=0; jit < J; jit++){
21534                 if (*(costlist+jit) > T) ntot++;
21535         }
21536 
21537         int cur;
21538         // sort matchlist by cost
21539         int* indx = new int[J];
21540         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21541         vector<int> myindx (indx, indx+J);
21542         vector<int>::iterator it;
21543         costlist_global=costlist;
21544         sort(myindx.begin(), myindx.end(), jiafunc);
21545 
21546         // put matchlist in the order of mycost
21547         int* templist = new int[J];
21548         int* temp2list = new int[J*nParts];
21549         int next = 0;
21550         
21551         for (it=myindx.begin(); it!=myindx.end();++it){
21552                 cur = *(costlist + *it);
21553                 if (cur > T){
21554                         
21555                         templist[next] = cur;
21556                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21557                         next = next + 1;
21558                 }
21559         }
21560         
21561         for (int jit=0; jit < ntot; jit++){
21562                 *(costlist+jit)=*(templist + jit);
21563                 //cout <<*(costlist+jit)<<", ";
21564                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21565         }
21566         //cout <<"\n";
21567         
21568         delete [] indx;
21569         //compute the average 
21570         
21571         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21572         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21573         
21574         
21575         int B=1;
21576         int B_init=B;
21577         int infeasible=0;
21578         
21579         for (int i=B_init; i<ntot; i++){
21580                 if (i==ntot) continue;
21581                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21582                 // branch on
21583                 infeasible = 0;
21584                 if (LIM < 0) LIM = B;
21585                 for (int j=0; j<B; j++){
21586                         
21587                         for (int vit=0; vit<nParts; vit++){
21588                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21589                         }
21590                         if (infeasible >= LIM) break;
21591                 }
21592                 
21593                 if (infeasible >= LIM){
21594                         *(costlist+B)=*(templist+i);
21595                         for (int vit=0; vit < nParts; vit++)
21596                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21597                         B=B+1;  
21598                 }
21599         }
21600         
21601         delete [] templist;
21602         delete [] temp2list;
21603         //cout<<"**************************************** "<<B<<" ***************************\n";
21604         
21605         if (branch_all < max_branching){
21606                 if (B>1)
21607                         {branch_all = branch_all + B -1 ; }
21608         }
21609         else B=1;
21610         
21611         return B;
21612         
21613 
21614 }
21615 
21616 
21617 // similar to branch_factor_2 except we branch on a match if it is infeasible with all other matches in matchlist (not just the ones we branch on). LIM plays similar role here.
21618 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21619         
21620         int ntot=0;
21621         for (int jit=0; jit < J; jit++){
21622                 if (*(costlist+jit) > T) ntot++;
21623         }
21624 
21625         int cur;
21626         // sort matchlist by cost
21627         int* indx = new int[J];
21628         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21629         vector<int> myindx (indx, indx+J);
21630         vector<int>::iterator it;
21631         costlist_global=costlist;
21632         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21633 
21634         // put matchlist in the order of mycost
21635         int* templist = new int[J];
21636         int* temp2list = new int[J*nParts];
21637         int next = 0;
21638         
21639         for (it=myindx.begin(); it!=myindx.end();++it){
21640                 cur = *(costlist + *it);
21641                 if (cur > T){
21642                         
21643                         templist[next] = cur;
21644                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21645                         next = next + 1;
21646                 }
21647         }
21648         
21649         for (int jit=0; jit < ntot; jit++){
21650                 *(costlist+jit)=*(templist + jit);
21651                 //cout <<*(costlist+jit)<<", ";
21652                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21653         }
21654         //cout <<"\n";
21655         
21656         delete [] indx;
21657         //compute the average 
21658         
21659         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21660         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21661         
21662         
21663         int B=1;
21664         int B_init=B;
21665         int infeasible=0;
21666         // if we're near the bottom of the tree then explore more... this is because the larger weights are not likely to change much,
21667         // whereas the smaller ones can have many permutations
21668         if (LIM < 0) LIM = ntot-1;
21669         for (int i=B_init; i<ntot; i++){
21670                 if (i==ntot) continue;
21671                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21672                 // branch on
21673                 infeasible = 0;
21674                 
21675                 for (int j=0; j<ntot; j++){
21676                         if (j == i) continue;
21677                         for (int vit=0; vit<nParts; vit++){
21678                                 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21679                         }
21680                         if (infeasible >= LIM) break;
21681                 }
21682                 
21683                 if (infeasible >= LIM){
21684                         *(costlist+B)=*(templist+i);
21685                         for (int vit=0; vit < nParts; vit++)
21686                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21687                         B=B+1;  
21688                 }
21689         }
21690         
21691         delete [] templist;
21692         delete [] temp2list;
21693         //cout<<"**************************************** "<<B<<" ***************************\n";
21694         
21695         
21696         if (branch_all < max_branching){
21697                 if (B>1)
21698                         {branch_all = branch_all + B-1;}
21699         }
21700         else B=1;
21701         
21702         return B;
21703         
21704 
21705 }
21706 
21707 // We branch based on distribution of the cost of the J largest matches. Roughly speaking, if there is a match which has significantly larger weight than others, then we branch just on that
21708 // match. Otherwise, we branch on similar weighted matches.
21709 // As before we always branch on the match with the largest cost so worst case we'll get greedy.
21710 // We compute standard dev of the J costs, and then if the difference between the cost of a match and the largest cost is within stmult*standard dev, then we branch on it.
21711 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21712         int sum=0;
21713         float average =0;
21714         int ntot=0;
21715         for (int jit=0; jit < J; jit++){
21716                 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21717         }
21718         average = ((float)sum)/((float)ntot);
21719         int cur;
21720         // sort matchlist by cost
21721         int* indx = new int[J];
21722         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21723         vector<int> myindx (indx, indx+J);
21724         vector<int>::iterator it;
21725         costlist_global=costlist;
21726         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21727 
21728         // put matchlist in the order of mycost
21729         int* templist = new int[J];
21730         int* temp2list = new int[J*nParts];
21731         int next = 0;
21732         
21733         for (it=myindx.begin(); it!=myindx.end();++it){
21734                 cur = *(costlist + *it);
21735                 if (cur > T){
21736                         
21737                         templist[next] = cur;
21738                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21739                         next = next + 1;
21740                 }
21741         }
21742         
21743         for (int jit=0; jit < ntot; jit++){
21744                 *(costlist+jit)=*(templist + jit);
21745                 //cout <<*(costlist+jit)<<", ";
21746                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21747         }
21748         //cout <<"\n";
21749         
21750         delete [] indx;
21751         delete [] templist;
21752         delete [] temp2list;
21753         
21754         if (ntot == 1) return 1;
21755         
21756         // look at the average, standard dev etc. If standard dev very small, i.e., costs very similar, then branch on the similar
21757         // costs
21758         float sq_sum=0.0;
21759         //cout <<"costlist:";
21760         for (int i=0; i< ntot; i++){
21761                 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21762                 //cout <<*(costlist+i)<<", ";
21763         }       
21764         //cout <<"\n";
21765         
21766         float variance = sq_sum/ntot;
21767         float stdev = (float)pow((float)variance,(float)0.5);
21768         
21769         //cout <<"stdev: "<<int(stdev)<<"\n";
21770         
21771         int B=1;
21772         int largest = *costlist;
21773         //cout <<"largest: "<<largest<<"\n";
21774         for (int i=1; i<ntot; i++){
21775                 int cur = *(costlist+i);
21776                 if (largest-cur < (float)(stdev*stmult)) B++;
21777                 else break;
21778         
21779         }
21780         //cout <<"B: "<<B<<"\n";
21781         if (branch_all < max_branching){
21782                 if (B>1)
21783                         {branch_all = branch_all + B-1;}
21784         }
21785         else B=1;
21786         
21787         return B;
21788         
21789 
21790 }
21791 
21792 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21793         
21794         int ntot=0;
21795         for (int jit=0; jit < J; jit++){
21796                 if (*(costlist+jit) > T) ntot++;
21797         }
21798 
21799         int cur;
21800         // sort matchlist by cost
21801         int* indx = new int[J];
21802         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21803         vector<int> myindx (indx, indx+J);
21804         vector<int>::iterator it;
21805         costlist_global=costlist;
21806         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21807 
21808         // put matchlist in the order of mycost
21809         int* templist = new int[J];
21810         int* temp2list = new int[J*nParts];
21811         int next = 0;
21812         
21813         for (it=myindx.begin(); it!=myindx.end();++it){
21814                 cur = *(costlist + *it);
21815                 if (cur > T){
21816                         
21817                         templist[next] = cur;
21818                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21819                         next = next + 1;
21820                 }
21821         }
21822         
21823         for (int jit=0; jit < ntot; jit++){
21824                 *(costlist+jit)=*(templist + jit);
21825                 //cout <<*(costlist+jit)<<", ";
21826                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21827         }
21828         //cout <<"\n";
21829         
21830         for (int jit=1; jit < ntot; jit++){
21831         
21832              if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21833              
21834                      for (int vit=0; vit < nParts; vit++){
21835                              if ( matchlist[jit*nParts + vit] >  matchlist[vit])
21836                                  break;
21837                              if ( matchlist[jit*nParts + vit] ==  matchlist[vit])
21838                                  continue;
21839                              if ( matchlist[jit*nParts + vit] <  matchlist[vit])
21840                              {
21841                                  // swap
21842                                  for (int swp=0; swp < nParts; swp++){
21843                                        int tmp  = matchlist[swp];
21844                                        matchlist[swp]= matchlist[jit*nParts + swp];
21845                                        matchlist[jit*nParts + swp] = tmp;
21846                                  }
21847                                  break;
21848                              
21849                              }   
21850                      }
21851              }
21852         
21853         }
21854         
21855         
21856         delete [] indx;
21857         //compute the average 
21858         
21859         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21860         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21861         
21862         
21863         int B=1;
21864         int B_init=B;
21865         int infeasible=0;
21866         
21867         for (int i=B_init; i<ntot; i++){
21868                 if (i==ntot) continue;
21869                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21870                 // branch on
21871                 infeasible = 0;
21872                 if (LIM < 0) LIM = B;
21873                 for (int j=0; j<B; j++){
21874                         
21875                         for (int vit=0; vit<nParts; vit++){
21876                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21877                         }
21878                         if (infeasible >= LIM) break;
21879                 }
21880                 
21881                 if (infeasible >= LIM){
21882                         *(costlist+B)=*(templist+i);
21883                         for (int vit=0; vit < nParts; vit++)
21884                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21885                         B=B+1;  
21886                 }
21887         }
21888         
21889         delete [] templist;
21890         delete [] temp2list;
21891         //cout<<"**************************************** "<<B<<" ***************************\n";
21892         
21893         if (branch_all < max_branching){
21894                 if (B>1)
21895                         {branch_all = branch_all + B -1 ; }
21896         }
21897         else B=1;
21898         
21899         return B;
21900         
21901 
21902 }
21903 
21904 
21905 Dict Util::get_transform_params(EMData* image, string xform, string convention)
21906 {
21907         const Transform * transform = image->get_attr(xform);
21908         Dict transform_params = transform->get_params(convention);
21909         delete transform;
21910         return transform_params;
21911 }
21912 
21913 
21914 static void compose_transform2(float psi1, float sx1, float sy1, float psi2, float sx2, float sy2, float & out_psi, float & out_sx, float & out_sy)
21915 {
21916         Dict t_params;
21917         t_params["type"]   = "2D";
21918         t_params["alpha"]  = psi1;
21919     t_params["tx"]     = sx1;
21920     t_params["ty"]     = sy1;
21921     t_params["mirror"] = 0;
21922     t_params["scale"]  = 1.0;
21923         Transform t1(t_params);
21924         t_params["alpha"]  = psi2;
21925     t_params["tx"]     = sx2;
21926     t_params["ty"]     = sy2;
21927         Transform t2(t_params);
21928         Transform tt = t2*t1;
21929         Dict d = tt.get_params("2D");
21930         out_psi = d["alpha"];
21931         out_sx  = d["tx"];
21932         out_sy  = d["ty"];
21933 }
21934 
21935 void Util::constrained_helix( vector<EMData*> data, vector<EMData*> fdata, vector<EMData*> refproj, vector<EMData*> rotproj
21936                 , vector<float> dp_dphi_rise_delta, vector<int> nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc
21937                 , bool FindPsi, float psi_max, vector<EMData*> crefim, vector<int> numr, int maxrin, string mode, int cnx, int cny)
21938 {
21939         if (dp_dphi_rise_delta.size() < 4) {
21940                 printf("Not enough parameters (dp_dphi_rise_delta)");
21941                 return;
21942         }
21943         if (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc.size() < 9) {
21944                 printf("Not enough parameters (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc)");
21945                 return;
21946         }
21947         float dpsi;
21948         //float dp    = dp_dphi_rise_delta[0];
21949         float dphi  = dp_dphi_rise_delta[1];
21950         int   rise  = static_cast<int>(dp_dphi_rise_delta[2] + 0.2);
21951         float delta = dp_dphi_rise_delta[3];
21952         int  nphi      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[0];
21953         int  phiwobble = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[1];
21954         int  range     = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[2];
21955         int  ywobble   = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[3];
21956         bool Dsym      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[4];
21957         int  nwx       = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[5];
21958         int  nwy       = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[6];
21959         int  nwxc      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[7];
21960         int  nwyc      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[8];
21961 
21962         const int ndata = data.size();
21963 
21964         std::vector< boost::shared_ptr<EMData> > objectsToDelete; // objects added to this vector are automatically deleted at the end of this function
21965 
21966         vector<float> c0 = data[0]->get_attr("ptcl_source_coord");
21967         vector< vector<EMData*> > ccfs(ndata, vector<EMData*>(nphi));
21968         vector< vector<EMData*> > ccfr(0);
21969         if (! Dsym) {
21970                 ccfr.resize(ndata, vector<EMData*>(nphi));
21971         }
21972         for (int im = 0; im < ndata; ++im) {
21973                 for (int iphi = 0; iphi < nphi; ++iphi) {
21974                         std::auto_ptr<EMData> corr( correlation( refproj[iphi], fdata[im], CIRCULANT, true) );
21975                         ccfs[im][iphi] = Util::window( corr.get(), nwx, nwy);
21976                         objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfs[im][iphi]));
21977                         if (! Dsym) {
21978                                 std::auto_ptr<EMData> corr2( correlation( rotproj[iphi], fdata[im], CIRCULANT, true) );
21979                                 ccfr[im][iphi] = Util::window( corr2.get(), nwx, nwy);
21980                                 objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfr[im][iphi]));
21981                         }
21982                 }
21983         }
21984         vector<float> dxshiftlocal(ndata, 0);
21985         vector<float> dyshiftlocal(ndata, 0);
21986         vector<float> dphilocal(ndata, 0);
21987 
21988         vector<float> xshiftlocal(ndata, 0);
21989         vector<float> xrshiftlocal(ndata, 0);
21990         vector<float> mxshiftlocal(ndata, 0);
21991         vector<float> yshiftlocal(ndata, 0);
21992         vector<float> yrshiftlocal(ndata, 0);
21993         vector<float> myshiftlocal(ndata, 0);
21994         vector<float> philocal(ndata, 0);
21995         vector<float> phirlocal(ndata, 0);
21996         vector<float> mphilocal(ndata, 0);
21997         float dirma = -1.0e23f;
21998         for (int idir = -1; idir < 2; idir += 2) {
21999                 float tmax = -1.0e23f;
22000                 float mpsi;
22001                 for (int ix = 1; ix < nwx-1; ++ix) {                                         //#  X shift
22002                         //#cout << "im: ", len(ccfs), ix,time()-start_time
22003                         int six = ix - nwxc;
22004                         for (int iy = 1+ywobble; iy < nwy-ywobble-1; ++iy) {                     //#  Y shift
22005                                 int siy = iy - nwyc;
22006                                 yshiftlocal[0] = float(iy-nwyc);
22007                                 yrshiftlocal[0] = float(iy-nwyc);
22008                                 for (int iphi = 0; iphi < nphi; ++iphi) {                            //#  phi search
22009                                         float qphi = iphi*delta;
22010                                         philocal[0]  = qphi;
22011                                         phirlocal[0] = fmod( 180.0f - qphi + ((int)(fabs((180.0f-qphi)/360.0f))+1)*360.0f , 360.0f );
22012                                         //# we use the first segment as a reference, so there is no interpolation, just copy the correlation
22013                                         //#  Select largest correlation within +/- range pixels from the location we explore
22014                                         float mxm = -1.0e23f;
22015                                         float mxr;
22016                                         for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {    //#  X wobble
22017                                                 float qcf = ccfs[0][iphi]->get_value_at(iux,iy);
22018                                                 if (qcf > mxm) {
22019                                                         mxm = qcf;
22020                                                         xshiftlocal[0] = float(iux-nwxc);
22021                                                 }
22022                                         }
22023                                         if (! Dsym) {
22024                                                 mxr = -1.0e23f;
22025                                                 for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {     //# Xr wobble
22026                                                         float qcf = ccfr[0][iphi]->get_value_at(iux,iy);
22027                                                         if (qcf > mxr) {
22028                                                                 mxr = qcf;
22029                                                                 xrshiftlocal[0] = float(iux-nwxc);
22030                                                         }
22031                                                 }
22032                                         }
22033                                         for ( int im = 1; im < ndata; ++im) {                                                                             //#  predicted locations
22034                                                 //# dst is distance between segment 0 and current segment in pixels
22035                                                 vector<float> cim = data[im]->get_attr("ptcl_source_coord");
22036                                                 float dst = sqrt( (c0[0] - cim[0])*(c0[0] - cim[0]) + (c0[1] - cim[1])*(c0[1] - cim[1]));
22037                                                 //# predict for all remaining segments assuming number 0
22038                                                 qphi = idir*(dst/rise)*dphi;
22039                                                 float pphi = fmod(philocal[0] + qphi + ((int)(abs(qphi/360.0f))+1)*360.0f , 360.0f);                          //#  predicted phi with full angular accuracy, not an integer
22040                                                 int pix = six; //# predicted x shift
22041                                                 int piy = siy; //#  predicted y shift
22042                                                 int xix = pix + nwxc;
22043                                                 int yiy = piy + nwyc;
22044                                                 //#  Local x search
22045                                                 int fix = int(xix);
22046                                                 float xdif = xix - fix;
22047                                                 float xrem = 1.0f - xdif;
22048                                                 int fiy = int(yiy);
22049                                                 float ydif = yiy - fiy;
22050                                                 float yrem = 1.0f - ydif;
22051                                                 float ciq = -1.0e23f;
22052                                                 //# interpolate correlation at pphi
22053                                                 qphi = pphi/delta;
22054                                                 int ttphi = (int( qphi +  ((int)(abs(qphi/nphi))+1)*nphi+ 0.5))%nphi;
22055                                                 for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {                                               //#  phi wobble
22056                                                         int tphi = (ttphi+lphi+nphi)%nphi;
22057                                                         for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {                       //#  X wobble
22058                                                                 for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {               //#  Y wobble
22059                                                                         float qcf = xrem*yrem*ccfs[im][tphi]->get_value_at(iux,iuy)
22060                                                                                                 + xdif*yrem*ccfs[im][tphi]->get_value_at(iux+1,iuy)
22061                                                                                                 + xrem*ydif*ccfs[im][tphi]->get_value_at(iux,iuy+1)
22062                                                                                                 + xdif*ydif*ccfs[im][tphi]->get_value_at(iux+1,iuy+1);
22063                                                                         if (qcf > ciq) {
22064                                                                                 ciq = qcf;
22065                                                                                 xshiftlocal[im] = iux + xdif - nwxc;
22066                                                                                 yshiftlocal[im] = iuy + ydif - nwyc;
22067                                                                                 philocal[im]    = tphi * delta;
22068                                                                         }
22069                                                                 }
22070                                                         }
22071                                                 }
22072                                                 mxm += ciq;
22073                                                 //# now for rotated
22074                                                 if (! Dsym) {
22075                                                         qphi = idir*(dst/rise)*dphi;
22076                                                         pphi = fmod(phirlocal[0] + qphi + ((int)(abs(qphi/360.0f))+1)*360.0f, 360.0f);                      //#  predicted phi for rotated 180 defs with full angular accuracy, not an integer
22077                                                         pix = six; //# predicted x shift
22078                                                         piy = siy; //#  predicted y shift
22079                                                         xix = pix + nwxc;
22080                                                         yiy = piy + nwyc;
22081                                                         fix = int(xix);
22082                                                         xdif = xix - fix;
22083                                                         xrem = 1.0f - xdif;
22084                                                         fiy = int(yiy);
22085                                                         ydif = yiy - fiy;
22086                                                         yrem = 1.0f - ydif;
22087                                                         ciq = -1.0e23f;
22088                                                         //# interpolate correlation at pphi
22089                                                         for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {                                           //#  phi wobble
22090                                                                 qphi = lphi*delta;
22091                                                                 float qtphi = fmod( pphi + qphi + (int(fabs(qphi/360.0f))+1)*360.0f , 360.0f);
22092                                                                 qphi = fmod(540.0f-qtphi, 360.0f) / delta;
22093                                                                 int tphi = (int( qphi + (int(fabs(qphi/nphi))+1)*nphi  + 0.5))%nphi;
22094                                                                 for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {                   //#  X wobble
22095                                                                         for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {           //#  Y wobble
22096                                                                                 float qcf = xrem*yrem*ccfr[im][tphi]->get_value_at(iux,iuy)
22097                                                                                                         + xdif*yrem*ccfr[im][tphi]->get_value_at(iux+1,iuy)
22098                                                                                                         + xrem*ydif*ccfr[im][tphi]->get_value_at(iux,iuy+1)
22099                                                                                                         + xdif*ydif*ccfr[im][tphi]->get_value_at(iux+1,iuy+1);
22100                                                                                 if (qcf > ciq) {
22101                                                                                         ciq = qcf;
22102                                                                                         xrshiftlocal[im] = iux + xdif - nwxc;
22103                                                                                         yrshiftlocal[im] = iuy + ydif - nwyc;
22104                                                                                         phirlocal[im]    = int(qtphi/delta+0.5f)*delta;
22105                                                                                 }
22106                                                                         }
22107                                                                 }
22108                                                         }
22109                                                         mxr += ciq;
22110                                                 } else {
22111                                                         mxr = mxm-1.e5;
22112                                                 }
22113                                         }
22114                                         if ( mxr > mxm ) {
22115                                                 if (mxr > tmax) {
22116                                                         tmax = mxr;
22117                                                         mpsi = 270.0f;
22118                                                         for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xrshiftlocal[im];
22119                                                         for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yrshiftlocal[im];
22120                                                         for (int im = 0; im < ndata; ++im) mphilocal[im]    = fmod(540.0f-phirlocal[im], 360.0f);
22121                                                 }
22122                                         } else {
22123                                                 if (mxm > tmax) {
22124                                                         tmax = mxm;
22125                                                         mpsi = 90.0f;
22126                                                         for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xshiftlocal[im];
22127                                                         for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yshiftlocal[im];
22128                                                         for (int im = 0; im < ndata; ++im) mphilocal[im]    = philocal[im];
22129                                                 }
22130                                         }
22131                                 }
22132                         }
22133                 }
22134                 if (tmax > dirma) {
22135                         dirma = tmax;
22136                         dpsi = mpsi;
22137                         for (int im = 0; im < ndata; ++im) dxshiftlocal[im] = mxshiftlocal[im];
22138                         for (int im = 0; im < ndata; ++im) dyshiftlocal[im] = myshiftlocal[im];
22139                         for (int im = 0; im < ndata; ++im) dphilocal[im]    = mphilocal[im];
22140                 }
22141         }
22142 
22143         for (int im = 0; im < ndata; ++im) {
22144                 float psx  = dxshiftlocal[im];
22145                 float psy  = dyshiftlocal[im];
22146                 float pphi = dphilocal[im];
22147                 float epsi;
22148                 float bestang;
22149                 if (FindPsi) {
22150                         float qphi = pphi/delta;
22151                         int iphi = ( int(qphi + ((int)(abs(qphi/nphi))+1)*nphi + 0.5f))%nphi ;
22152                         //#cout <<  " ref number and current parameters reduced to 2D  ",iphi,0.0, psx, psy
22153                         Dict params = Util::get_transform_params(data[im], "xform.projection", "spider");
22154                         float opsi3 = params["psi"];
22155                         float opx3 = -static_cast<float>(params["tx"]);
22156                         float opy3 = -static_cast<float>(params["ty"]);
22157                         //#cout << " old 3D params in data ",ophi, otheta, opsi3, opx3, opy3
22158                         float gamma = (abs(opsi3 - 90) < abs(opsi3 - 270)) ? (90) : (270);
22159                         float oalpha, osx, osy;
22160                         compose_transform2(0, opx3, opy3, gamma-opsi3, 0, 0, oalpha, osx, osy); //# reduce 3D to 2D
22161                         //#cout << " old 3D params, -> 2D ",oalpha, osx, osy
22162                         //# combine previous with the current in plane
22163                         //#cout << " current 2D combined with old 2D rotation",oalpha, csx, csy
22164                         //#  Find what the shift is without the angle
22165                         float junk, nnsx, nnsy;
22166                         compose_transform2(0, psx, psy, -oalpha, 0, 0, junk, nnsx, nnsy);
22167                         //#cout << " 2D shift without angle ",nnsx, nnsy
22168 
22169                         std::auto_ptr<EMData> cimage( Util::Polar2Dm(data[im], cnx+nnsx, cny+nnsy, numr, mode) );
22170                         Util::Frngs(cimage.get(), numr);
22171                         std::auto_ptr<EMData> temp( Util::Crosrng_msg_s( cimage.get(), crefim[iphi], numr) );
22172 
22173                         int ipr = int(psi_max*maxrin/360.0f + 0.5f);
22174                         int incpsi = (dpsi == 270.0f) ? (maxrin/2) : (0);
22175                         float qn = -1.0e23f;
22176                         for (int ips = -ipr; ips < ipr+1; ++ips) {
22177                                 int tot = (ips + incpsi + maxrin)%maxrin;
22178                                 float tval = temp->get_value_at(tot);
22179                                 if (tval > qn) {
22180                                         qn = tval;
22181                                         bestang = ang_n(tot+1.0f, mode, maxrin);
22182                                 }
22183                         }
22184                         //#cout << " best angle ",bestang
22185                         bestang = fmod(bestang - (dpsi-90.0f) + 720.0f, 360.0f);
22186                         //#cout << " angle applied ",bestang
22187                         //#rot_shift2D(data[im],-bestang).write_image("rotated.hdf",im)
22188                         //std::auto_ptr<EMData> rot_data_im( data[im]->rot_scale_trans2D_background(-bestang, 0, 0, 1) );
22189                         //fdata[im] = (rot_data_im->is_complex()) ? (rot_data_im->do_ift()) : (rot_data_im->do_fft());
22190 
22191                         //#cout <<  " New composed 3D  ",dpsi,bestang, nnsx, nnsy
22192 
22193                         epsi = fmod(bestang+dpsi, 360.0f);
22194                         psx = nnsx;
22195                         psy = nnsy;
22196                         //#cout <<  " New composed 3D  ",pphi,90.0,epsi, psx, psy
22197                 } else {
22198                         epsi = dpsi;
22199                         bestang = 0;
22200                 }
22201                 data[im]->set_attr("bestang",fmod(720.0f-bestang, 360.0f));
22202                 //printf("  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f\n", pphi, 90.0, epsi, psx, psy);
22203                 Dict t_params;
22204                 t_params["type"]  = "spider";
22205                 t_params["phi"]   = pphi;
22206         t_params["theta"] = 90.0f;
22207         t_params["psi"]   = epsi;
22208         t_params["tx"]    = -psx;
22209         t_params["ty"]    = -psy;
22210         Transform t(t_params);
22211         data[im]->set_attr("xform.projection", &t);
22212         }
22213 }
22214 
22215 void Util::constrained_helix_test( vector<EMData*> data, vector<EMData*> fdata, vector<EMData*> refproj, vector<EMData*> rotproj
22216                 , vector<float> dp_dphi_rise_delta, vector<int> nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc
22217                 , bool FindPsi, float psi_max, vector<EMData*> crefim, vector<int> numr, int maxrin, string mode, int cnx, int cny)
22218 {
22219         if (dp_dphi_rise_delta.size() < 4) {
22220                 printf("Not enough parameters (dp_dphi_rise_delta)");
22221                 return;
22222         }
22223         if (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc.size() < 9) {
22224                 printf("Not enough parameters (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc)");
22225                 return;
22226         }
22227         float dpsi;
22228         //float dp    = dp_dphi_rise_delta[0];
22229         float dphi  = dp_dphi_rise_delta[1];
22230         int   rise  = static_cast<int>(dp_dphi_rise_delta[2] + 0.2);
22231         float delta = dp_dphi_rise_delta[3];
22232         int  nphi      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[0];
22233         int  phiwobble = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[1];
22234         int  range     = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[2];
22235         int  ywobble   = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[3];
22236         bool Dsym      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[4];
22237         int  nwx       = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[5];
22238         int  nwy       = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[6];
22239         int  nwxc      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[7];
22240         int  nwyc      = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[8];
22241 
22242         const int ndata = data.size();
22243 
22244         std::vector< boost::shared_ptr<EMData> > objectsToDelete; // objects added to this vector are automatically deleted at the end of this function
22245 
22246         vector<float> c0 = data[0]->get_attr("ptcl_source_coord");
22247         vector< vector<EMData*> > ccfs(ndata, vector<EMData*>(nphi));
22248         vector< vector<EMData*> > ccfr(0);
22249         if (! Dsym) {
22250                 ccfr.resize(ndata, vector<EMData*>(nphi));
22251         }
22252         for (int im = 0; im < ndata; ++im) {
22253                 for (int iphi = 0; iphi < nphi; ++iphi) {
22254                         std::auto_ptr<EMData> corr( correlation( refproj[iphi], fdata[im], CIRCULANT, true) );
22255                         ccfs[im][iphi] = Util::window( corr.get(), nwx, nwy);
22256                         objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfs[im][iphi]));
22257                         if (! Dsym) {
22258                                 std::auto_ptr<EMData> corr2( correlation( rotproj[iphi], fdata[im], CIRCULANT, true) );
22259                                 ccfr[im][iphi] = Util::window( corr2.get(), nwx, nwy);
22260                                 objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfr[im][iphi]));
22261                         }
22262                 }
22263         }
22264         vector<float> dxshiftlocal(ndata, 0);
22265         vector<float> dyshiftlocal(ndata, 0);
22266         vector<float> dphilocal(ndata, 0);
22267 
22268         vector<float> xshiftlocal(ndata, 0);
22269         vector<float> xrshiftlocal(ndata, 0);
22270         vector<float> mxshiftlocal(ndata, 0);
22271         vector<float> yshiftlocal(ndata, 0);
22272         vector<float> yrshiftlocal(ndata, 0);
22273         vector<float> myshiftlocal(ndata, 0);
22274         vector<float> philocal(ndata, 0);
22275         vector<float> phirlocal(ndata, 0);
22276         vector<float> mphilocal(ndata, 0);
22277         float dirma = -1.0e23f;
22278         for (int yidir = -1; yidir < 2; yidir += 2) {
22279                 for (int idir = -1; idir < 2; idir += 2) {
22280                         float tmax = -1.0e23f;
22281                         float mpsi;
22282                         for (int ix = 1; ix < nwx-1; ++ix) {                                         //#  X shift
22283                                 //#cout << "im: ", len(ccfs), ix,time()-start_time
22284                                 int six = ix - nwxc;
22285                                 for (int iy = 1+ywobble; iy < nwy-ywobble-1; ++iy) {                     //#  Y shift
22286                                         int siy = iy - nwyc;
22287                                         yshiftlocal[0] = float(iy-nwyc);
22288                                         yrshiftlocal[0] = float(iy-nwyc);
22289                                         for (int iphi = 0; iphi < nphi; ++iphi) {                            //#  phi search
22290                                                 float qphi = iphi*delta;
22291                                                 philocal[0]  = qphi;
22292                                                 phirlocal[0] = fmod( 180.0f - qphi + ((int)(fabs((180.0f-qphi)/360.0f))+1)*360.0f , 360.0f );
22293                                                 //# we use the first segment as a reference, so there is no interpolation, just copy the correlation
22294                                                 //#  Select largest correlation within +/- range pixels from the location we explore
22295                                                 float mxm = -1.0e23f;
22296                                                 float mxr;
22297                                                 for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {    //#  X wobble
22298                                                         float qcf = ccfs[0][iphi]->get_value_at(iux,iy);
22299                                                         if (qcf > mxm) {
22300                                                                 mxm = qcf;
22301                                                                 xshiftlocal[0] = float(iux-nwxc);
22302                                                         }
22303                                                 }
22304                                                 if (! Dsym) {
22305                                                         mxr = -1.0e23f;
22306                                                         for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {     //# Xr wobble
22307                                                                 float qcf = ccfr[0][iphi]->get_value_at(iux,iy);
22308                                                                 if (qcf > mxr) {
22309                                                                         mxr = qcf;
22310                                                                         xrshiftlocal[0] = float(iux-nwxc);
22311                                                                 }
22312                                                         }
22313                                                 }
22314                                                 for ( int im = 1; im < ndata; ++im) {                                                                             //#  predicted locations
22315                                                         //# dst is distance between segment 0 and current segment in pixels
22316                                                         vector<float> cim = data[im]->get_attr("ptcl_source_coord");
22317                                                         float dst = sqrt( (c0[0] - cim[0])*(c0[0] - cim[0]) + (c0[1] - cim[1])*(c0[1] - cim[1]));
22318                                                         //# predict for all remaining segments assuming number 0
22319                                                         qphi = idir*(dst/rise)*dphi;
22320                                                         //float pphi = fmod(philocal[0] + qphi + ((int)(abs(qphi/360.0f))+1)*360.0f , 360.0f);                          //#  predicted phi with full angular accuracy, not an integer
22321                                                         int pix = six; //# predicted x shift
22322                                                         //int piy = siy; //#  predicted y shift
22323                                                         Dict pret = predict(philocal[0], siy, dst, idir, yidir, rise, dphi, true);
22324                                                         float piy = pret["predy"];
22325                                                         float pphi = pret["predphi"];
22326                                                         int xix = pix + nwxc;
22327                                                         float yiy = piy + nwyc;
22328                                                         //#  Local x search
22329                                                         int fix = int(xix);
22330                                                         float xdif = xix - fix;
22331                                                         float xrem = 1.0f - xdif;
22332                                                         int fiy = int(yiy);
22333                                                         float ydif = yiy - fiy;
22334                                                         float yrem = 1.0f - ydif;
22335                                                         float ciq = -1.0e23f;
22336                                                         //# interpolate correlation at pphi
22337                                                         qphi = pphi/delta;
22338                                                         int ttphi = (int( qphi +  ((int)(abs(qphi/nphi))+1)*nphi+ 0.5))%nphi;
22339                                                         for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {                                               //#  phi wobble
22340                                                                 int tphi = (ttphi+lphi+nphi)%nphi;
22341                                                                 for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {                       //#  X wobble
22342                                                                         for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {               //#  Y wobble
22343                                                                                 float qcf = xrem*yrem*ccfs[im][tphi]->get_value_at(iux,iuy)
22344                                                                                                         + xdif*yrem*ccfs[im][tphi]->get_value_at(iux+1,iuy)
22345                                                                                                         + xrem*ydif*ccfs[im][tphi]->get_value_at(iux,iuy+1)
22346                                                                                                         + xdif*ydif*ccfs[im][tphi]->get_value_at(iux+1,iuy+1);
22347                                                                                 if (qcf > ciq) {
22348                                                                                         ciq = qcf;
22349                                                                                         xshiftlocal[im] = iux + xdif - nwxc;
22350                                                                                         yshiftlocal[im] = iuy + ydif - nwyc;
22351                                                                                         philocal[im]    = tphi * delta;
22352                                                                                 }
22353                                                                         }
22354                                                                 }
22355                                                         }
22356                                                         mxm += ciq;
22357                                                         //# now for rotated
22358                                                         if (! Dsym) {
22359                                                                 qphi = idir*(dst/rise)*dphi;
22360                                                                 //pphi = fmod(phirlocal[0] + qphi + ((int)(abs(qphi/360.0f))+1)*360.0f, 360.0f);                      //#  predicted phi for rotated 180 defs with full angular accuracy, not an integer
22361                                                                 pix = six; //# predicted x shift
22362                                                                 //piy = siy; //#  predicted y shift
22363                                                                 Dict pret = predict(phirlocal[0], siy, dst, idir, yidir, rise, dphi, true);
22364                                                             piy = pret["predy"];
22365                                                             pphi = pret["predphi"];
22366                                                                 xix = pix + nwxc;
22367                                                                 yiy = piy + nwyc;
22368                                                                 fix = int(xix);
22369                                                                 xdif = xix - fix;
22370                                                                 xrem = 1.0f - xdif;
22371                                                                 fiy = int(yiy);
22372                                                                 ydif = yiy - fiy;
22373                                                                 yrem = 1.0f - ydif;
22374                                                                 ciq = -1.0e23f;
22375                                                                 //# interpolate correlation at pphi
22376                                                                 for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {                                           //#  phi wobble
22377                                                                         qphi = lphi*delta;
22378                                                                         float qtphi = fmod( pphi + qphi + (int(fabs(qphi/360.0f))+1)*360.0f , 360.0f);
22379                                                                         qphi = fmod(540.0f-qtphi, 360.0f) / delta;
22380                                                                         int tphi = (int( qphi + (int(fabs(qphi/nphi))+1)*nphi  + 0.5))%nphi;
22381                                                                         for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {                   //#  X wobble
22382                                                                                 for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {           //#  Y wobble
22383                                                                                         float qcf = xrem*yrem*ccfr[im][tphi]->get_value_at(iux,iuy)
22384                                                                                                                 + xdif*yrem*ccfr[im][tphi]->get_value_at(iux+1,iuy)
22385                                                                                                                 + xrem*ydif*ccfr[im][tphi]->get_value_at(iux,iuy+1)
22386                                                                                                                 + xdif*ydif*ccfr[im][tphi]->get_value_at(iux+1,iuy+1);
22387                                                                                         if (qcf > ciq) {
22388                                                                                                 ciq = qcf;
22389                                                                                                 xrshiftlocal[im] = iux + xdif - nwxc;
22390                                                                                                 yrshiftlocal[im] = iuy + ydif - nwyc;
22391                                                                                                 phirlocal[im]    = int(qtphi/delta+0.5f)*delta;
22392                                                                                         }
22393                                                                                 }
22394                                                                         }
22395                                                                 }
22396                                                                 mxr += ciq;
22397                                                         } else {
22398                                                                 mxr = mxm-1.e5;
22399                                                         }
22400                                                 }
22401                                                 if ( mxr > mxm ) {
22402                                                         if (mxr > tmax) {
22403                                                                 tmax = mxr;
22404                                                                 mpsi = 270.0f;
22405                                                                 for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xrshiftlocal[im];
22406                                                                 for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yrshiftlocal[im];
22407                                                                 for (int im = 0; im < ndata; ++im) mphilocal[im]    = fmod(540.0f-phirlocal[im], 360.0f);
22408                                                         }
22409                                                 } else {
22410                                                         if (mxm > tmax) {
22411                                                                 tmax = mxm;
22412                                                                 mpsi = 90.0f;
22413                                                                 for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xshiftlocal[im];
22414                                                                 for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yshiftlocal[im];
22415                                                                 for (int im = 0; im < ndata; ++im) mphilocal[im]    = philocal[im];
22416                                                         }
22417                                                 }
22418                                         }
22419                                 }
22420                         }
22421                         if (tmax > dirma) {
22422                                 dirma = tmax;
22423                                 dpsi = mpsi;
22424                                 for (int im = 0; im < ndata; ++im) dxshiftlocal[im] = mxshiftlocal[im];
22425                                 for (int im = 0; im < ndata; ++im) dyshiftlocal[im] = myshiftlocal[im];
22426                                 for (int im = 0; im < ndata; ++im) dphilocal[im]    = mphilocal[im];
22427                         }
22428                 }
22429         }
22430 
22431         for (int im = 0; im < ndata; ++im) {
22432                 float psx  = dxshiftlocal[im];
22433                 float psy  = dyshiftlocal[im];
22434                 float pphi = dphilocal[im];
22435                 float epsi;
22436                 float bestang;
22437                 if (FindPsi) {
22438                         float qphi = pphi/delta;
22439                         int iphi = ( int(qphi + ((int)(abs(qphi/nphi))+1)*nphi + 0.5f))%nphi ;
22440                         //#cout <<  " ref number and current parameters reduced to 2D  ",iphi,0.0, psx, psy
22441                         Dict params = Util::get_transform_params(data[im], "xform.projection", "spider");
22442                         float opsi3 = params["psi"];
22443                         float opx3 = -static_cast<float>(params["tx"]);
22444                         float opy3 = -static_cast<float>(params["ty"]);
22445                         //#cout << " old 3D params in data ",ophi, otheta, opsi3, opx3, opy3
22446                         float gamma = (abs(opsi3 - 90) < abs(opsi3 - 270)) ? (90) : (270);
22447                         float oalpha, osx, osy;
22448                         compose_transform2(0, opx3, opy3, gamma-opsi3, 0, 0, oalpha, osx, osy); //# reduce 3D to 2D
22449                         //#cout << " old 3D params, -> 2D ",oalpha, osx, osy
22450                         //# combine previous with the current in plane
22451                         //#cout << " current 2D combined with old 2D rotation",oalpha, csx, csy
22452                         //#  Find what the shift is without the angle
22453                         float junk, nnsx, nnsy;
22454                         compose_transform2(0, psx, psy, -oalpha, 0, 0, junk, nnsx, nnsy);
22455                         //#cout << " 2D shift without angle ",nnsx, nnsy
22456 
22457                         std::auto_ptr<EMData> cimage( Util::Polar2Dm(data[im], cnx+nnsx, cny+nnsy, numr, mode) );
22458                         Util::Frngs(cimage.get(), numr);
22459                         std::auto_ptr<EMData> temp( Util::Crosrng_msg_s( cimage.get(), crefim[iphi], numr) );
22460 
22461                         int ipr = int(psi_max*maxrin/360.0f + 0.5f);
22462                         int incpsi = (dpsi == 270.0f) ? (maxrin/2) : (0);
22463                         float qn = -1.0e23f;
22464                         for (int ips = -ipr; ips < ipr+1; ++ips) {
22465                                 int tot = (ips + incpsi + maxrin)%maxrin;
22466                                 float tval = temp->get_value_at(tot);
22467                                 if (tval > qn) {
22468                                         qn = tval;
22469                                         bestang = ang_n(tot+1.0f, mode, maxrin);
22470                                 }
22471                         }
22472                         //#cout << " best angle ",bestang
22473                         bestang = fmod(bestang - (dpsi-90.0f) + 720.0f, 360.0f);
22474                         //#cout << " angle applied ",bestang
22475                         //#rot_shift2D(data[im],-bestang).write_image("rotated.hdf",im)
22476                         //std::auto_ptr<EMData> rot_data_im( data[im]->rot_scale_trans2D_background(-bestang, 0, 0, 1) );
22477                         //fdata[im] = (rot_data_im->is_complex()) ? (rot_data_im->do_ift()) : (rot_data_im->do_fft());
22478 
22479                         //#cout <<  " New composed 3D  ",dpsi,bestang, nnsx, nnsy
22480 
22481                         epsi = fmod(bestang+dpsi, 360.0f);
22482                         psx = nnsx;
22483                         psy = nnsy;
22484                         //#cout <<  " New composed 3D  ",pphi,90.0,epsi, psx, psy
22485                 } else {
22486                         epsi = dpsi;
22487                         bestang = 0;
22488                 }
22489                 data[im]->set_attr("bestang",fmod(720.0f-bestang, 360.0f));
22490                 //printf("  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f\n", pphi, 90.0, epsi, psx, psy);
22491                 Dict t_params;
22492                 t_params["type"]  = "spider";
22493                 t_params["phi"]   = pphi;
22494         t_params["theta"] = 90.0f;
22495         t_params["psi"]   = epsi;
22496         t_params["tx"]    = -psx;
22497         t_params["ty"]    = -psy;
22498         Transform t(t_params);
22499         data[im]->set_attr("xform.projection", &t);
22500         }
22501 }
22502 
22503 Dict Util::predict(float phig, float yg, float dst, float sgn, float ysgn, float dpp, float dphi, bool backpred){
22504         
22505         float back = 1.0;
22506         if (!backpred)
22507                 back=-1.0;
22508         float predphi;
22509         float predy;
22510         if (fmod(dst, dpp) <= 0.5*dpp)  {
22511                 predphi = fmod( phig + back*sgn * floor(dst/dpp)* dphi, float(360.0));
22512                 predy = yg + back*ysgn*(fmod(dst, dpp));
22513                 if (predy > 0)
22514                         if (fmod(predy,dpp) > 0.5*dpp){
22515                                 predy = predy - dpp;
22516                                 predphi = fmod( (predphi + sgn*dphi),float(360.0));
22517                         }
22518                 else{
22519                         if (fmod(abs(predy), dpp) > 0.5*dpp) {
22520                                 predy = predy + dpp;
22521                                 predphi = fmod( (predphi - sgn*dphi), float(360.0));
22522                         }
22523                 }
22524                 
22525         }                               
22526         else{
22527                 predphi = fmod ( (phig + back*sgn * (floor(dst/dpp)* dphi + dphi)), float(360.0));
22528                 predy = yg + back*ysgn*(fmod(dst,dpp) - dpp) ;
22529                 if (predy > 0){
22530                         if (fmod(predy, dpp) > 0.5*dpp){
22531                                 predy = predy - dpp;
22532                                 predphi = fmod( (predphi + sgn*dphi),float(360.0));
22533                         }
22534                 }
22535                 else{
22536                         if (fmod(abs(predy), dpp) > 0.5*dpp){
22537                                 predy = predy + dpp;
22538                                 predphi = fmod( (predphi - sgn*dphi), float(360.0));
22539                         }
22540                 }
22541                 
22542         }
22543         Dict retvals;
22544         retvals["predy"] = predy;
22545         retvals["predphi"] = predphi;
22546         return retvals;
22547 }

Generated on Tue Jun 11 12:40:27 2013 for EMAN2 by  doxygen 1.4.7