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

util_sparx.cpp

Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Pawel A.Penczek, 09/09/2006 (Pawel.A.Penczek@uth.tmc.edu)
00007  * Copyright (c) 2000-2006 The University of Texas - Houston Medical School
00008  *
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  *
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  *
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  *
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  *
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  *
00034  */
00035 
00036 #ifdef _WIN32
00037 #pragma warning(disable:4819)
00038 #include <malloc.h>
00039 #endif  //_WIN32
00040 
00041 #include <cstring>
00042 #include <ctime>
00043 #include <iostream>
00044 #include <cstdio>
00045 #include <cstdlib>
00046 #include <cassert>
00047 #include <boost/format.hpp>
00048 #include "emdata.h"
00049 #include "util.h"
00050 #include "fundamentals.h"
00051 #include "lapackblas.h"
00052 #include "lbfgsb.h"
00053 using namespace EMAN;
00054 #include "steepest.h"
00055 #include "emassert.h"
00056 #include "randnum.h"
00057 
00058 #include <gsl/gsl_sf_bessel.h>
00059 #include <gsl/gsl_sf_bessel.h>
00060 #include <cmath>
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) {
03144                         circ[numr2i+1] *= w;
03145                 } else {
03146                         circ[numr2i+1] *= 0.5*w;
03147                 }
03148                 for (int j = 2+numr2i; j < numr3i+numr2i; ++j) {
03149                         circ[j] *= w;
03150                 }
03151         }
03152 }
03153 
03154 #define  b(i)            b[i-1]
03155 void Util::prb1d(double *b, int npoint, float *pos) {
03156         double  c2,c3;
03157         int     nhalf;
03158 
03159         nhalf = npoint/2 + 1;
03160         *pos  = 0.0;
03161 
03162         if (npoint == 7) {
03163                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03164                      - 6.*b(6) + 31.*b(7);
03165                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03166         }
03167         else if (npoint == 5) {
03168                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03169                    + 46.*b(5) ) / (-70.);
03170                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03171         }
03172         else if (npoint == 3) {
03173                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03174                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03175         }
03176         //else if (npoint == 9) {
03177         else  { // at least one has to be true!!
03178                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03179                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03180                      + 1092.*b(9) ) / (-4620.);
03181                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03182                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03183         }
03184         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03185 }
03186 #undef  b
03187 
03188 #define  circ1(i)        circ1[i-1]
03189 #define  circ2(i)        circ2[i-1]
03190 #define  t(i)            t[i-1]
03191 #define  q(i)            q[i-1]
03192 #define  b(i)            b[i-1]
03193 #define  t7(i)           t7[i-1]
03194 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03195         //  neg = 0 straight,  neg = 1 mirrored
03196         int nring = numr.size()/3;
03197         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03198         int maxrin = numr[numr.size()-1];
03199         double qn;   float  tot;
03200         float *circ1 = circ1p->get_data();
03201         float *circ2 = circ2p->get_data();
03202 /*
03203 c checks single position, neg is flag for checking mirrored position
03204 c
03205 c  input - fourier transforms of rings!
03206 c  first set is conjugated (mirrored) if neg
03207 c  circ1 already multiplied by weights!
03208 c       automatic arrays
03209         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03210         double precision  q(maxrin)
03211         double precision  t7(-3:3)
03212 */
03213         float *t;
03214         double t7[7], *q;
03215         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03216         float  pos;
03217 
03218 #ifdef _WIN32
03219         ip = -(int)(log((float)maxrin)/log(2.0f));
03220 #else
03221         ip = -(int) (log2(maxrin));
03222 #endif  //_WIN32
03223 
03224         q = (double*)calloc(maxrin, sizeof(double));
03225         t = (float*)calloc(maxrin, sizeof(float));
03226 
03227 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03228         for (i=1; i<=nring; i++) {
03229                 numr3i = numr(3,i);
03230                 numr2i = numr(2,i);
03231 
03232                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03233 
03234                 if (numr3i != maxrin) {
03235                          // test .ne. first for speed on some compilers
03236                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03237                         t(2)            = 0.0;
03238 
03239                         if (neg) {
03240                                 // first set is conjugated (mirrored)
03241                                 for (j=3;j<=numr3i;j=j+2) {
03242                                         jc = j+numr2i-1;
03243                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03244                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03245                                 }
03246                         } else {
03247                                 for (j=3;j<=numr3i;j=j+2) {
03248                                         jc = j+numr2i-1;
03249                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03250                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03251                                 }
03252                         }
03253                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03254                 } else {
03255                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03256                         if (neg) {
03257                                 // first set is conjugated (mirrored)
03258                                 for (j=3;j<=maxrin;j=j+2) {
03259                                         jc = j+numr2i-1;
03260                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03261                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03262                                 }
03263                         } else {
03264                                 for (j=3;j<=maxrin;j=j+2) {
03265                                         jc = j+numr2i-1;
03266                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03267                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03268                                 }
03269                         }
03270                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03271                 }
03272         }
03273 
03274         fftr_d(q,ip);
03275 
03276         qn = -1.0e20;
03277         for (j=1;j<=maxrin;j++) {
03278            if (q(j) >= qn) {
03279                   qn = q(j); jtot = j;
03280            }
03281         }
03282 
03283         for (k=-3; k<=3; k++) {
03284                 j = (jtot+k+maxrin-1)%maxrin + 1;
03285                 t7(k+4) = q(j);
03286         }
03287 
03288         prb1d(t7,7,&pos);
03289 
03290         tot = (float)jtot + pos;
03291 
03292         if (q) free(q);
03293         if (t) free(t);
03294 
03295         Dict retvals;
03296         retvals["qn"] = qn;
03297         retvals["tot"] = tot;
03298         return  retvals;
03299 }
03300 
03301 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03302    //  neg = 0 straight,  neg = 1 mirrored
03303         int nring = numr.size()/3;
03304         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03305         int maxrin = numr[numr.size()-1];
03306         double qn;   float  tot;
03307         float *circ1 = circ1p->get_data();
03308         float *circ2 = circ2p->get_data();
03309 /*
03310 c checks single position, neg is flag for checking mirrored position
03311 c
03312 c  input - fourier transforms of rings!
03313 c  first set is conjugated (mirrored) if neg
03314 c  multiplication by weights!
03315 c       automatic arrays
03316         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03317         double precision  q(maxrin)
03318         double precision  t7(-3:3)
03319 */
03320         float *t;
03321         double t7[7], *q;
03322         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03323         float  pos;
03324 
03325 #ifdef _WIN32
03326         ip = -(int)(log((float)maxrin)/log(2.0f));
03327 #else
03328         ip = -(int) (log2(maxrin));
03329 #endif  //_WIN32
03330 
03331         q = (double*)calloc(maxrin, sizeof(double));
03332         t = (float*)calloc(maxrin, sizeof(float));
03333 
03334 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03335         for (i=1;i<=nring;i++) {
03336                 numr3i = numr(3,i);
03337                 numr2i = numr(2,i);
03338 
03339                 t(1) = circ1(numr2i) * circ2(numr2i);
03340 
03341                 if (numr3i != maxrin) {
03342                         // test .ne. first for speed on some compilers
03343                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03344                         t(2)      = 0.0;
03345 
03346                         if (neg) {
03347                                 // first set is conjugated (mirrored)
03348                                 for (j=3; j<=numr3i; j=j+2) {
03349                                         jc = j+numr2i-1;
03350                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03351                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03352                                 }
03353                         } else {
03354                                 for (j=3; j<=numr3i; j=j+2) {
03355                                         jc = j+numr2i-1;
03356                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03357                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03358                                 }
03359                         }
03360                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03361                 } else {
03362                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03363                         if (neg) {
03364                                 // first set is conjugated (mirrored)
03365                                 for (j=3; j<=maxrin; j=j+2) {
03366                                         jc = j+numr2i-1;
03367                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03368                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03369                                 }
03370                         } else {
03371                                 for (j=3; j<=maxrin; j=j+2) {
03372                                 jc = j+numr2i-1;
03373                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03374                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03375                                 }
03376                         }
03377                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03378                 }
03379         }
03380 
03381         fftr_d(q,ip);
03382 
03383         qn = -1.0e20;
03384         for (j=1;j<=maxrin;j++) {
03385                 //cout << j << "  " << q(j) << endl;
03386                 if (q(j) >= qn) {
03387                         qn = q(j);
03388                         jtot = j;
03389                 }
03390         }
03391 
03392         for (k=-3; k<=3; k++) {
03393                 j = (jtot+k+maxrin-1)%maxrin + 1;
03394                 t7(k+4) = q(j);
03395         }
03396 
03397         prb1d(t7,7,&pos);
03398 
03399         tot = (float)jtot + pos;
03400 
03401         //if (q) free(q);
03402         if (t) free(t);
03403 
03404         Dict retvals;
03405         //tot = 1;
03406         //qn = q(1);
03407         retvals["qn"] = qn;
03408         retvals["tot"] = tot;
03409 
03410         if (q) free(q);
03411 
03412         return  retvals;
03413 }
03414 
03415 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03416         int nring = numr.size()/3;
03417         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03418         int maxrin = numr[numr.size()-1];
03419         double qn; float tot; double qm; float tmt;
03420         float *circ1 = circ1p->get_data();
03421         float *circ2 = circ2p->get_data();
03422 /*
03423 c
03424 c  checks both straight & mirrored positions
03425 c
03426 c  input - fourier transforms of rings!!
03427 c  circ1 already multiplied by weights!
03428 c
03429 */
03430 
03431         // dimension             circ1(lcirc),circ2(lcirc)
03432 
03433         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03434         double *t, *q, t7[7];
03435 
03436         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03437         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03438 
03439         qn  = 0.0f;
03440         qm  = 0.0f;
03441         tot = 0.0f;
03442         tmt = 0.0f;
03443 #ifdef _WIN32
03444         ip = -(int)(log((float)maxrin)/log(2.0f));
03445 #else
03446         ip = -(int)(log2(maxrin));
03447 #endif  //_WIN32
03448   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03449 
03450         //  c - straight  = circ1 * conjg(circ2)
03451         //  zero q array
03452 
03453         q = (double*)calloc(maxrin,sizeof(double));
03454 
03455         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03456         //   zero t array
03457         t = (double*)calloc(maxrin,sizeof(double));
03458 
03459    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03460         for (i=1; i<=nring; i++) {
03461 
03462                 numr3i = numr(3,i);   // Number of samples of this ring
03463                 numr2i = numr(2,i);   // The beginning point of this ring
03464 
03465                 t1   = circ1(numr2i) * circ2(numr2i);
03466                 q(1) += t1;
03467                 t(1) += t1;
03468 
03469                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03470                 if (numr3i == maxrin)  {
03471                         q(2) += t1;
03472                         t(2) += t1;
03473                 } else {
03474                         q(numr3i+1) += t1;
03475                         t(numr3i+1) += t1;
03476                 }
03477 
03478                 for (j=3; j<=numr3i; j += 2) {
03479                         jc     = j+numr2i-1;
03480 
03481 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03482 //                                ----- -----    ----- -----
03483 //                                 t1     t2      t3    t4
03484 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03485 //                                    ----- -----    ----- -----
03486 //                                     t1    t2       t3    t4
03487 
03488                         c1     = circ1(jc);
03489                         c2     = circ1(jc+1);
03490                         d1     = circ2(jc);
03491                         d2     = circ2(jc+1);
03492 
03493                         t1     = c1 * d1;
03494                         t2     = c2 * d2;
03495                         t3     = c1 * d2;
03496                         t4     = c2 * d1;
03497 
03498                         q(j)   += t1 + t2;
03499                         q(j+1) += -t3 + t4;
03500                         t(j)   += t1 - t2;
03501                         t(j+1) += -t3 - t4;
03502                 }
03503         }
03504         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03505         fftr_d(q,ip);
03506 
03507         qn  = -1.0e20;
03508         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03509                 if (q(j) >= qn) {
03510                         qn  = q(j);
03511                         jtot = j;
03512                 }
03513         }
03514 
03515         for (k=-3; k<=3; k++) {
03516                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03517                 t7(k+4) = q(j);
03518         }
03519 
03520         // interpolate
03521         prb1d(t7,7,&pos);
03522         tot = (float)(jtot)+pos;
03523         // Do not interpolate
03524         //tot = (float)(jtot);
03525 
03526         // mirrored
03527         fftr_d(t,ip);
03528 
03529         // find angle
03530         qm = -1.0e20;
03531         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03532                 if ( t(j) >= qm ) {
03533                         qm   = t(j);
03534                         jtot = j;
03535                 }
03536         }
03537 
03538         for (k=-3; k<=3; k++)  {
03539                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03540                 t7(k+4) = t(j);
03541         }
03542 
03543         // interpolate
03544 
03545         prb1d(t7,7,&pos);
03546         tmt = float(jtot) + pos;
03547         // Do not interpolate
03548         //tmt = float(jtot);
03549 
03550         free(t);
03551         free(q);
03552 
03553         Dict retvals;
03554         retvals["qn"] = qn;
03555         retvals["tot"] = tot;
03556         retvals["qm"] = qm;
03557         retvals["tmt"] = tmt;
03558         return retvals;
03559 }
03560 
03561 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03562         int nring = numr.size()/3;
03563         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03564         int maxrin = numr[numr.size()-1];
03565         double qn; float tot; double qm; float tmt;
03566         float *circ1 = circ1p->get_data();
03567         float *circ2 = circ2p->get_data();
03568 /*
03569 c
03570 c  checks both straight & mirrored positions
03571 c
03572 c  input - fourier transforms of rings!!
03573 c  circ1 already multiplied by weights!
03574 c
03575 */
03576 
03577         // dimension             circ1(lcirc),circ2(lcirc)
03578 
03579         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03580         double *t, *q;
03581 
03582         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03583         float t1, t2, t3, t4, c1, c2, d1, d2;
03584 
03585         qn  = 0.0f;
03586         qm  = 0.0f;
03587         tot = 0.0f;
03588         tmt = 0.0f;
03589 #ifdef _WIN32
03590         ip = -(int)(log((float)maxrin)/log(2.0f));
03591 #else
03592         ip = -(int)(log2(maxrin));
03593 #endif  //_WIN32
03594   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03595 
03596         //  c - straight  = circ1 * conjg(circ2)
03597         //  zero q array
03598 
03599         q = (double*)calloc(maxrin,sizeof(double));
03600 
03601         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03602         //   zero t array
03603         t = (double*)calloc(maxrin,sizeof(double));
03604 
03605    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03606         for (i=1; i<=nring; i++) {
03607 
03608                 numr3i = numr(3,i);   // Number of samples of this ring
03609                 numr2i = numr(2,i);   // The beginning point of this ring
03610 
03611                 t1   = circ1(numr2i) * circ2(numr2i);
03612                 q(1) += t1;
03613                 t(1) += t1;
03614 
03615                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03616                 if (numr3i == maxrin)  {
03617                         q(2) += t1;
03618                         t(2) += t1;
03619                 } else {
03620                         q(numr3i+1) += t1;
03621                         t(numr3i+1) += t1;
03622                 }
03623 
03624                 for (j=3; j<=numr3i; j += 2) {
03625                         jc     = j+numr2i-1;
03626 
03627 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03628 //                                ----- -----    ----- -----
03629 //                                 t1     t2      t3    t4
03630 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03631 //                                    ----- -----    ----- -----
03632 //                                     t1    t2       t3    t4
03633 
03634                         c1     = circ1(jc);
03635                         c2     = circ1(jc+1);
03636                         d1     = circ2(jc);
03637                         d2     = circ2(jc+1);
03638 
03639                         t1     = c1 * d1;
03640                         t2     = c2 * d2;
03641                         t3     = c1 * d2;
03642                         t4     = c2 * d1;
03643 
03644                         q(j)   += t1 + t2;
03645                         q(j+1) += -t3 + t4;
03646                         t(j)   += t1 - t2;
03647                         t(j+1) += -t3 - t4;
03648                 }
03649         }
03650         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03651         fftr_d(q,ip);
03652 
03653         qn  = -1.0e20;
03654 
03655         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03656         int jstep = static_cast<int>(delta/360.0*maxrin);
03657         if (jstep < 1) { jstep = 1; }
03658 
03659         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03660                 if (q(j) >= qn) {
03661                         qn  = q(j);
03662                         jtot = j;
03663                 }
03664         }
03665 
03666         //for (k=-3; k<=3; k++) {
03667         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03668         //      t7(k+4) = q(j);
03669         //}
03670 
03671         // interpolate
03672         //prb1d(t7,7,&pos);
03673         //tot = (float)(jtot)+pos;
03674         // Do not interpolate
03675         tot = (float)(jtot);
03676 
03677         // mirrored
03678         fftr_d(t,ip);
03679 
03680         // find angle
03681         qm = -1.0e20;
03682         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03683                 if ( t(j) >= qm ) {
03684                         qm   = t(j);
03685                         jtot = j;
03686                 }
03687         }
03688 
03689         //for (k=-3; k<=3; k++)  {
03690         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03691         //      t7(k+4) = t(j);
03692         //}
03693 
03694         // interpolate
03695 
03696         //prb1d(t7,7,&pos);
03697         //tmt = float(jtot) + pos;
03698         // Do not interpolate
03699         tmt = float(jtot);
03700 
03701         free(t);
03702         free(q);
03703 
03704         Dict retvals;
03705         retvals["qn"] = qn;
03706         retvals["tot"] = tot;
03707         retvals["qm"] = qm;
03708         retvals["tmt"] = tmt;
03709         return retvals;
03710 }
03711 
03712 
03713 Dict Util::Crosrng_psi_0_180_no_mirror(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03714         int nring = numr.size()/3;
03715         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03716         int maxrin = numr[numr.size()-1];
03717         double qn; float tot;
03718         float *circ1 = circ1p->get_data();
03719         float *circ2 = circ2p->get_data();
03720 
03721         // dimension             circ1(lcirc),circ2(lcirc)
03722 
03723         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03724         double  *q, t7[7];
03725 
03726         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03727         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03728 
03729         qn  = 0.0f;
03730         tot = 0.0f;
03731 #ifdef _WIN32
03732         ip = -(int)(log((float)maxrin)/log(2.0f));
03733 #else
03734         ip = -(int)(log2(maxrin));
03735 #endif  //_WIN32
03736   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03737 
03738         //  c - straight  = circ1 * conjg(circ2)
03739         //  zero q array
03740 
03741         q = (double*)calloc(maxrin,sizeof(double));
03742 
03743    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03744         for (i=1; i<=nring; i++) {
03745 
03746                 numr3i = numr(3,i);   // Number of samples of this ring
03747                 numr2i = numr(2,i);   // The beginning point of this ring
03748 
03749                 t1   = circ1(numr2i) * circ2(numr2i);
03750                 q(1) += t1;
03751 
03752                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03753                 if (numr3i == maxrin)  {
03754                         q(2) += t1;
03755                         
03756                 } else {
03757                         q(numr3i+1) += t1;
03758                 }
03759 
03760                 for (j=3; j<=numr3i; j += 2) {
03761                         jc     = j+numr2i-1;
03762 
03763 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03764 //                                ----- -----    ----- -----
03765 //                                 t1     t2      t3    t4
03766 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03767 //                                    ----- -----    ----- -----
03768 //                                     t1    t2       t3    t4
03769 
03770                         c1     = circ1(jc);
03771                         c2     = circ1(jc+1);
03772                         d1     = circ2(jc);
03773                         d2     = circ2(jc+1);
03774 
03775                         t1     = c1 * d1;
03776                         t2     = c2 * d2;
03777                         t3     = c1 * d2;
03778                         t4     = c2 * d1;
03779 
03780                         q(j)   += t1 + t2;
03781                         q(j+1) += -t3 + t4;
03782 
03783                 }
03784         }
03785         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03786 
03787         fftr_d(q,ip);
03788 
03789         int psi_range  = int(psi_max/360.0*maxrin+0.5);
03790         const int psi_0 = 0;
03791         int psi_180    = int(  180.0/360.0*maxrin+0.5);
03792 
03793         qn  = -1.0e20;
03794         for (k=-psi_range; k<=psi_range; k++) {
03795                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03796                 if (q(j) >= qn) {
03797                         qn  = q(j);
03798                         jtot = j;
03799                 }
03800         }
03801 
03802         for (k=-psi_range; k<=psi_range; k++) {
03803                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03804                 if (q(j) >= qn) {
03805                         qn  = q(j);
03806                         jtot = j;
03807                 }
03808         }
03809 
03810         for (k=-3; k<=3; k++) {
03811                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03812                 t7(k+4) = q(j);
03813         }
03814 
03815         // interpolate
03816         prb1d(t7,7,&pos);
03817         tot = (float)(jtot)+pos;
03818         // Do not interpolate
03819         //tot = (float)(jtot);
03820 
03821         free(q);
03822 
03823         Dict retvals;
03824         retvals["qn"] = qn;
03825         retvals["tot"] = tot;
03826         
03827         return retvals;
03828 }
03829 
03830 
03831 
03832 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
03833 // flag 0 - straight, 1 - mirror
03834 
03835         int nring = numr.size()/3;
03836         int maxrin = numr[numr.size()-1];
03837         double qn; float tot;
03838         float *circ1 = circ1p->get_data();
03839         float *circ2 = circ2p->get_data();
03840 
03841         double *q;
03842 
03843         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03844         float t1, t2, t3, t4, c1, c2, d1, d2;
03845 
03846         qn  = 0.0f;
03847         tot = 0.0f;
03848 #ifdef _WIN32
03849         ip = -(int)(log((float)maxrin)/log(2.0f));
03850 #else
03851         ip = -(int)(log2(maxrin));
03852 #endif  //_WIN32
03853 
03854         //  c - straight  = circ1 * conjg(circ2)
03855         //  zero q array
03856 
03857         q = (double*)calloc(maxrin,sizeof(double));
03858         int neg = 1-2*flag;
03859    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03860 
03861         for (i=1; i<=nring; i++) {
03862 
03863                 numr3i = numr(3,i);   // Number of samples of this ring
03864                 numr2i = numr(2,i);   // The beginning point of this ring
03865 
03866                 t1   = circ1(numr2i) * circ2(numr2i);
03867                 q(1) += t1;
03868 
03869                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03870                 if (numr3i == maxrin)  {
03871                         q(2) += t1;
03872                 } else {
03873                         q(numr3i+1) += t1;
03874                 }
03875 
03876                 for (j=3; j<=numr3i; j += 2) {
03877                         jc     = j+numr2i-1;
03878 
03879         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03880         //                                ----- -----    ----- -----
03881         //                                 t1     t2      t3    t4
03882 
03883                         c1     = circ1(jc);
03884                         c2     = circ1(jc+1);
03885                         d1     = circ2(jc);
03886                         d2     = circ2(jc+1);
03887 
03888                         t1     = c1 * d1;
03889                         t3     = c1 * d2;
03890                         t2     = c2 * d2;
03891                         t4     = c2 * d1;
03892 
03893                         q(j)   += t1 + t2*neg;
03894                         q(j+1) += -t3 + t4*neg;
03895                 }
03896         }
03897 
03898         fftr_d(q,ip);
03899 
03900         qn  = -1.0e20;
03901         int psi_pos = int(psi/360.0*maxrin+0.5);
03902         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03903 
03904         for (k=-psi_range; k<=psi_range; k++) {
03905                 j = ( k + psi_pos + maxrin -1 )%maxrin+1;
03906                 if (q(j) >= qn) {
03907                         qn  = q(j);
03908                         jtot = j;
03909                 }
03910         }
03911 
03912         tot = (float)(jtot);
03913         free(q);
03914 
03915         Dict retvals;
03916         retvals["qn"] = qn;
03917         retvals["tot"] = tot;
03918         return retvals;
03919 }
03920 
03921 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
03922 // Computes both straight and mirrored
03923 
03924         int nring = numr.size()/3;
03925         int maxrin = numr[numr.size()-1];
03926         double qn; float tot; double qm; float tmt;
03927         float *circ1 = circ1p->get_data();
03928         float *circ2 = circ2p->get_data();
03929 
03930         double *t, *q;
03931 
03932         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03933         float t1, t2, t3, t4, c1, c2, d1, d2;
03934 
03935         qn  = 0.0f;
03936         qm  = 0.0f;
03937         tot = 0.0f;
03938         tmt = 0.0f;
03939 #ifdef _WIN32
03940         ip = -(int)(log((float)maxrin)/log(2.0f));
03941 #else
03942         ip = -(int)(log2(maxrin));
03943 #endif  //_WIN32
03944 
03945         //  c - straight  = circ1 * conjg(circ2)
03946         //  zero q array
03947 
03948         q = (double*)calloc(maxrin,sizeof(double));
03949         
03950         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03951         //   zero t array
03952         t = (double*)calloc(maxrin,sizeof(double));
03953         
03954    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03955         
03956         for (i=1; i<=nring; i++) {
03957 
03958                 numr3i = numr(3,i);   // Number of samples of this ring
03959                 numr2i = numr(2,i);   // The beginning point of this ring
03960 
03961                 t1   = circ1(numr2i) * circ2(numr2i);
03962                 q(1) += t1;
03963                 t(1) += t1;
03964                 
03965                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03966                 if (numr3i == maxrin)  {
03967                         q(2) += t1;
03968                         t(2) += t1;
03969                 } else {
03970                         q(numr3i+1) += t1;
03971                         t(numr3i+1) += t1;
03972                 }
03973 
03974                 for (j=3; j<=numr3i; j += 2) {
03975                         jc     = j+numr2i-1;
03976 
03977         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03978         //                                ----- -----    ----- -----
03979         //                                 t1     t2      t3    t4
03980 
03981                         c1     = circ1(jc);
03982                         c2     = circ1(jc+1);
03983                         d1     = circ2(jc);
03984                         d2     = circ2(jc+1);
03985 
03986                         t1     = c1 * d1;
03987                         t3     = c1 * d2;
03988                         t2     = c2 * d2;
03989                         t4     = c2 * d1;
03990 
03991                         q(j)   += t1 + t2;
03992                         q(j+1) += -t3 + t4;
03993                         t(j)   += t1 - t2;
03994                         t(j+1) += -t3 - t4;
03995                 }
03996         }
03997 
03998         fftr_d(q,ip);
03999 
04000         qn  = -1.0e20;
04001         int psi_pos = int(psi/360.0*maxrin+0.5);
04002         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
04003 
04004         for (k=-psi_range; k<=psi_range; k++) {
04005                 j = (k+psi_pos+maxrin-1)%maxrin+1;
04006                 if (q(j) >= qn) {
04007                         qn  = q(j);
04008                         jtot = j;
04009                 }
04010         }
04011 
04012         tot = (float)(jtot);
04013         free(q);
04014 
04015     // mirrored
04016         fftr_d(t,ip);
04017 
04018         qm  = -1.0e20;
04019 
04020         for (k=-psi_range; k<=psi_range; k++) {
04021                 j = (k+psi_pos+maxrin-1)%maxrin+1;
04022                 if (t(j) >= qm) {
04023                         qm  = t(j);
04024                         jtot = j;
04025                 }
04026         }
04027 
04028         tmt = (float)(jtot);
04029         free(t);
04030 
04031         Dict retvals;
04032         retvals["qn"] = qn;
04033         retvals["tot"] = tot;
04034         retvals["qm"] = qm;
04035         retvals["tmt"] = tmt;
04036         return retvals;
04037 }
04038 
04039 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04040         int nring = numr.size()/3;
04041         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04042         int maxrin = numr[numr.size()-1];
04043         double qn; float tot;
04044         float *circ1 = circ1p->get_data();
04045         float *circ2 = circ2p->get_data();
04046 /*
04047 c
04048 c  checks only straight position
04049 c
04050 c  input - fourier transforms of rings!!
04051 c  circ1 already multiplied by weights!
04052 c
04053 */
04054 
04055         // dimension             circ1(lcirc),circ2(lcirc)
04056 
04057         // q(maxrin), t7(-3:3)  //maxrin+2 removed
04058         double *q, t7[7];
04059 
04060         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04061         float c1, c2, d1, d2, pos;
04062 
04063         qn  = 0.0;
04064         tot = 0.0;
04065 #ifdef _WIN32
04066         ip = -(int)(log((float)maxrin)/log(2.0f));
04067 #else
04068    ip = -(int)(log2(maxrin));
04069 #endif  //_WIN32
04070         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04071 
04072         //  c - straight  = circ1 * conjg(circ2)
04073         //  zero q array
04074 
04075         q = (double*)calloc(maxrin,sizeof(double));
04076 
04077                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04078         for (i=1; i<=nring; i++) {
04079 
04080                 numr3i = numr(3,i);   // Number of samples of this ring
04081                 numr2i = numr(2,i);   // The beginning point of this ring
04082 
04083                 q(1) += circ1(numr2i) * circ2(numr2i);
04084 
04085                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04086                 else  q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04087 
04088                 for (j=3; j<=numr3i; j += 2) {
04089                         jc     = j+numr2i-1;
04090 
04091 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04092 //                                ----- -----    ----- -----
04093 //                                 t1     t2      t3    t4
04094 
04095                         c1     = circ1(jc);
04096                         c2     = circ1(jc+1);
04097                         d1     = circ2(jc);
04098                         d2     = circ2(jc+1);
04099 
04100                         q(j)   += c1 * d1 + c2 * d2;
04101                         q(j+1) += -c1 * d2 + c2 * d1;
04102                 }
04103         }
04104 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04105         fftr_d(q,ip);
04106 
04107         qn  = -1.0e20;
04108         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04109                 if (q(j) >= qn) {
04110                         qn  = q(j);
04111                         jtot = j;
04112                 }
04113         }
04114 
04115         for (k=-3; k<=3; k++)  {
04116                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04117                 t7(k+4) = q(j);
04118         }
04119 
04120         // interpolate
04121         prb1d(t7,7,&pos);
04122         tot = (float)(jtot)+pos;
04123         // Do not interpolate
04124         //*tot = (float)(jtot);
04125 
04126         free(q);
04127 
04128         Dict retvals;
04129         retvals["qn"] = qn;
04130         retvals["tot"] = tot;
04131         return retvals;
04132 }
04133 
04134 #define  dout(i,j)        dout[i+maxrin*j]
04135 #define  circ1b(i)        circ1b[i-1]
04136 #define  circ2b(i)        circ2b[i-1]
04137 
04138 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04139 
04140    // dimension         circ1(lcirc),circ2(lcirc)
04141 
04142         int   ip, jc, numr3i, numr2i, i, j;
04143         float t1, t2, t3, t4, c1, c2, d1, d2;
04144 
04145         int nring = numr.size()/3;
04146         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04147         int maxrin = numr[numr.size()-1];
04148 
04149         float* circ1b = circ1->get_data();
04150         float* circ2b = circ2->get_data();
04151 
04152         // t(maxrin), q(maxrin)  // removed +2
04153         double *t, *q;
04154 
04155         q = (double*)calloc(maxrin,sizeof(double));
04156         t = (double*)calloc(maxrin,sizeof(double));
04157 
04158 #ifdef _WIN32
04159         ip = -(int)(log((float)maxrin)/log(2.0f));
04160 #else
04161         ip = -(int)(log2(maxrin));
04162 #endif  //_WIN32
04163 
04164         //  q - straight  = circ1 * conjg(circ2)
04165 
04166         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04167 
04168         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04169 
04170         for (i=1; i<=nring; i++) {
04171 
04172                 numr3i = numr(3,i);
04173                 numr2i = numr(2,i);
04174 
04175                 t1   = circ1b(numr2i) * circ2b(numr2i);
04176                 q(1) = q(1)+t1;
04177                 t(1) = t(1)+t1;
04178 
04179                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04180                 if (numr3i == maxrin)  {
04181                         q(2) += t1;
04182                         t(2) += t1;
04183                 } else {
04184                         q(numr3i+1) += t1;
04185                         t(numr3i+1) += t1;
04186                 }
04187 
04188                 for (j=3; j<=numr3i; j=j+2) {
04189                         jc     = j+numr2i-1;
04190 
04191                         c1     = circ1b(jc);
04192                         c2     = circ1b(jc+1);
04193                         d1     = circ2b(jc);
04194                         d2     = circ2b(jc+1);
04195 
04196                         t1     = c1 * d1;
04197                         t3     = c1 * d2;
04198                         t2     = c2 * d2;
04199                         t4     = c2 * d1;
04200 
04201                         q(j)   += t1 + t2;
04202                         q(j+1) += - t3 + t4;
04203                         t(j)   += t1 - t2;
04204                         t(j+1) += - t3 - t4;
04205                 }
04206         }
04207 
04208         // straight
04209         fftr_d(q,ip);
04210 
04211         // mirrored
04212         fftr_d(t,ip);
04213 
04214         EMData* out = new EMData();
04215         out->set_size(maxrin,2,1);
04216         float *dout = out->get_data();
04217         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04218         //out->set_size(maxrin,1,1);
04219         //float *dout = out->get_data();
04220         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04221         free(t);
04222         free(q);
04223         return out;
04224 }
04225 
04226 
04227 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04228 
04229         int maxrin = numr[numr.size()-1];
04230 
04231         vector<float> r(2*maxrin);
04232 
04233         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04234 
04235         return r;
04236 }
04237 
04238 #define  dout(i,j)        dout[i+maxrin*j]
04239 #define  circ1b(i)        circ1b[i-1]
04240 #define  circ2b(i)        circ2b[i-1]
04241 
04242 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04243 
04244    // dimension         circ1(lcirc),circ2(lcirc)
04245 
04246         int   ip, jc, numr3i, numr2i, i, j;
04247         float t1, t2, t3, t4, c1, c2, d1, d2;
04248 
04249         int nring = numr.size()/3;
04250         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04251         int maxrin = numr[numr.size()-1];
04252 
04253         float* circ1b = circ1->get_data();
04254         float* circ2b = circ2->get_data();
04255 
04256 #ifdef _WIN32
04257         ip = -(int)(log((float)maxrin)/log(2.0f));
04258 #else
04259         ip = -(int)(log2(maxrin));
04260 #endif  //_WIN32
04261         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04262 
04263         //  q - straight  = circ1 * conjg(circ2)
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 
04272                 t1   = circ1b(numr2i) * circ2b(numr2i);
04273                 q(1) += t1;
04274                 t(1) += t1;
04275 
04276                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04277                 if (numr3i == maxrin)  {
04278                         q(2) += t1;
04279                         t(2) += t1;
04280                 } else {
04281                         q(numr3i+1) += t1;
04282                         t(numr3i+1) += t1;
04283                 }
04284 
04285                 for (j=3; j<=numr3i; j=j+2) {
04286                         jc     = j+numr2i-1;
04287 
04288                         c1     = circ1b(jc);
04289                         c2     = circ1b(jc+1);
04290                         d1     = circ2b(jc);
04291                         d2     = circ2b(jc+1);
04292 
04293                         t1     = c1 * d1;
04294                         t3     = c1 * d2;
04295                         t2     = c2 * d2;
04296                         t4     = c2 * d1;
04297 
04298                         q(j)   += t1 + t2;
04299                         q(j+1) += -t3 + t4;
04300                         t(j)   += t1 - t2;
04301                         t(j+1) += -t3 - t4;
04302                 }
04303         }
04304         // straight
04305         fftr_q(q,ip);
04306         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04307 
04308         // mirrored
04309         fftr_q(t,ip);
04310 }
04311 
04312 
04313 
04314 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04315 {
04316 
04317         int   ip, jc, numr3i, numr2i, i, j;
04318         float t1, t2, t3, t4, c1, c2, d1, d2;
04319 
04320         int nring = numr.size()/3;
04321         int maxrin = numr[numr.size()-1];
04322 
04323         float* circ1b = circ1->get_data();
04324         float* circ2b = circ2->get_data();
04325 
04326         double *q;
04327 
04328         q = (double*)calloc(maxrin,sizeof(double));
04329 
04330 #ifdef _WIN32
04331         ip = -(int)(log((float)maxrin)/log(2.0f));
04332 #else
04333         ip = -(int)(log2(maxrin));
04334 #endif  //_WIN32
04335 
04336          //  q - straight  = circ1 * conjg(circ2)
04337 
04338         for (i=1;i<=nring;i++) {
04339 
04340                 numr3i = numr(3,i);
04341                 numr2i = numr(2,i);
04342 
04343                 t1   = circ1b(numr2i) * circ2b(numr2i);
04344                 q(1) = q(1)+t1;
04345 
04346                 if (numr3i == maxrin)  {
04347                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04348                         q(2) = q(2)+t1;
04349                 } else {
04350                         t1              = circ1b(numr2i+1) * circ2b(numr2i+1);
04351                         q(numr3i+1) = q(numr3i+1)+t1;
04352                 }
04353 
04354                 for (j=3;j<=numr3i;j=j+2) {
04355                         jc     = j+numr2i-1;
04356 
04357                         c1     = circ1b(jc);
04358                         c2     = circ1b(jc+1);
04359                         d1     = circ2b(jc);
04360                         d2     = circ2b(jc+1);
04361 
04362                         t1     = c1 * d1;
04363                         t3     = c1 * d2;
04364                         t2     = c2 * d2;
04365                         t4     = c2 * d1;
04366 
04367                         q(j)   = q(j)   + t1 + t2;
04368                         q(j+1) = q(j+1) - t3 + t4;
04369                 }
04370         }
04371 
04372         // straight
04373         fftr_d(q,ip);
04374 
04375         EMData* out = new EMData();
04376         out->set_size(maxrin,1,1);
04377         float *dout = out->get_data();
04378         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04379         free(q);
04380         return out;
04381 
04382 }
04383 
04384 
04385 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04386 {
04387 
04388         int   ip, jc, numr3i, numr2i, i, j;
04389         float t1, t2, t3, t4, c1, c2, d1, d2;
04390 
04391         int nring = numr.size()/3;
04392         int maxrin = numr[numr.size()-1];
04393 
04394         float* circ1b = circ1->get_data();
04395         float* circ2b = circ2->get_data();
04396 
04397         double *t;
04398 
04399         t = (double*)calloc(maxrin,sizeof(double));
04400 
04401 #ifdef _WIN32
04402         ip = -(int)(log((float)maxrin)/log(2.0f));
04403 #else
04404         ip = -(int)(log2(maxrin));
04405 #endif  //_WIN32
04406 
04407          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04408 
04409         for (i=1;i<=nring;i++) {
04410 
04411                 numr3i = numr(3,i);
04412                 numr2i = numr(2,i);
04413 
04414                 t1   = circ1b(numr2i) * circ2b(numr2i);
04415                 t(1) = t(1)+t1;
04416 
04417                 if (numr3i == maxrin)  {
04418                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04419                         t(2) = t(2)+t1;
04420                 }
04421 
04422                 for (j=3;j<=numr3i;j=j+2) {
04423                         jc     = j+numr2i-1;
04424 
04425                         c1     = circ1b(jc);
04426                         c2     = circ1b(jc+1);
04427                         d1     = circ2b(jc);
04428                         d2     = circ2b(jc+1);
04429 
04430                         t1     = c1 * d1;
04431                         t3     = c1 * d2;
04432                         t2     = c2 * d2;
04433                         t4     = c2 * d1;
04434 
04435                         t(j)   = t(j)   + t1 - t2;
04436                         t(j+1) = t(j+1) - t3 - t4;
04437                 }
04438         }
04439 
04440         // mirrored
04441         fftr_d(t,ip);
04442 
04443         EMData* out = new EMData();
04444         out->set_size(maxrin,1,1);
04445         float *dout = out->get_data();
04446         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04447         free(t);
04448         return out;
04449 
04450 }
04451 
04452 #undef circ1b
04453 #undef circ2b
04454 #undef dout
04455 
04456 #undef  circ1
04457 #undef  circ2
04458 #undef  t
04459 #undef  q
04460 #undef  b
04461 #undef  t7
04462 
04463 
04464 #define    QUADPI                   3.141592653589793238462643383279502884197
04465 #define    PI2                      2*QUADPI
04466 
04467 float Util::ener(EMData* ave, vector<int> numr) {
04468         ENTERFUNC;
04469         long double ener,en;
04470 
04471         int nring = numr.size()/3;
04472         float *aveptr = ave->get_data();
04473 
04474         ener = 0.0;
04475         for (int i=1; i<=nring; i++) {
04476                 int numr3i = numr(3,i);
04477                 int np     = numr(2,i)-1;
04478                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04479                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04480                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04481                 ener += en/numr3i;
04482         }
04483         EXITFUNC;
04484         return static_cast<float>(ener);
04485 }
04486 
04487 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04488         ENTERFUNC;
04489         long double ener, en;
04490         float arg, cs, si;
04491 
04492         int nima = data.size();
04493         int nring = numr.size()/3;
04494         int maxrin = numr(3,nring);
04495 
04496         ener = 0.0;
04497         for (int i=1; i<=nring; i++) {
04498                 int numr3i = numr(3,i);
04499                 int np     = numr(2,i)-1;
04500                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04501                 float temp1 = 0.0, temp2 = 0.0;
04502                 for (int kk=0; kk<nima; kk++) {
04503                         float *ptr = data[kk]->get_data();
04504                         temp1 += ptr[np];
04505                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04506                 }
04507                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04508                 for (int j=2; j<numr3i; j+=2) {
04509                         float tempr = 0.0, tempi = 0.0;
04510                         for (int kk=0; kk<nima; kk++) {
04511                                 float *ptr = data[kk]->get_data();
04512                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04513                                 cs = cos(arg);
04514                                 si = sin(arg);
04515                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04516                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04517                         }
04518                         en += tq*(tempr*tempr+tempi*tempi);
04519                 }
04520                 ener += en/numr3i;
04521         }
04522         EXITFUNC;
04523         return static_cast<float>(ener);
04524 }
04525 
04526 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04527         int nring = numr.size()/3;
04528         float *ave = avep->get_data();
04529         float *dat = datp->get_data();
04530         int i, j, numr3i, np;
04531         float  arg, cs, si;
04532         int maxrin = numr(3,nring);
04533         if(mirror == 1) { //for mirrored data has to be conjugated
04534                 for (i=1; i<=nring; i++) {
04535                         numr3i = numr(3,i);
04536                         np     = numr(2,i)-1;
04537                         ave[np]   += dat[np];
04538                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04539                         for (j=2; j<numr3i; j=j+2) {
04540                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04541                                 cs = cos(arg);
04542                                 si = sin(arg);
04543                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04544                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04545                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04546                         }
04547                 }
04548         } else {
04549                 for (i=1; i<=nring; i++) {
04550                         numr3i = numr(3,i);
04551                         np     = numr(2,i)-1;
04552                         ave[np]   += dat[np];
04553                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04554                         for (j=2; j<numr3i; j=j+2) {
04555                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04556                                 cs = cos(arg);
04557                                 si = sin(arg);
04558                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04559                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04560                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04561                         }
04562                 }
04563         }
04564         avep->update();
04565         EXITFUNC;
04566 }
04567 
04568 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04569         int nring = numr.size()/3;
04570         float *ave = avep->get_data();
04571         float *dat = datp->get_data();
04572         int i, j, numr3i, np;
04573         float  arg, cs, si;
04574         int maxrin = numr(3,nring);
04575         if(mirror == 1) { //for mirrored data has to be conjugated
04576                 for (i=1; i<=nring; i++) {
04577                         numr3i = numr(3,i);
04578                         np     = numr(2,i)-1;
04579                         ave[np]   -= dat[np];
04580                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04581                         for (j=2; j<numr3i; j=j+2) {
04582                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04583                                 cs = cos(arg);
04584                                 si = sin(arg);
04585                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04586                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04587                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04588                         }
04589                 }
04590         } else {
04591                 for (i=1; i<=nring; i++) {
04592                         numr3i = numr(3,i);
04593                         np     = numr(2,i)-1;
04594                         ave[np]   -= dat[np];
04595                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04596                         for (j=2; j<numr3i; j=j+2) {
04597                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04598                                 cs = cos(arg);
04599                                 si = sin(arg);
04600                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04601                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04602                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04603                         }
04604                 }
04605         }
04606         avep->update();
04607         EXITFUNC;
04608 }
04609 
04610 
04611 #undef    QUADPI
04612 #undef    PI2
04613 
04614 #undef  numr
04615 #undef  circ
04616 
04617 
04618 #define QUADPI   3.141592653589793238462643383279502884197
04619 #define PI2      QUADPI*2
04620 #define deg_rad  QUADPI/180.0
04621 #define rad_deg  180.0/QUADPI
04622 
04623 struct ori_t
04624 {
04625     int iphi;
04626     int itht;
04627     int id;
04628 };
04629 
04630 
04631 struct cmpang
04632 {
04633     bool operator()( const ori_t& a, const ori_t& b )
04634     {
04635         if( a.itht != b.itht )
04636         {
04637             return a.itht < b.itht;
04638         }
04639 
04640         return a.iphi < b.iphi;
04641     }
04642 };
04643 
04644 
04645 vector<double> Util::cml_weights(const vector<float>& cml){
04646         static const int NBIN = 100;
04647         int nline=cml.size()/2;
04648         vector<double> weights(nline);
04649 
04650         vector<ori_t> angs(nline);
04651         for( int i=0; i < nline; ++i ) {
04652                 angs[i].iphi = int( NBIN*cml[2*i] );
04653                 angs[i].itht = int( NBIN*cml[2*i+1] );
04654                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04655                 angs[i].id = i;
04656         }
04657 
04658         //std::cout << "# of angs: " << angs.size() << std::endl;
04659 
04660         std::sort( angs.begin(), angs.end(), cmpang() );
04661 
04662         vector<float> newphi;
04663         vector<float> newtht;
04664         vector< vector<int> > indices;
04665 
04666         int curt_iphi = -1;
04667         int curt_itht = -1;
04668         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04669                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04670                         Assert( indices.size() > 0 );
04671                         indices.back().push_back(angs[i].id);
04672                 } else {
04673                         curt_iphi = angs[i].iphi;
04674                         curt_itht = angs[i].itht;
04675 
04676                         newphi.push_back( float(curt_iphi)/NBIN );
04677                         newtht.push_back( float(curt_itht)/NBIN );
04678                         indices.push_back( vector<int>(1,angs[i].id) );
04679                 }
04680         }
04681 
04682         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04683 
04684 
04685         int num_agl = newphi.size();
04686 
04687         if(num_agl>2) {
04688                 vector<double> w=Util::vrdg(newphi, newtht);
04689 
04690                 Assert( w.size()==newphi.size() );
04691                 Assert( indices.size()==newphi.size() );
04692 
04693                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04694                     /*
04695                     std::cout << "phi,tht,w,n: ";
04696                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04697                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04698                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04699                     std::cout << indices[i].size() << "(";
04700                     */
04701 
04702                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04703                             int id = indices[i][j];
04704                             weights[id] = w[i]/indices[i].size();
04705                             //std::cout << id << " ";
04706                     }
04707 
04708                     //std::cout << ")" << std::endl;
04709 
04710                 }
04711         } else {
04712                 cout<<"warning in Util.cml_weights"<<endl;
04713                 double val = PI2/float(nline);
04714                 for(int i=0; i<nline; i++)  weights[i]=val;
04715         }
04716 
04717         return weights;
04718 
04719 }
04720 
04721 /****************************************************
04722  * New code for common-lines
04723  ****************************************************/
04724 
04725 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04726 {
04727         int i;
04728         int nx=img->get_xsize();
04729         float *img_ptr  = img->get_data();
04730         float *line_ptr = line->get_data();
04731         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04732         img->update();
04733 }
04734 
04735 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04736         int j;
04737         int nx = sino->get_xsize();
04738         int i = nx * pos_line;
04739         float r1, r2;
04740         float *line_ptr = line->get_data();
04741         float *sino_ptr = sino->get_data();
04742         for (j=ilf;j<=ihf; j += 2) {
04743                 r1 = line_ptr[j];
04744                 r2 = line_ptr[j + 1];
04745                 sino_ptr[i + j - ilf] = r1;
04746                 sino_ptr[i + j - ilf + 1] = r2;
04747                 sino_ptr[i + nx * nblines + j - ilf] = r1;
04748                 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04749         }
04750         sino->update();
04751 }
04752 
04753 vector<double> Util::cml_init_rot(vector<float> Ori){
04754         int nb_ori = Ori.size() / 4;
04755         int i, ind;
04756         float ph, th, ps;
04757         double cph, cth, cps, sph, sth, sps;
04758         vector<double> Rot(nb_ori*9);
04759         for (i=0; i<nb_ori; ++i){
04760                 ind = i*4;
04761                 // spider convention phi=psi-90, psi=phi+90
04762                 ph = Ori[ind+2]-90;
04763                 th = Ori[ind+1];
04764                 ps = Ori[ind]+90;
04765                 ph *= deg_rad;
04766                 th *= deg_rad;
04767                 ps *= deg_rad;
04768                 // pre-calculate some trigo stuffs
04769                 cph = cos(ph);
04770                 cth = cos(th);
04771                 cps = cos(ps);
04772                 sph = sin(ph);
04773                 sth = sin(th);
04774                 sps = sin(ps);
04775                 // fill rotation matrix
04776                 ind = i*9;
04777                 Rot[ind] = cph*cps-cth*sps*sph;
04778                 Rot[ind+1] = cph*sps+cth*cps*sph;
04779                 Rot[ind+2] = sth*sph;
04780                 Rot[ind+3] = -sph*cps-cth*sps*cph;
04781                 Rot[ind+4] = -sph*sps+cth*cps*cph;
04782                 Rot[ind+5] = sth*cph;
04783                 Rot[ind+6] = sth*sps;
04784                 Rot[ind+7] = -sth*cps;
04785                 Rot[ind+8] = cth;
04786         }
04787 
04788         return Rot;
04789 }
04790 
04791 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04792         float ph, ps;
04793         double cph, cth, cps, sph, sth, sps;
04794         int ind = iprj*9;
04795         // spider convention phi=psi-90, psi=phi+90
04796         ph = nps-90;
04797         ps = nph+90;
04798         ph *= deg_rad;
04799         th *= deg_rad;
04800         ps *= deg_rad;
04801         // pre-calculate some trigo stuffs
04802         cph = cos(ph);
04803         cth = cos(th);
04804         cps = cos(ps);
04805         sph = sin(ph);
04806         sth = sin(th);
04807         sps = sin(ps);
04808         // fill rotation matrix
04809         Rot[ind] = (float)(cph*cps-cth*sps*sph);
04810         Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04811         Rot[ind+2] = (float)(sth*sph);
04812         Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04813         Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04814         Rot[ind+5] = (float)(sth*cph);
04815         Rot[ind+6] = (float)(sth*sps);
04816         Rot[ind+7] = (float)(-sth*cps);
04817         Rot[ind+8] = (float)(cth);
04818 
04819         return Rot;
04820 }
04821 
04822 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04823         vector<int> com(2*(n_prj - 1));
04824         int a = i_prj*9;
04825         int i, b, c;
04826         int n1=0, n2=0;
04827         float vmax = 1 - 1.0e-6f;
04828         double r11, r12, r13, r23, r31, r32, r33;
04829 
04830         c = 0;
04831         for (i=0; i<n_prj; ++i){
04832                 if (i!=i_prj){
04833                         b = i*9;
04834                         // this is equivalent to R = A*B'
04835                         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04836                         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04837                         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04838                         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04839                         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04840                         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04841                         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04842                         if (r33 > vmax) {
04843                             n2 = 270;
04844                             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04845                         }
04846                         else if (r33 < -vmax) {
04847                             n2 = 270;
04848                             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04849                         } else {
04850                             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04851                             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04852                             if (n1 < 0) {n1 += 360;}
04853                             if (n2 <= 0) {n2 = abs(n2);}
04854                             else {n2 = 360 - n2;}
04855                         }
04856 
04857                         if (n1 >= 360){n1 = n1 % 360;}
04858                         if (n2 >= 360){n2 = n2 % 360;}
04859 
04860                         // store common-lines
04861                         b = c*2;
04862                         com[b] = n1;
04863                         com[b+1] = n2;
04864                         ++c;
04865                 }
04866         }
04867 
04868     return com;
04869 
04870 }
04871 
04872 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04873         vector<int> com(2*n_lines);
04874         int a=0, b, c, l;
04875         int n1=0, n2=0, mem=-1;
04876         float vmax = 1 - 1.0e-6f;
04877         double r11, r12, r13, r23, r31, r32, r33;
04878         c = 0;
04879         for (l=0; l<n_lines; ++l){
04880                 c = 2*l;
04881                 if (seq[c]!=mem){
04882                     mem = seq[c];
04883                     a = seq[c]*9;
04884                 }
04885                 b = seq[c+1]*9;
04886 
04887                 // this is equivalent to R = A*B'
04888                 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04889                 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04890                 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04891                 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04892                 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04893                 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04894                 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04895                 if (r33 > vmax) {
04896                     n2 = 270;
04897                     n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04898                 }
04899                 else if (r33 < -vmax) {
04900                     n2 = 270;
04901                     n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04902                 } else {
04903                     n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04904                     n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04905                     if (n1 < 0) {n1 += 360;}
04906                     if (n2 <= 0) {n2 = abs(n2);}
04907                     else {n2 = 360 - n2;}
04908                 }
04909                 if (n1 >= 360){n1 = n1 % 360;}
04910                 if (n2 >= 360){n2 = n2 % 360;}
04911 
04912                 // store common-lines
04913                 com[c] = n1;
04914                 com[c+1] = n2;
04915         }
04916 
04917         return com;
04918 
04919 }
04920 
04921 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04922         // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04923         vector<double> cml(2*nlines); // [phi, theta] / line
04924         float ph1, th1;
04925         float ph2, th2;
04926         double nx, ny, nz;
04927         double norm;
04928         double sth1=0, sph1=0, cth1=0, cph1=0;
04929         double sth2, sph2, cth2, cph2;
04930         int l, ind, c;
04931         int mem = -1;
04932         for (l=0; l<nlines; ++l){
04933                 c = 2*l;
04934                 if (seq[c]!=mem){
04935                         mem = seq[c];
04936                         ind = 4*seq[c];
04937                         ph1 = Ori[ind]*deg_rad;
04938                         th1 = Ori[ind+1]*deg_rad;
04939                         sth1 = sin(th1);
04940                         sph1 = sin(ph1);
04941                         cth1 = cos(th1);
04942                         cph1 = cos(ph1);
04943                 }
04944                 ind = 4*seq[c+1];
04945                 ph2 = Ori[ind]*deg_rad;
04946                 th2 = Ori[ind+1]*deg_rad;
04947                 sth2 = sin(th2);
04948                 cth2 = cos(th2);
04949                 sph2 = sin(ph2);
04950                 cph2 = cos(ph2);
04951                 // cross product
04952                 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04953                 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04954                 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04955                 norm = sqrt(nx*nx+ny*ny+nz*nz);
04956                 nx /= norm;
04957                 ny /= norm;
04958                 nz /= norm;
04959                 // apply mirror if need
04960                 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04961                 // compute theta and phi
04962                 cml[c+1] = acos(nz);
04963                 if (cml[c+1] == 0) {cml[c] = 0;}
04964                 else {
04965                         cml[c+1] *= rad_deg;
04966                         if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04967                         cml[c] = rad_deg * atan2(nx, ny);
04968                         cml[c] = fmod(360 + cml[c], 360);
04969 
04970                 }
04971         }
04972 
04973         return cml;
04974 }
04975 
04976 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04977         double res = 0;
04978         double buf = 0;
04979         float* line_1;
04980         float* line_2;
04981         int i, n, ind;
04982         int lnlen = data[0]->get_xsize();
04983         for (n=0; n<n_lines; ++n) {
04984                 ind = n*2;
04985                 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04986                 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04987                 buf = 0;
04988                 for (i=0; i<lnlen; ++i) {
04989                     buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04990                 }
04991                 res += buf * weights[n];
04992         }
04993 
04994         return res;
04995 
04996 }
04997 
04998 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04999                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05000         // res: [best_disc, best_ipsi]
05001         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
05002         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
05003         vector<double> res(2);
05004         int lnlen = data[0]->get_xsize();
05005         int end = 2*(n_prj-1);
05006         double disc, buf, bdisc, tmp;
05007         int n, i, ipsi, ind, bipsi, c;
05008         float* line_1;
05009         float* line_2;
05010         bdisc = 1.0e6;
05011         bipsi = -1;
05012         // loop psi
05013         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05014                 // discrepancy
05015                 disc = 0;
05016                 c = 0;
05017                 for (n=0; n<n_prj; ++n) {
05018                         if(n!=iprj) {
05019                                 ind = 2*c;
05020                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05021                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05022                                 buf = 0;
05023                                 for (i=0; i<lnlen; ++i) {
05024                                         tmp = line_1[i]-line_2[i];
05025                                         buf += tmp*tmp;
05026                                 }
05027                                 disc += buf * weights[iw[c]];
05028                                 ++c;
05029                         }
05030                 }
05031                 // select the best value
05032                 if (disc <= bdisc) {
05033                         bdisc = disc;
05034                         bipsi = ipsi;
05035                 }
05036                 // update common-lines
05037                 for (i=0; i<end; i+=2){
05038                         com[i] += d_psi;
05039                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05040                 }
05041         }
05042         res[0] = bdisc;
05043         res[1] = float(bipsi);
05044 
05045         return res;
05046 }
05047 
05048 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
05049                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05050         // res: [best_disc, best_ipsi]
05051         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
05052         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
05053         vector<double> res(2);
05054         int lnlen = data[0]->get_xsize();
05055         int end = 2*(n_prj-1);
05056         double disc, buf, bdisc, tmp;
05057         int n, i, ipsi, ind, bipsi, c;
05058         float* line_1;
05059         float* line_2;
05060         bdisc = 1.0e6;
05061         bipsi = -1;
05062         // loop psi
05063         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05064                 // discrepancy
05065                 disc = 0;
05066                 c = 0;
05067                 for (n=0; n<n_prj; ++n) {
05068                         if(n!=iprj) {
05069                                 ind = 2*c;
05070                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05071                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05072                                 buf = 0;
05073                                 for (i=0; i<lnlen; ++i) {
05074                                         tmp = line_1[i]-line_2[i];
05075                                         buf += tmp*tmp;
05076                                 }
05077                                 disc += buf;
05078                                 ++c;
05079                         }
05080                 }
05081                 // select the best value
05082                 if (disc <= bdisc) {
05083                         bdisc = disc;
05084                         bipsi = ipsi;
05085                 }
05086                 // update common-lines
05087                 for (i=0; i<end; i+=2){
05088                         com[i] += d_psi;
05089                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05090                 }
05091         }
05092         res[0] = bdisc;
05093         res[1] = float(bipsi);
05094 
05095         return res;
05096 }
05097 
05098 #undef  QUADPI
05099 #undef  PI2
05100 #undef  deg_rad
05101 #undef  rad_deg
05102 
05103 /****************************************************
05104  * END OF NEW CODE FOR COMMON-LINES
05105  ****************************************************/
05106 
05107 // helper function for k-means
05108 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05109         ENTERFUNC;
05110 
05111         int nima = data.size();
05112         vector<float> res(nima);
05113         double result = 0.;
05114         double valmin = 1.0e20;
05115         int valpos = -1;
05116 
05117         for (int kk=0; kk<nima; kk++){
05118         result = 0;
05119 
05120         float *y_data = data[kk]->get_data();
05121         float *x_data = image->get_data();
05122         long totsize = image->get_xsize()*image->get_ysize();
05123         for (long i = 0; i < totsize; i++) {
05124             double temp = x_data[i]- y_data[i];
05125             result += temp*temp;
05126         }
05127         result /= totsize;
05128         res[kk] = (float)result;
05129 
05130         if(result<valmin) {valmin = result; valpos = kk;}
05131 
05132         }
05133 
05134         Dict retvals;
05135         retvals["dist"] = res;
05136         retvals["pos"]  = valpos;
05137 
05138         EXITFUNC;
05139         return retvals;
05140 
05141 }
05142 
05143 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05144         ENTERFUNC;
05145 
05146         int nima = data.size();
05147         vector<float> res(nima);
05148         double result = 0.;
05149         double valmin = 1.0e20;
05150         int valpos = -1;
05151 
05152         for (int kk=0; kk<nima; kk++){
05153         result = 0;
05154         //validate_input_args(image, data[kk]);
05155 
05156         float *y_data = data[kk]->get_data();
05157         float *x_data = image->get_data();
05158 
05159         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05160         int nx  = data[kk]->get_xsize();
05161         int ny  = data[kk]->get_ysize();
05162         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05163         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05164 
05165         int ixb = 2*((nx+1)%2);
05166         int iyb = ny%2;
05167         int iz = 0;
05168 
05169         for ( int iy = 0; iy <= ny-1; iy++) {
05170             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05171                 int ii = ix + (iy  + iz * ny)* lsd2;
05172                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05173             }
05174         }
05175         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05176             int ii = (iy  + iz * ny)* lsd2;
05177             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05178             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05179         }
05180         if(nx%2 == 0) {
05181             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05182                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05183                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05184                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05185             }
05186 
05187         }
05188         result *= 2;
05189         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05190         if(ny%2 == 0) {
05191             int ii = (ny/2  + iz * ny)* lsd2;
05192             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05193         }
05194         if(nx%2 == 0) {
05195             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05196             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05197             if(ny%2 == 0) {
05198                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05199                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05200             }
05201         }
05202 
05203         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05204         res[kk] = (float)result;
05205 
05206         if(result<valmin) {valmin = result; valpos = kk;}
05207 
05208         }
05209 
05210         Dict retvals;
05211         retvals["dist"] = res;
05212         retvals["pos"]  = valpos;
05213 
05214         EXITFUNC;
05215         return retvals;
05216 }
05217 
05218 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05219     long int d2 = group2[s2 - 1] - group2[0];
05220     long int p2 = 0;
05221     long int i1 = 0;
05222     long int i2 = 0;
05223     long int max = 0;
05224     long int cont = 0;
05225     long int i = 0;
05226     int stop1 = 0;
05227     int stop2 = 0;
05228 
05229     for (i=0; i<s1; i++) {
05230         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05231         if (p2 >= s2) {p2 = s2 - 1;}
05232         i1 = p2;
05233         i2 = p2;
05234         max = s2;
05235         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05236 
05237         stop1 = 0;
05238         stop2 = 0;
05239         while (max--) {
05240             if (group1[i] == group2[i1]) {
05241                 if (flag) {stb[cont] = group1[i];}
05242                 cont++;
05243                 break;
05244             }
05245             if (group2[i1] < group1[i]) {stop1=1;}
05246             if (group1[i] == group2[i2]) {
05247                 if (flag) {stb[cont] = group1[i];}
05248                 cont++;
05249                 break;
05250             }
05251             if (group2[i2] > group1[i]) {stop2=1;}
05252             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05253 
05254             if (stop1 & stop2) {break;}
05255             i1--;
05256             i2++;
05257             if (i1 < 0) {i1 = 0;}
05258             if (i2 >= s2) {i2 = s2 - 1;}
05259         }
05260         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05261     }
05262 
05263     return cont;
05264 }
05265 
05266 
05267 
05268 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*(size_t)nx]
05269 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05270 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05271 {
05272         /* Exception Handle */
05273         if (!img) {
05274                 throw NullPointerException("NULL input image");
05275         }
05276         /* ============================== */
05277 
05278         // Get the size of the input image
05279         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05280         /* ============================== */
05281 
05282 
05283         /* Exception Handle */
05284         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)
05285         {
05286                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05287                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05288         }
05289         /* ============================== */
05290 
05291 
05292         /*    Calculation of the start point */
05293         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05294         /* ============================*/
05295 
05296 
05297         /* Calculation of the size of the decimated image */
05298         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05299         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05300         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05301         if(r1>1){r1=1;}
05302         if(r2>1){r2=1;}
05303         if(r3>1){r3=1;}
05304         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05305         /* ===========================================*/
05306 
05307 
05308         EMData* img2 = new EMData();
05309         img2->set_size(new_nx,new_ny,new_nz);
05310         float *new_ptr = img2->get_data();
05311         float *old_ptr = img->get_data();
05312         int iptr, jptr, kptr = 0;
05313         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05314                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05315                         for (int i=new_st_x; i<nx; i+=x_step) {
05316                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05317                         iptr++;}
05318                 jptr++;}
05319         kptr++;}
05320         img2->update();
05321         return img2;
05322 }
05323 #undef old_ptr
05324 #undef new_ptr
05325 
05326 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05327 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05328 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05329 {
05330         /* Exception Handle */
05331         if (!img) throw NullPointerException("NULL input image");
05332         /* ============================== */
05333 
05334         // Get the size of the input image
05335         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05336         /* ============================== */
05337 
05338         /* Exception Handle */
05339         if(new_nx>nx || new_ny>ny || new_nz>nz)
05340                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05341         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)
05342                 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05343         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))))
05344                 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05345         /* ============================== */
05346 
05347         /*    Calculation of the start point */
05348         int  new_st_x = nx/2-new_nx/2 + x_offset,
05349              new_st_y = ny/2-new_ny/2 + y_offset,
05350              new_st_z = nz/2-new_nz/2 + z_offset;
05351         /* ============================== */
05352 
05353         /* Exception Handle */
05354         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05355                 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05356         /* ============================== */
05357 
05358         EMData* wind = img->copy_empty_head();
05359         wind->set_size(new_nx, new_ny, new_nz);
05360         float *outp=wind->get_data();
05361         float *inp=img->get_data();
05362 
05363         for (int k=0; k<new_nz; k++)
05364                 for(int j=0; j<new_ny; j++)
05365                         for(int i=0; i<new_nx; i++)
05366                                 outp(i,j,k) = inp(i,j,k);
05367         wind->update();
05368         return wind;
05369 }
05370 #undef inp
05371 #undef outp
05372 
05373 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05374 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05375 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)
05376 {
05377         /* Exception Handle */
05378         if (!img)  throw NullPointerException("NULL input image");
05379         /* ============================== */
05380 
05381         // Get the size of the input image
05382         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05383         /* ============================== */
05384 
05385         /* Exception Handle */
05386         if(new_nx<nx || new_ny<ny || new_nz<nz)
05387                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05388         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)
05389                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05390         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))))
05391                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05392         /* ============================== */
05393 
05394         EMData* pading = img->copy_head();
05395         pading->set_size(new_nx, new_ny, new_nz);
05396         float *inp  = img->get_data();
05397         float *outp = pading->get_data();
05398 
05399 
05400         /* Calculation of the average and the circumference values for background substitution
05401         =======================================================================================*/
05402         float background;
05403 
05404         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05405         else if (strcmp(params,"circumference")==0) {
05406                 float sum1=0.0f;
05407                 size_t cnt=0;
05408                 for(int i=0;i<nx;i++) {
05409                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05410                         cnt+=2;
05411                 }
05412                 if(nz-1 == 0) {
05413                         for (int j=1;j<ny-1;j++) {
05414                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05415                                 cnt+=2;
05416                         }
05417                 } else {
05418                         for (int k=1;k<nz-1;k++) {
05419                                 for (int j=1;j<ny-1;j++) {
05420                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05421                                         cnt+=2;
05422                                 }
05423                         }
05424                 }
05425                 background = sum1/cnt;
05426         } else {
05427                 background = static_cast<float>( atof( params ) );
05428         }
05429         /*=====================================================================================*/
05430 
05431          /*Initial Padding */
05432         int new_st_x=0,new_st_y=0,new_st_z=0;
05433         for (int k=0;k<new_nz;k++)
05434                 for(int j=0;j<new_ny;j++)
05435                         for (int i=0;i<new_nx;i++)
05436                                 outp(i,j,k)=background;
05437         /*============================== */
05438 
05439         /*    Calculation of the start point */
05440         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05441         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05442         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05443         /* ============================== */
05444 
05445         for (int k=0;k<nz;k++)
05446                 for(int j=0;j<ny;j++)
05447                         for(int i=0;i<nx;i++)
05448                                 outp(i,j,k)=inp(i,j,k);
05449         pading->update();
05450         return pading;
05451 }
05452 #undef inp
05453 #undef outp
05454 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05455 
05456 void Util::colreverse(float* beg, float* end, int nx) {
05457         float* tmp = new float[nx];
05458         int n = (end - beg)/nx;
05459         int nhalf = n/2;
05460         for (int i = 0; i < nhalf; i++) {
05461                 // swap col i and col n-1-i
05462                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05463                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05464                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05465         }
05466         delete[] tmp;
05467 }
05468 
05469 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05470 {
05471         int nxy = nx*ny;
05472         colreverse(beg, end, nxy);
05473 }
05474 
05475 
05476 void Util::cyclicshift(EMData *image, Dict params) {
05477 
05478         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05479 
05480         int dx = params["dx"];
05481         int dy = params["dy"];
05482         int dz = params["dz"];
05483 
05484         // The reverse trick we're using shifts to the left (a negative shift)
05485         int nx = image->get_xsize();
05486         dx %= nx;
05487         if (dx < 0) dx += nx;
05488         int ny = image->get_ysize();
05489         dy %= ny;
05490         if (dy < 0) dy += ny;
05491         int nz = image->get_zsize();
05492         dz %= nz;
05493         if (dz < 0) dz += nz;
05494 
05495         int mx = -(dx - nx);
05496         int my = -(dy - ny);
05497         int mz = -(dz - nz);
05498 
05499         float* data = image->get_data();
05500         // x-reverses
05501         if (mx != 0) {
05502                 for (int iz = 0; iz < nz; iz++)
05503                        for (int iy = 0; iy < ny; iy++) {
05504                                 // reverses for column iy
05505                                 size_t offset = nx*iy + (size_t)nx*ny*iz; // starting location for column iy in slice iz
05506                                 reverse(&data[offset],&data[offset+mx]);
05507                                 reverse(&data[offset+mx],&data[offset+nx]);
05508                                 reverse(&data[offset],&data[offset+nx]);
05509                         }
05510         }
05511         // y-reverses
05512         if (my != 0) {
05513                 for (int iz = 0; iz < nz; iz++) {
05514                         size_t offset = (size_t)nx*ny*iz;
05515                         colreverse(&data[offset], &data[offset + my*nx], nx);
05516                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05517                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05518                 }
05519         }
05520         if (mz != 0) {
05521                 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05522                 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05523                 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05524         }
05525         image->update();
05526 }
05527 
05528 //-----------------------------------------------------------------------------------------------------------------------
05529 
05530 
05531 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05532 {
05533         if (image->is_complex())
05534                 throw ImageFormatException("Cannot do histogram on Fourier image");
05535         //float hmax, hmin;
05536         float *imageptr=0, *maskptr=0;
05537         int nx=image->get_xsize();
05538         int ny=image->get_ysize();
05539         int nz=image->get_zsize();
05540 
05541         if(mask != NULL){
05542                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05543                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05544                 maskptr =mask->get_data();
05545         }
05546         if( nbins == 0) nbins = nx;
05547         vector <float> freq(2*nbins, 0.0);
05548 
05549         imageptr=image->get_data();
05550         if( hmin == hmax ) {
05551                 if(mask == NULL) {
05552                         hmax = image->get_attr("maximum");
05553                         hmin = image->get_attr("minimum");
05554                 } else {
05555                         bool  First = true;
05556                         for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05557                         if (maskptr[i]>=0.5f) {
05558                                         if(First) {
05559                                                 hmax = imageptr[i];
05560                                                 hmin = imageptr[i];
05561                                                 First = false;
05562                                         } else {
05563                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05564                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05565                                         }
05566                                 }
05567                         }
05568                 }
05569         }
05570         float hdiff = hmax - hmin;
05571         float ff = (nbins-1)/hdiff;
05572         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05573         if(mask == NULL) {
05574                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05575                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05576                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05577                 }
05578         } else {
05579                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05580                         if(maskptr[i] >= 0.5) {
05581                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05582                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05583                         }
05584                 }
05585         }
05586         return freq;
05587 }
05588 
05589 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05590 {
05591         /* Exception Handle */
05592         if (img->is_complex() || ref->is_complex())
05593                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05594 
05595         if(mask != NULL){
05596                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05597                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05598         /* ===================================================== */
05599 
05600         /* Image size calculation */
05601         size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05602         size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05603         /* ===================================================== */
05604 
05605         /* The reference image attributes */
05606         float *ref_ptr = ref->get_data();
05607         float ref_h_min = ref->get_attr("minimum");
05608         float ref_h_max = ref->get_attr("maximum");
05609         float ref_h_avg = ref->get_attr("mean");
05610         float ref_h_sig = ref->get_attr("sigma");
05611         /* ===================================================== */
05612 
05613         /* Input image under mask attributes */
05614         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05615 
05616         vector<float> img_data = Util::infomask(img, mask);
05617         float img_avg = img_data[0];
05618         float img_sig = img_data[1];
05619 
05620         /* The image under mask -- size calculation */
05621         int cnt=0;
05622         for(size_t i=0;i<size_img;++i)
05623                 if (mask_ptr[i]>0.5f)
05624                                 cnt++;
05625         /* ===================================================== */
05626 
05627         /* Histogram of reference image calculation */
05628         float ref_h_diff = ref_h_max - ref_h_min;
05629 
05630         #ifdef _WIN32
05631                 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05632         #else
05633                 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05634         #endif  //_WIN32
05635 
05636         float *ref_freq_bin = new float[3*hist_len];
05637 
05638         //initialize value in each bin to zero
05639         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05640 
05641         for (size_t i = 0;i < size_ref;++i) {
05642                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05643                 ref_freq_bin[L]++;
05644         }
05645         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05646 
05647         //Parameters Calculation (i.e) 'A' x + 'B'
05648         float A = ref_h_sig/img_sig;
05649         float B = ref_h_avg - (A*img_avg);
05650 
05651         vector<float> args;
05652         args.push_back(A);
05653         args.push_back(B);
05654 
05655         vector<float> scale;
05656         scale.push_back(1.e-7f*A);
05657         scale.push_back(-1.e-7f*B);
05658 
05659         vector<float> ref_freq_hist;
05660         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05661 
05662         vector<float> data;
05663         data.push_back(ref_h_diff);
05664         data.push_back(ref_h_min);
05665 
05666         Dict parameter;
05667 
05668         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05669         parameter["args"] = args;
05670         parameter["scale"]= scale;
05671         parameter["data"] = data;
05672         parameter["ref_freq_bin"] = ref_freq_hist;
05673         parameter["size_img"]=(double)size_img;
05674         parameter["hist_len"]=hist_len;
05675         /* ===================================================== */
05676 
05677         return parameter;
05678 }
05679 
05680 
05681 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)
05682 {
05683         float *img_ptr = img->get_data();
05684         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05685 
05686         int *img_freq_bin = new int[3*hist_len];
05687         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05688         for(size_t i = 0;i < size_img;++i) {
05689                 if(mask_ptr[i] > 0.5f) {
05690                         float img_xn = img_ptr[i]*PA + PB;
05691                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05692                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05693                 }
05694         }
05695         int freq_hist = 0;
05696 
05697         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);
05698         freq_hist = (-freq_hist);
05699         return static_cast<float>(freq_hist);
05700 }
05701 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05702 #define    QUADPI                       3.141592653589793238462643383279502884197
05703 #define    DGR_TO_RAD                   QUADPI/180
05704 #define    DM(I)                        DM          [I-1]
05705 #define    SS(I)                        SS          [I-1]
05706 Dict Util::CANG(float PHI,float THETA,float PSI)
05707 {
05708         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05709         vector<float>   DM,SS;
05710 
05711         for(int i =0;i<9;i++) DM.push_back(0);
05712 
05713         for(int i =0;i<6;i++) SS.push_back(0);
05714 
05715         CPHI = cos(double(PHI)*DGR_TO_RAD);
05716         SPHI = sin(double(PHI)*DGR_TO_RAD);
05717         CTHE = cos(double(THETA)*DGR_TO_RAD);
05718         STHE = sin(double(THETA)*DGR_TO_RAD);
05719         CPSI = cos(double(PSI)*DGR_TO_RAD);
05720         SPSI = sin(double(PSI)*DGR_TO_RAD);
05721 
05722         SS(1) = float(CPHI);
05723         SS(2) = float(SPHI);
05724         SS(3) = float(CTHE);
05725         SS(4) = float(STHE);
05726         SS(5) = float(CPSI);
05727         SS(6) = float(SPSI);
05728 
05729         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05730         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05731         DM(3) = float(-STHE*CPSI);
05732         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05733         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05734         DM(6) = float(STHE*SPSI);
05735         DM(7) = float(STHE*CPHI);
05736         DM(8) = float(STHE*SPHI);
05737         DM(9) = float(CTHE);
05738 
05739         Dict DMnSS;
05740         DMnSS["DM"] = DM;
05741         DMnSS["SS"] = SS;
05742 
05743         return(DMnSS);
05744 }
05745 #undef SS
05746 #undef DM
05747 #undef QUADPI
05748 #undef DGR_TO_RAD
05749 //-----------------------------------------------------------------------------------------------------------------------
05750 struct t_BPCQ_line{
05751         int rX;     // radius along X axe
05752         int offset; // offset of voxel in volume - beginning of the line
05753         float xbb;  // XBB coefficient
05754         float ybb;  // YBB coefficient
05755 };
05756 
05757 void Util::BPCQ( EMData *B, EMData *CUBE, const int radius )
05758 {
05759         if (B->is_complex()) {
05760                 B->do_ift_inplace();
05761                 B->depad();
05762         }
05763 
05764         const Transform * transform = B->get_attr("xform.projection");
05765         Dict transform_params = transform->get_params("spider");
05766 
05767         // ---- build DM matrix (transform matrix) - convert from 3x4 matrix to 2x3 matrix (only 2 first rows are nedeed)
05768         std::vector<float> DM = transform->get_matrix();
05769         DM[3+0] = DM[4+0];
05770         DM[3+1] = DM[4+1];
05771         DM[3+2] = DM[4+2];
05772 
05773         delete transform;
05774 
05775         const int NSAM = B->get_xsize();
05776         const int NROW = B->get_ysize();
05777 
05778         // buffer "lines_to_process" should be aligned to size of cache line (usually 64 or 128 bytes)
05779         t_BPCQ_line * lines_to_process;
05780 #ifdef _WIN32
05781         if ( (lines_to_process = (t_BPCQ_line *)_aligned_malloc( 4*radius*radius*sizeof(t_BPCQ_line), 256 )) == NULL )
05782 #else
05783         if ( posix_memalign( reinterpret_cast<void**>(&lines_to_process), 256, 4*radius*radius*sizeof(t_BPCQ_line) ) != 0 )
05784 #endif  //_WIN32
05785         {
05786                 throw std::bad_alloc();
05787         }
05788         t_BPCQ_line * first_free_line = lines_to_process;
05789 
05790         // calculate lines parameters
05791         {
05792                 //  Unsure about sign of shifts, check later PAP 06/28/09
05793                 const float x_shift_plus_center = float(NSAM/2 +1) + float(transform_params[ "tx" ]);
05794                 const float y_shift_plus_center = float(NROW/2 +1) + float(transform_params[ "ty" ]);
05795 
05796                 const int sizeX = CUBE->get_xsize();
05797                 const int sizeY = CUBE->get_ysize();
05798 
05799                 const int centerX = sizeX / 2;
05800                 const int centerY = sizeY / 2;
05801                 const int centerZ = CUBE->get_zsize() /2;
05802 
05803                 for ( int rZ=-radius; rZ<=radius; ++rZ ) {
05804                         for ( int rY=-radius; rY<=radius; ++rY ) {
05805                                 const int sqRX = radius*radius - rZ*rZ - rY*rY;
05806                                 if (sqRX >= 0) {
05807 #ifdef  _WIN32
05808                                         first_free_line->rX     = static_cast<int>( floor(sqrtf(sqRX)+0.5) );
05809 #else
05810                                         first_free_line->rX     = static_cast<int>( roundf(sqrtf(sqRX)) );
05811 #endif  //_WIN32
05812                                         first_free_line->offset = sizeX*( centerY+rY + sizeY*(centerZ+rZ) ) + centerX - first_free_line->rX;
05813                                         first_free_line->xbb    = rZ*DM[2] + rY*DM[1] + x_shift_plus_center;
05814                                         first_free_line->ybb    = rZ*DM[5] + rY*DM[4] + y_shift_plus_center;
05815                                         ++first_free_line;
05816                                 }
05817                         }
05818                 }
05819         }
05820 
05821         const float * const Bptr = B->get_data();
05822         float * const CUBE_begin = CUBE->get_data();
05823 
05824         // update voxels in volume
05825         // this loop takes more than 95% of calculations time spent in Util::BPCQ function
05826         for ( t_BPCQ_line * iLine = lines_to_process; iLine < first_free_line; ++iLine ) {
05827                 const int rX_first = -(iLine->rX);
05828                 const int rX_last  =   iLine->rX;
05829                 float  *CUBE_ptr = CUBE_begin + iLine->offset;
05830                 for (int rX=rX_first; rX<=rX_last; ++rX, ++CUBE_ptr) {
05831                         const float XB  = rX * DM[0] + iLine->xbb;
05832                         const float YB  = rX * DM[3] + iLine->ybb;
05833                         const int IQX = int(XB);
05834                         const int IQY = int(YB);
05835                         if ( IQX < 1 || IQX >= NSAM || IQY < 1 || IQY >= NROW )  continue;
05836                         const float DIPX = XB-IQX;
05837                         const float DIPY = YB-IQY;
05838                         const float b00 = Bptr[IQX-1+((IQY-1)*NSAM)];
05839                         const float b01 = Bptr[IQX-1+((IQY-0)*NSAM)];
05840                         const float b10 = Bptr[IQX-0+((IQY-1)*NSAM)];
05841                         const float b11 = Bptr[IQX-0+((IQY-0)*NSAM)];
05842                         *(CUBE_ptr) = *(CUBE_ptr) + b00 + DIPY*(b01-b00) + DIPX*(b10-b00+DIPY*(b11-b10-b01+b00));
05843                 }
05844         }
05845 
05846         free(lines_to_process);
05847 }
05848 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05849 #define    W(i,j)                       Wptr    [i+(j)*Wnx]
05850 #define    PROJ(i,j)            PROJptr [i+(j)*NNNN]
05851 #define    SS(i,j)              SS          [i+(j)*6]
05852 
05853 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05854 {
05855 
05856         --K; // now indexes are started from 0
05857 
05858         int NSAM = PROJ->get_xsize();
05859         int NROW = PROJ->get_ysize();
05860 
05861         if (PROJ->is_fftpadded()) {
05862                 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);  // correction for DFT image
05863         }
05864 
05865     const int ntotal = NSAM*NROW;
05866         const float q = 2.0f;
05867         const float qt = 8.0f/q;
05868         //  Fix for padding 2x
05869         const int ipad = 1;
05870         NSAM *= ipad;
05871         NROW *= ipad;
05872         const int NNNN = NSAM+2-(NSAM%2);
05873         const int NX2 = NSAM/2;
05874         const int NR2  = NROW/2;
05875 
05876         const int NANG = int(SS.size())/6;
05877 
05878         EMData* W = new EMData();
05879         const int Wnx = NNNN/2;
05880         W->set_size(Wnx,NROW,1);
05881         W->to_zero();
05882         float *Wptr = W->get_data();
05883 
05884         for (int L=0; L<NANG; L++) {
05885                 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);
05886                 const float tmp2 = SS(3,L)*( SS(0,K)*SS(1,L) - SS(0,L)*SS(1,K) );
05887                 float OX = SS(5,K)*tmp2 + SS(4,K)*tmp1;
05888                 float OY = SS(4,K)*tmp2 - SS(5,K)*tmp1;
05889                 if(OX < 0.0f) {
05890                         OX = -OX;
05891                         OY = -OY;
05892                 }
05893 
05894                 if( OX > 1.0e-6f || fabs(OY) > 1.0e-6f ) {
05895                         for (int J=0; J<NROW; ++J) {
05896                                 const float JY_OY = (J > NR2) ? ((J-NROW)*OY) : (J*OY);
05897                                 int xma = NX2;
05898                                 int xmi = 0;
05899                                 const float fxma = ( q-JY_OY) / OX;
05900                                 const float fxmi = (-q-JY_OY) / OX;
05901                                 if (fxma < xmi || fxmi > xma ) {
05902                                         continue;
05903                                 }
05904                                 if (fxma < xma) {
05905                                         xma = static_cast<int>(fxma+0.5f);
05906                                 }
05907                                 if (fxmi > xmi) {
05908                                         xmi = static_cast<int>(fxmi+0.5f);
05909                                 }
05910                                 for( int I=xmi; I<=xma; ++I ) {
05911                                         const float Y = I*OX + JY_OY;
05912                                         W(I,J) += exp(-qt*Y*Y);
05913                                 }
05914                         }
05915                 } else {
05916                         for (int J=0; J<NROW; ++J) {
05917                                 for (int I=0; I<NNNN/2; ++I) {
05918                                         W(I,J) += 1.0f;
05919                                 }
05920                         }
05921                 }
05922         }
05923 
05924     EMData* proj_in = PROJ;
05925 
05926         const bool realOnInput = PROJ->is_real();
05927     if (realOnInput) {
05928                 // copy input image and run DFT on it
05929                 PROJ = PROJ->norm_pad( false, ipad);
05930                 PROJ->do_fft_inplace();
05931                 PROJ->update();
05932     }
05933     float * PROJptr = PROJ->get_data();
05934 
05935         const float osnr = 1.0f/SNR;
05936         const float WNRMinv = 1.0f/W(0,0);
05937         for (int J=0; J<NROW; ++J)  {
05938                 float sy = (J > NR2) ? (J - NROW) : (J);
05939                 sy /= NROW;
05940                 sy *= sy;
05941                 for (int I=0; I<NNNN; I+=2) {
05942                         const int KX = I/2;
05943                         const float temp = W(KX,J)*WNRMinv;
05944                         float WW = temp/(temp*temp + osnr);
05945                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
05946                         const float sx = float(KX) / NSAM;
05947                         WW *= exp(qt*(sy + sx*sx));
05948                         PROJ(I,J)   *= WW;
05949                         PROJ(I+1,J) *= WW;
05950                 }
05951         }
05952         delete W; W = 0;
05953 
05954         PROJ->do_ift_inplace();
05955         PROJ->depad();
05956 
05957         if (realOnInput) {
05958                 // copy data back to input image
05959                 float* data_src = PROJ->get_data();
05960                 float* data_dst = proj_in->get_data();
05961                 memcpy( data_dst, data_src, ntotal * sizeof(float) );
05962                 delete PROJ;
05963         }
05964 
05965         proj_in->update();
05966 }
05967 /*
05968 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05969 {
05970         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05971         float WW,OX,OY,Y;
05972 
05973         NSAM = PROJ->get_xsize();
05974         NROW = PROJ->get_ysize();
05975         //  Fix for padding 2x
05976         int ntotal = NSAM*NROW;
05977         int ipad = 1;
05978         NSAM *= ipad;
05979         NROW *= ipad;
05980         NNNN = NSAM+2-(NSAM%2);
05981         NR2  = NROW/2;
05982 
05983         NANG = int(SS.size())/6;
05984 
05985         EMData* W = new EMData();
05986         int Wnx = NNNN/2;
05987         W->set_size(Wnx,NROW,1);
05988         W->to_zero();
05989         float *Wptr = W->get_data();
05990         float *PROJptr = PROJ->get_data();
05991         for (L=1; L<=NANG; L++) {
05992                 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);
05993                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05994                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05995                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05996         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
05997 
05998                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
05999                         for(int J=1;J<=NROW;J++) {
06000                                 JY = (J-1);
06001                                 if(JY > NR2) JY=JY-NROW;
06002                                 for(int I=1;I<=NNNN/2;I++) {
06003                                         Y =  fabs(OX * (I-1) + OY * JY);
06004                                         if(Y < 2.0f) {
06005                                         W(I,J) += exp(-4*Y*Y);
06006         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
06007                                 }
06008                         }
06009                 } else {
06010                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
06011                 }
06012         }
06013         EMData* proj_in = PROJ;
06014 
06015         PROJ = PROJ->norm_pad( false, ipad);
06016         PROJ->do_fft_inplace();
06017         PROJ->update();
06018         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
06019         PROJptr = PROJ->get_data();
06020 
06021         float WNRMinv,temp;
06022         float osnr = 1.0f/SNR;
06023         WNRMinv = 1.0f/W(1,1);
06024         for(int J=1;J<=NROW;J++)
06025                 for(int I=1;I<=NNNN;I+=2) {
06026                         KX           = (I+1)/2;
06027                         temp         = W(KX,J)*WNRMinv;
06028                         WW           = temp/(temp*temp + osnr);
06029                         PROJ(I,J)   *= WW;
06030                         PROJ(I+1,J) *= WW;
06031                 }
06032         delete W; W = 0;
06033         PROJ->do_ift_inplace();
06034         PROJ->depad();
06035 
06036         float* data_src = PROJ->get_data();
06037         float* data_dst = proj_in->get_data();
06038 
06039         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
06040 
06041         proj_in->update();
06042 
06043         delete PROJ;
06044 }
06045 */
06046 #undef PROJ
06047 #undef W
06048 #undef SS
06049 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
06050 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
06051 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
06052 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
06053 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
06054 #define    CC(i)                        CC          [i-1]
06055 #define    CP(i)                        CP          [i-1]
06056 #define    VP(i)                        VP          [i-1]
06057 #define    VV(i)                        VV          [i-1]
06058 #define    AMAX1(i,j)                   i>j?i:j
06059 #define    AMIN1(i,j)                   i<j?i:j
06060 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
06061 {
06062         float rad2deg =(180.0f/3.1415926f);
06063         float deg2rad = (3.1415926f/180.0f);
06064 
06065         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
06066 
06067         NSAM = PROJ->get_xsize();
06068         NROW = PROJ->get_ysize();
06069 
06070         if (PROJ->is_fftpadded()) {
06071                 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);  // correction for DFT image
06072         }
06073 
06074         NNNN = NSAM+2-(NSAM%2);
06075         NR2  = NROW/2;
06076         NANG = int(SS.size())/6;
06077 
06078         float RI[9];
06079         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06080         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06081         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06082         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06083         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06084         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06085         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06086         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06087         RI(3,3)=SS(3,NUMP);
06088 
06089         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06090 
06091         EMData* W = new EMData();
06092         int Wnx = NNNN/2;
06093         W->set_size(NNNN/2,NROW,1);
06094         W->to_one();
06095         float *Wptr = W->get_data();
06096 
06097         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06098 
06099         for (L=1; L<=NANG; L++) {
06100                 if (L != NUMP) {
06101                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06102                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06103                         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);
06104 
06105                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
06106                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06107                         ALPHA=rad2deg*float(asin(CCN));
06108                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06109                         if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
06110                         if(ALPHA<1.0E-6) {
06111                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06112                         } else {
06113                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06114                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
06115                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06116                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06117                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06118                                 CP(1)   = 0.0;CP(2) = 0.0;
06119                                 VP(1)   = 0.0;VP(2) = 0.0;
06120 
06121                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06122                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06123                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06124                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06125 
06126                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06127 
06128                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
06129                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
06130                                 float tmpinv = 1.0f/TMP;
06131                                 for(int J=1;J<=NROW;J++) {
06132                                         JY = (J-1);
06133                                         if (JY>NR2)  JY=JY-NROW;
06134                                         for(int I=1;I<=NNNN/2;I++) {
06135                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06136                                                 RT     = 1.0f-FV/FM;
06137                                                 W(I,J) += ((RT>0.0f)*RT);
06138                                         }
06139                                 }
06140                         }
06141                 }
06142         }
06143 
06144     EMData* proj_in = PROJ;
06145     const bool realOnInput = PROJ->is_real();
06146 
06147         if (realOnInput) {
06148                 // copy input image and run DFT on it
06149                 PROJ = PROJ->norm_pad( false, 1 );
06150                 PROJ->do_fft_inplace();
06151                 PROJ->update();
06152         }
06153         float *PROJptr = PROJ->get_data();
06154 
06155         int KX;
06156         float WW;
06157         for(int J=1; J<=NROW; J++)
06158                 for(int I=1; I<=NNNN; I+=2) {
06159                         KX          =  (I+1)/2;
06160                         WW          =  1.0f/W(KX,J);
06161                         PROJ(I,J)   = PROJ(I,J)*WW;
06162                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
06163                 }
06164         delete W; W = 0;
06165         PROJ->do_ift_inplace();
06166         PROJ->depad();
06167 
06168         if (realOnInput) {
06169                 // copy data back to input image
06170                 float* data_src = PROJ->get_data();
06171                 float* data_dst = proj_in->get_data();
06172                 int ntotal = NSAM*NROW;
06173                 memcpy( data_dst, data_src, ntotal * sizeof(float) );
06174                 delete PROJ;
06175         }
06176 
06177         proj_in->update();
06178 }
06179 #undef   AMAX1
06180 #undef   AMIN1
06181 #undef   RI
06182 #undef   CC
06183 #undef   CP
06184 #undef   VV
06185 #undef   VP
06186 #undef   W
06187 #undef   SS
06188 #undef   PROJ
06189 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
06190 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06191 {
06192         float cst  = cs*1.0e7f;
06193 
06194         wgh /= 100.0;
06195         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06196         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06197         float ak2 = ak*ak;
06198         float g1 = dzz*1.0e4f*lambda*ak2;
06199         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06200 
06201         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06202         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06203 
06204         return ctfv;
06205 }
06206 
06207 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06208 {
06209         /***********
06210         ***get the size of the image for validation purpose
06211         **************/
06212         int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();  //Aren't  these  implied?  Please check and let me know, PAP.
06213         /********
06214         ***Exception Handle
06215         *************/
06216         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06217                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06218 
06219         size_t i, size = (size_t)nx*ny*nz;
06220 
06221         float* img_ptr = image->get_data();
06222         float* mask_ptr = mask->get_data();
06223 
06224         int ln=0;  //length of the output image = number of points under the mask.
06225         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06226 
06227         EMData* new_image = new EMData();
06228         new_image->set_size(ln,1,1); /* set size of the new image */
06229         float *new_ptr    = new_image->get_data();
06230 
06231         ln=-1;
06232         for(i = 0;i < size;i++){
06233                 if(mask_ptr[i] > 0.5f) {
06234                         ln++;
06235                         new_ptr[ln]=img_ptr[i];
06236                 }
06237         }
06238 
06239         return new_image;
06240 }
06241 
06242 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06243 {
06244         /********
06245         ***Exception Handle
06246         *************/
06247         if(mask == NULL)
06248                 throw ImageDimensionException("The mask cannot be an null image");
06249 
06250         /***********
06251         ***get the size of the mask
06252         **************/
06253         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06254 
06255         size_t i,size = (size_t)nx*ny*nz;                        /* loop counters */
06256         /* new image declaration */
06257         EMData *new_image = new EMData();
06258         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06259         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06260         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06261         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06262         int count = 0;
06263         float sum_under_mask = 0.0 ;
06264         for(i = 0;i < size;i++){
06265                         if(mask_ptr[i] > 0.5f){
06266                                 new_ptr[i] = img_ptr[count];
06267                                 sum_under_mask += img_ptr[count];
06268                                 count++;
06269                                 if( count > image->get_xsize() ) {
06270                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06271                                 }
06272                         }
06273         }
06274 
06275         if( count > image->get_xsize() ) {
06276             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06277         }
06278 
06279         float avg_under_mask = sum_under_mask / count;
06280         for(i = 0;i < size;i++) {
06281                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06282         }
06283         new_image->update();
06284         return new_image;
06285 }
06286 
06287 
06288 
06289 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06290 {
06291         vector<float>new_peak;
06292         int n1=peak1.size()/3;
06293         float p_size2=p_size*p_size;
06294         for (int i=0;i<n1;++i) {
06295                 vector<float>::iterator it2= peak1.begin()+3*i;
06296                 bool push_back1=true;
06297                 int n2=peak2.size()/3;
06298                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06299                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06300                 if(n2 ==0) {
06301                         new_peak.push_back(*it2);
06302                         new_peak.push_back(*(it2+1));
06303                         new_peak.push_back(*(it2+2));
06304                 } else  {
06305                         int j=0;
06306                         while (j< n2-1 ) {
06307                                 vector<float>::iterator it3= peak2.begin()+3*j;
06308                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06309                                 if(d2< p_size2 ) {
06310                                         if( (*it2)<(*it3) ) {
06311                                                 new_peak.push_back(*it3);
06312                                                 new_peak.push_back(*(it3+1));
06313                                                 new_peak.push_back(*(it3+2));
06314                                                 peak2.erase(it3);
06315                                                 peak2.erase(it3);
06316                                                 peak2.erase(it3);
06317                                                 push_back1=false;
06318                                         } else {
06319                                                 peak2.erase(it3);
06320                                                 peak2.erase(it3);
06321                                                 peak2.erase(it3);
06322                                         }
06323                                 } else  j=j+1;
06324                                 n2=peak2.size()/3;
06325                         }
06326                         if(push_back1) {
06327                                 new_peak.push_back(*it2);
06328                                 new_peak.push_back(*(it2+1));
06329                                 new_peak.push_back(*(it2+2));
06330                         }
06331                 }
06332         }
06333         return new_peak;
06334 }
06335 
06336 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06337 {
06338         // n size of the covariance/correlation matrix
06339         // covmat --- covariance/correlation matrix (n by n)
06340         // eigval --- returns eigenvalues
06341         // eigvec --- returns eigenvectors
06342 
06343         ENTERFUNC;
06344 
06345         int i;
06346 
06347         // make a copy of covmat so that it will not be overwritten
06348         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06349 
06350         char NEEDV = 'V';
06351         char UPLO = 'U';
06352         int lwork = -1;
06353         int info = 0;
06354         float *work, wsize;
06355 
06356         //  query to get optimal workspace
06357         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06358         lwork = (int)wsize;
06359 
06360         work = (float *)calloc(lwork, sizeof(float));
06361         //  calculate eigs
06362         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06363         free(work);
06364         EXITFUNC;
06365         return info;
06366 }
06367 
06368 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06369 {
06370 
06371         ENTERFUNC;
06372         int len = covmatpy.size();
06373         float *eigvec;
06374         float *eigval;
06375         float *covmat;
06376         int status = 0;
06377         eigval = (float*)calloc(ncov,sizeof(float));
06378         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06379         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06380 
06381         const float *covmat_ptr;
06382         covmat_ptr = &covmatpy[0];
06383         for(int i=0;i<len;i++){
06384             covmat[i] = covmat_ptr[i];
06385         }
06386 
06387         status = Util::coveig(ncov, covmat, eigval, eigvec);
06388 
06389         vector<float> eigval_py(ncov);
06390         const float *eigval_ptr;
06391         eigval_ptr = &eigval[0];
06392         for(int i=0;i<ncov;i++){
06393             eigval_py[i] = eigval_ptr[i];
06394         }
06395 
06396         vector<float> eigvec_py(ncov*ncov);
06397         const float *eigvec_ptr;
06398         eigvec_ptr = &eigvec[0];
06399         for(int i=0;i<ncov*ncov;i++){
06400             eigvec_py[i] = eigvec_ptr[i];
06401         }
06402 
06403         Dict res;
06404         res["eigval"] = eigval_py;
06405         res["eigvec"] = eigvec_py;
06406 
06407         EXITFUNC;
06408         return res;
06409 }
06410 
06411 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06412 {
06413         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06414 
06415         k=(int)pw.size();
06416         l=0;
06417         m=k;
06418         n2=n+2;
06419         n1=n+1;
06420         klmd=k+l+m;
06421         klm2d= k+l+m+2;
06422         nklmd=k+l+m+n;
06423         n2d=n+2;
06424         /*size has to be increased when N is large*/
06425         n_larg=klmd*2;
06426         klm2d=n_larg+klm2d;
06427         klmd=n_larg+klmd;
06428         nklmd=n_larg+nklmd;
06429         int size_q=klm2d*n2d;
06430         int size_cu=nklmd*2;
06431         static int i__;
06432 
06433          double *q ;
06434          double *x ;
06435          double *res;
06436          double *cu;
06437          float *q2;
06438          float *pw_;
06439          long int *iu;
06440          double *s;
06441          q = (double*)calloc(size_q,sizeof(double));
06442          x = (double*)calloc(n2d,sizeof(double));
06443          res = (double*)calloc(klmd,sizeof(double));
06444          cu =(double*)calloc(size_cu,sizeof(double));
06445          s = (double*)calloc(klmd,sizeof(double));
06446          q2 = (float*)calloc(size_q,sizeof(float));
06447          iu = (long int*)calloc(size_cu,sizeof(long int));
06448          pw_ = (float*)calloc(k,sizeof(float));
06449 
06450         for( i__ =0;i__<k;++i__)
06451                 {
06452                 pw_[i__]=log(pw[i__]); }
06453         long int l_k=k;
06454         long int l_n=n;
06455         long int l_iswi=iswi;
06456         vector<float> cl1_res;
06457         cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06458         free(q);
06459         free(x);
06460         free(res);
06461         free(s);
06462         free(cu);
06463         free(q2);
06464         free(iu);
06465         free(pw_);
06466         return cl1_res;
06467 }
06468 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)
06469 {
06470     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06471     float r__1;
06472     int tmp__i;
06473     long int i__, j;
06474     --s;
06475     --res;
06476     iu -= 3;
06477     cu -= 3;
06478     --x;
06479     long int klm2d;
06480     klm2d= *k+*k+2;
06481     klm2d=klm2d+klm2d;
06482     q_dim1 = klm2d;
06483     q_offset = 1 + q_dim1;
06484     q -= q_offset;
06485     q2_dim1 = klm2d;
06486     q2_offset = 1 + q2_dim1;
06487     q2 -= q2_offset;
06488     i__2=0;
06489     i__1 = *n - 1;
06490     tmp__i=0;
06491     for (j = 1; j <= i__1; ++j) {
06492         i__2 = *k;
06493         tmp__i+=1;
06494         for (i__ = 1; i__ <= i__2; ++i__) {
06495             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06496             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06497         }
06498     }
06499     for  (i__ = 1; i__ <= i__2; ++i__)
06500       { q2[i__ + *n * q2_dim1] = 1.f;
06501             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06502         }
06503    vector<float> fit_res;
06504    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06505    return fit_res;
06506 }
06507 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)
06508 {
06509     /* System generated locals */
06510     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06511 
06512     /* Local variables */
06513     long int i__, j, m, n1, ii, jj;
06514     double tmp;
06515     vector<float> p;
06516     --x;
06517     q_dim1 = *klm2d;
06518     q_offset = 1 + q_dim1;
06519     q -= q_offset;
06520     q1_dim1 = *klm2d;
06521     q1_offset = 1 + q1_dim1;
06522     q1 -= q1_offset;
06523     --s;
06524     --res;
06525     iu -= 3;
06526     cu -= 3;
06527 
06528     /* Function Body */
06529     long int l = 0;
06530 
06531 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06532     m = *ks;
06533     n1 = *n + 1;
06534     if (*iswi == 1) {
06535         i__1 = n1;
06536         for (jj = 1; jj <= i__1; ++jj) {
06537             i__2 = *ks;
06538             for (ii = 1; ii <= i__2; ++ii) {
06539         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06540 
06541                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06542                         ;
06543             }
06544         }
06545     } else if (*iswi == 2) {
06546         i__1 = *ks;
06547         for (ii = 1; ii <= i__1; ++ii) {
06548             i__2 = n1;
06549             for (jj = 1; jj <= i__2; ++jj) {
06550                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06551                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06552             }
06553         }
06554     } else if (*iswi == 3) {
06555         l = 2;
06556         i__1 = n1;
06557         for (jj = 1; jj <= i__1; ++jj) {
06558             i__2 = *ks + 2;
06559             for (ii = 1; ii <= i__2; ++ii) {
06560                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06561             }
06562             i__2 = *ks;
06563             for (ii = 1; ii <= i__2; ++ii) {
06564                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06565             }
06566         }
06567     } else if (*iswi == 4) {
06568         l = 2;
06569         i__1 = n1;
06570         for (jj = 1; jj <= i__1; ++jj) {
06571             i__2 = *ks + 2;
06572             for (ii = 1; ii <= i__2; ++ii) {
06573                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06574             }
06575             i__2 = *ks;
06576             for (ii = 1; ii <= i__2; ++ii) {
06577                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06578             }
06579         }
06580     } else if (*iswi == 5) {
06581         l = 1;
06582         i__1 = n1;
06583         for (jj = 1; jj <= i__1; ++jj) {
06584             i__2 = *ks + 1;
06585             for (ii = 1; ii <= i__2; ++ii) {
06586                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06587             }
06588             i__2 = *ks;
06589             for (ii = 1; ii <= i__2; ++ii) {
06590                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06591             }
06592         }
06593     } else if (*iswi == 6) {
06594         l = 1;
06595         i__1 = n1;
06596         for (jj = 1; jj <= i__1; ++jj) {
06597             i__2 = *ks + 1;
06598             for (ii = 1; ii <= i__2; ++ii) {
06599                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06600             }
06601             i__2 = *ks;
06602             for (ii = 1; ii <= i__2; ++ii) {
06603                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06604             }
06605         }
06606     } else if (*iswi == 7) {
06607         l = 3;
06608         i__1 = n1;
06609         for (jj = 1; jj <= i__1; ++jj) {
06610             i__2 = *ks + 3;
06611             for (ii = 1; ii <= i__2; ++ii) {
06612                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06613             }
06614             i__2 = *ks;
06615             for (ii = 1; ii <= i__2; ++ii) {
06616                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06617             }
06618         }
06619     } else if (*iswi == 8) {
06620         l = 4;
06621         i__1 = n1;
06622         for (jj = 1; jj <= i__1; ++jj) {
06623             i__2 = *ks + 4;
06624             for (ii = 1; ii <= i__2; ++ii) {
06625                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06626             }
06627             i__2 = *ks;
06628             for (ii = 1; ii <= i__2; ++ii) {
06629                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06630             }
06631         }
06632     }
06633 
06634     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06635     i__1 = *ks;
06636     int tmp__j=0;
06637     for (i__ = 1; i__ <= i__1; ++i__) {
06638         tmp = 0.f;
06639         i__2 = *n - 1;
06640         for (j = 1; j <= i__2; ++j) {
06641         tmp__j=j;
06642             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06643         }
06644         tmp += x[*n];
06645         p.push_back(static_cast<float>(exp(tmp)));
06646         p.push_back(q1[i__ + q1_dim1]);
06647     }
06648     i__2=*n;
06649     for (i__=1;i__<=i__2;++i__)
06650         { p.push_back(static_cast<float>(x[i__]));}
06651     return p;
06652 }
06653 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06654         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06655 {
06656 
06657     long int q_dim1, q_offset, i__1, i__2;
06658     double d__1;
06659 
06660     static long int i__, j;
06661     static double z__;
06662     static long int n1, n2, ia, ii, kk, in, nk, js;
06663     static double sn, zu, zv;
06664     static long int nk1, klm, nkl, jmn, jpn;
06665     static double cuv;
06666     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06667     static float xmin;
06668     static double xmax;
06669     static long int iout;
06670     static double xsum;
06671     static long int iineg, maxit;
06672     static double toler;
06673     static float error;
06674     static double pivot;
06675     static long int kforce, iphase;
06676     static double tpivot;
06677 
06678     --s;
06679     --res;
06680     iu -= 3;
06681     cu -= 3;
06682     --x;
06683     q_dim1 = *klm2d;
06684     q_offset = 1 + q_dim1;
06685     q -= q_offset;
06686 
06687     /* Function Body */
06688     maxit = 500;
06689     kode = 0;
06690     toler = 1e-4f;
06691     iter = 0;
06692     n1 = *n + 1;
06693     n2 = *n + 2;
06694     nk = *n + *k;
06695     nk1 = nk + 1;
06696     nkl = nk + *l;
06697     nkl1 = nkl + 1;
06698     klm = *k + *l + *m;
06699     klm1 = klm + 1;
06700     klm2 = klm + 2;
06701     nklm = *n + klm;
06702     kforce = 1;
06703     iter = 0;
06704     js = 1;
06705     ia = 0;
06706 /* SET UP LABELS IN Q. */
06707     i__1 = *n;
06708     for (j = 1; j <= i__1; ++j) {
06709         q[klm2 + j * q_dim1] = (double) j;
06710 /* L10: */
06711     }
06712     i__1 = klm;
06713     for (i__ = 1; i__ <= i__1; ++i__) {
06714         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06715         if (q[i__ + n1 * q_dim1] >= 0.f) {
06716             goto L30;
06717         }
06718         i__2 = n2;
06719         for (j = 1; j <= i__2; ++j) {
06720             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06721 /* L20: */
06722         }
06723 L30:
06724         ;
06725     }
06726 /* SET UP PHASE 1 COSTS. */
06727     iphase = 2;
06728     i__1 = nklm;
06729     for (j = 1; j <= i__1; ++j) {
06730         cu[(j << 1) + 1] = 0.f;
06731         cu[(j << 1) + 2] = 0.f;
06732         iu[(j << 1) + 1] = 0;
06733         iu[(j << 1) + 2] = 0;
06734 /* L40: */
06735     }
06736     if (*l == 0) {
06737         goto L60;
06738     }
06739     i__1 = nkl;
06740     for (j = nk1; j <= i__1; ++j) {
06741         cu[(j << 1) + 1] = 1.f;
06742         cu[(j << 1) + 2] = 1.f;
06743         iu[(j << 1) + 1] = 1;
06744         iu[(j << 1) + 2] = 1;
06745 /* L50: */
06746     }
06747     iphase = 1;
06748 L60:
06749     if (*m == 0) {
06750         goto L80;
06751     }
06752     i__1 = nklm;
06753     for (j = nkl1; j <= i__1; ++j) {
06754         cu[(j << 1) + 2] = 1.f;
06755         iu[(j << 1) + 2] = 1;
06756         jmn = j - *n;
06757         if (q[jmn + n2 * q_dim1] < 0.f) {
06758             iphase = 1;
06759         }
06760 /* L70: */
06761     }
06762 L80:
06763     if (kode == 0) {
06764         goto L150;
06765     }
06766     i__1 = *n;
06767     for (j = 1; j <= i__1; ++j) {
06768         if ((d__1 = x[j]) < 0.) {
06769             goto L90;
06770         } else if (d__1 == 0) {
06771             goto L110;
06772         } else {
06773             goto L100;
06774         }
06775 L90:
06776         cu[(j << 1) + 1] = 1.f;
06777         iu[(j << 1) + 1] = 1;
06778         goto L110;
06779 L100:
06780         cu[(j << 1) + 2] = 1.f;
06781         iu[(j << 1) + 2] = 1;
06782 L110:
06783         ;
06784     }
06785     i__1 = *k;
06786     for (j = 1; j <= i__1; ++j) {
06787         jpn = j + *n;
06788         if ((d__1 = res[j]) < 0.) {
06789             goto L120;
06790         } else if (d__1 == 0) {
06791             goto L140;
06792         } else {
06793             goto L130;
06794         }
06795 L120:
06796         cu[(jpn << 1) + 1] = 1.f;
06797         iu[(jpn << 1) + 1] = 1;
06798         if (q[j + n2 * q_dim1] > 0.f) {
06799             iphase = 1;
06800         }
06801         goto L140;
06802 L130:
06803         cu[(jpn << 1) + 2] = 1.f;
06804         iu[(jpn << 1) + 2] = 1;
06805         if (q[j + n2 * q_dim1] < 0.f) {
06806             iphase = 1;
06807         }
06808 L140:
06809         ;
06810     }
06811 L150:
06812     if (iphase == 2) {
06813         goto L500;
06814     }
06815 /* COMPUTE THE MARGINAL COSTS. */
06816 L160:
06817     i__1 = n1;
06818     for (j = js; j <= i__1; ++j) {
06819         xsum = 0.;
06820         i__2 = klm;
06821         for (i__ = 1; i__ <= i__2; ++i__) {
06822             ii = (long int) q[i__ + n2 * q_dim1];
06823             if (ii < 0) {
06824                 goto L170;
06825             }
06826             z__ = cu[(ii << 1) + 1];
06827             goto L180;
06828 L170:
06829             iineg = -ii;
06830             z__ = cu[(iineg << 1) + 2];
06831 L180:
06832             xsum += q[i__ + j * q_dim1] * z__;
06833 /*  180       XSUM = XSUM + Q(I,J)*Z */
06834 /* L190: */
06835         }
06836         q[klm1 + j * q_dim1] = xsum;
06837 /* L200: */
06838     }
06839     i__1 = *n;
06840     for (j = js; j <= i__1; ++j) {
06841         ii = (long int) q[klm2 + j * q_dim1];
06842         if (ii < 0) {
06843             goto L210;
06844         }
06845         z__ = cu[(ii << 1) + 1];
06846         goto L220;
06847 L210:
06848         iineg = -ii;
06849         z__ = cu[(iineg << 1) + 2];
06850 L220:
06851         q[klm1 + j * q_dim1] -= z__;
06852 /* L230: */
06853     }
06854 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06855 L240:
06856     xmax = 0.f;
06857     if (js > *n) {
06858         goto L490;
06859     }
06860     i__1 = *n;
06861     for (j = js; j <= i__1; ++j) {
06862         zu = q[klm1 + j * q_dim1];
06863         ii = (long int) q[klm2 + j * q_dim1];
06864         if (ii > 0) {
06865             goto L250;
06866         }
06867         ii = -ii;
06868         zv = zu;
06869         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06870         goto L260;
06871 L250:
06872         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06873 L260:
06874         if (kforce == 1 && ii > *n) {
06875             goto L280;
06876         }
06877         if (iu[(ii << 1) + 1] == 1) {
06878             goto L270;
06879         }
06880         if (zu <= xmax) {
06881             goto L270;
06882         }
06883         xmax = zu;
06884         in = j;
06885 L270:
06886         if (iu[(ii << 1) + 2] == 1) {
06887             goto L280;
06888         }
06889         if (zv <= xmax) {
06890             goto L280;
06891         }
06892         xmax = zv;
06893         in = j;
06894 L280:
06895         ;
06896     }
06897     if (xmax <= toler) {
06898         goto L490;
06899     }
06900     if (q[klm1 + in * q_dim1] == xmax) {
06901         goto L300;
06902     }
06903     i__1 = klm2;
06904     for (i__ = 1; i__ <= i__1; ++i__) {
06905         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06906 /* L290: */
06907     }
06908     q[klm1 + in * q_dim1] = xmax;
06909 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06910 L300:
06911     if (iphase == 1 || ia == 0) {
06912         goto L330;
06913     }
06914     xmax = 0.f;
06915     i__1 = ia;
06916     for (i__ = 1; i__ <= i__1; ++i__) {
06917         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06918         if (z__ <= xmax) {
06919             goto L310;
06920         }
06921         xmax = z__;
06922         iout = i__;
06923 L310:
06924         ;
06925     }
06926     if (xmax <= toler) {
06927         goto L330;
06928     }
06929     i__1 = n2;
06930     for (j = 1; j <= i__1; ++j) {
06931         z__ = q[ia + j * q_dim1];
06932         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06933         q[iout + j * q_dim1] = z__;
06934 /* L320: */
06935     }
06936     iout = ia;
06937     --ia;
06938     pivot = q[iout + in * q_dim1];
06939     goto L420;
06940 L330:
06941     kk = 0;
06942     i__1 = klm;
06943     for (i__ = 1; i__ <= i__1; ++i__) {
06944         z__ = q[i__ + in * q_dim1];
06945         if (z__ <= toler) {
06946             goto L340;
06947         }
06948         ++kk;
06949         res[kk] = q[i__ + n1 * q_dim1] / z__;
06950         s[kk] = (double) i__;
06951 L340:
06952         ;
06953     }
06954 L350:
06955     if (kk > 0) {
06956         goto L360;
06957     }
06958     kode = 2;
06959     goto L590;
06960 L360:
06961     xmin = static_cast<float>( res[1] );
06962     iout = (long int) s[1];
06963     j = 1;
06964     if (kk == 1) {
06965         goto L380;
06966     }
06967     i__1 = kk;
06968     for (i__ = 2; i__ <= i__1; ++i__) {
06969         if (res[i__] >= xmin) {
06970             goto L370;
06971         }
06972         j = i__;
06973         xmin = static_cast<float>( res[i__] );
06974         iout = (long int) s[i__];
06975 L370:
06976         ;
06977     }
06978     res[j] = res[kk];
06979     s[j] = s[kk];
06980 L380:
06981     --kk;
06982     pivot = q[iout + in * q_dim1];
06983     ii = (long int) q[iout + n2 * q_dim1];
06984     if (iphase == 1) {
06985         goto L400;
06986     }
06987     if (ii < 0) {
06988         goto L390;
06989     }
06990     if (iu[(ii << 1) + 2] == 1) {
06991         goto L420;
06992     }
06993     goto L400;
06994 L390:
06995     iineg = -ii;
06996     if (iu[(iineg << 1) + 1] == 1) {
06997         goto L420;
06998     }
06999 /* 400 II = IABS(II) */
07000 L400:
07001     ii = abs(ii);
07002     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
07003     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
07004         goto L420;
07005     }
07006 /* BYPASS INTERMEDIATE VERTICES. */
07007     i__1 = n1;
07008     for (j = js; j <= i__1; ++j) {
07009         z__ = q[iout + j * q_dim1];
07010         q[klm1 + j * q_dim1] -= z__ * cuv;
07011         q[iout + j * q_dim1] = -z__;
07012 /* L410: */
07013     }
07014     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
07015     goto L350;
07016 /* GAUSS-JORDAN ELIMINATION. */
07017 L420:
07018     if (iter < maxit) {
07019         goto L430;
07020     }
07021     kode = 3;
07022     goto L590;
07023 L430:
07024     ++iter;
07025     i__1 = n1;
07026     for (j = js; j <= i__1; ++j) {
07027         if (j != in) {
07028             q[iout + j * q_dim1] /= pivot;
07029         }
07030 /* L440: */
07031     }
07032 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
07033 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
07034 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
07035 /*     DO 460 J=JS,N1 */
07036 /*        IF(J .EQ. IN) GO TO 460 */
07037 /*        Z = -Q(IOUT,J) */
07038 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
07039 /* 460 CONTINUE */
07040     i__1 = n1;
07041     for (j = js; j <= i__1; ++j) {
07042         if (j == in) {
07043             goto L460;
07044         }
07045         z__ = -q[iout + j * q_dim1];
07046         i__2 = klm1;
07047         for (i__ = 1; i__ <= i__2; ++i__) {
07048             if (i__ != iout) {
07049                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
07050             }
07051 /* L450: */
07052         }
07053 L460:
07054         ;
07055     }
07056     tpivot = -pivot;
07057     i__1 = klm1;
07058     for (i__ = 1; i__ <= i__1; ++i__) {
07059         if (i__ != iout) {
07060             q[i__ + in * q_dim1] /= tpivot;
07061         }
07062 /* L470: */
07063     }
07064     q[iout + in * q_dim1] = 1.f / pivot;
07065     z__ = q[iout + n2 * q_dim1];
07066     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
07067     q[klm2 + in * q_dim1] = z__;
07068     ii = (long int) abs(z__);
07069     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
07070         goto L240;
07071     }
07072     i__1 = klm2;
07073     for (i__ = 1; i__ <= i__1; ++i__) {
07074         z__ = q[i__ + in * q_dim1];
07075         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
07076         q[i__ + js * q_dim1] = z__;
07077 /* L480: */
07078     }
07079     ++js;
07080     goto L240;
07081 /* TEST FOR OPTIMALITY. */
07082 L490:
07083     if (kforce == 0) {
07084         goto L580;
07085     }
07086     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07087         goto L500;
07088     }
07089     kforce = 0;
07090     goto L240;
07091 /* SET UP PHASE 2 COSTS. */
07092 L500:
07093     iphase = 2;
07094     i__1 = nklm;
07095     for (j = 1; j <= i__1; ++j) {
07096         cu[(j << 1) + 1] = 0.f;
07097         cu[(j << 1) + 2] = 0.f;
07098 /* L510: */
07099     }
07100     i__1 = nk;
07101     for (j = n1; j <= i__1; ++j) {
07102         cu[(j << 1) + 1] = 1.f;
07103         cu[(j << 1) + 2] = 1.f;
07104 /* L520: */
07105     }
07106     i__1 = klm;
07107     for (i__ = 1; i__ <= i__1; ++i__) {
07108         ii = (long int) q[i__ + n2 * q_dim1];
07109         if (ii > 0) {
07110             goto L530;
07111         }
07112         ii = -ii;
07113         if (iu[(ii << 1) + 2] == 0) {
07114             goto L560;
07115         }
07116         cu[(ii << 1) + 2] = 0.f;
07117         goto L540;
07118 L530:
07119         if (iu[(ii << 1) + 1] == 0) {
07120             goto L560;
07121         }
07122         cu[(ii << 1) + 1] = 0.f;
07123 L540:
07124         ++ia;
07125         i__2 = n2;
07126         for (j = 1; j <= i__2; ++j) {
07127             z__ = q[ia + j * q_dim1];
07128             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07129             q[i__ + j * q_dim1] = z__;
07130 /* L550: */
07131         }
07132 L560:
07133         ;
07134     }
07135     goto L160;
07136 L570:
07137     if (q[klm1 + n1 * q_dim1] <= toler) {
07138         goto L500;
07139     }
07140     kode = 1;
07141     goto L590;
07142 L580:
07143     if (iphase == 1) {
07144         goto L570;
07145     }
07146 /* PREPARE OUTPUT. */
07147     kode = 0;
07148 L590:
07149     xsum = 0.;
07150     i__1 = *n;
07151     for (j = 1; j <= i__1; ++j) {
07152         x[j] = 0.f;
07153 /* L600: */
07154     }
07155     i__1 = klm;
07156     for (i__ = 1; i__ <= i__1; ++i__) {
07157         res[i__] = 0.f;
07158 /* L610: */
07159     }
07160     i__1 = klm;
07161     for (i__ = 1; i__ <= i__1; ++i__) {
07162         ii = (long int) q[i__ + n2 * q_dim1];
07163         sn = 1.f;
07164         if (ii > 0) {
07165             goto L620;
07166         }
07167         ii = -ii;
07168         sn = -1.f;
07169 L620:
07170         if (ii > *n) {
07171             goto L630;
07172         }
07173         x[ii] = sn * q[i__ + n1 * q_dim1];
07174         goto L640;
07175 L630:
07176         iimn = ii - *n;
07177         res[iimn] = sn * q[i__ + n1 * q_dim1];
07178         if (ii >= n1 && ii <= nk) {
07179             xsum += q[i__ + n1 * q_dim1];
07180         }
07181 L640:
07182         ;
07183     }
07184     error = (float)xsum;
07185     return;
07186 }
07187 
07188 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07189 {
07190         int j,d;
07191         EMData * e = new EMData();
07192         float *eptr, *imgptr;
07193         imgptr = img->get_data();
07194         float SSE = 0.f;
07195         for (j = 0 ; j < N ; j++) {
07196                 e->read_image(images,S[j]);
07197                 eptr = e->get_data();
07198                 for (d = 0; d < size; d++) {
07199                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07200                 }
07201         delete e;
07202         return SSE;
07203 }
07204 
07205 
07206 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07207 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07208 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07209 
07210 
07211 #define         quadpi                  3.141592653589793238462643383279502884197
07212 #define         dgr_to_rad              quadpi/180
07213 #define         deg_to_rad              quadpi/180
07214 #define         rad_to_deg              180/quadpi
07215 #define         rad_to_dgr              180/quadpi
07216 #define         TRUE                    1
07217 #define         FALSE                   0
07218 
07219 
07220 #define theta(i)                theta   [i-1]
07221 #define phi(i)                  phi     [i-1]
07222 #define weight(i)               weight  [i-1]
07223 #define lband(i)                lband   [i-1]
07224 #define ts(i)                   ts      [i-1]
07225 #define thetast(i)              thetast [i-1]
07226 #define key(i)                  key     [i-1]
07227 
07228 
07229 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07230 {
07231 
07232         ENTERFUNC;
07233 
07234         if ( th.size() != ph.size() ) {
07235                 LOGERR("images not same size");
07236                 throw ImageFormatException( "images not same size");
07237         }
07238 
07239         // rand_seed
07240         srand(10);
07241 
07242         int i,*key;
07243         int len = th.size();
07244         double *theta,*phi,*weight;
07245         theta   =       (double*) calloc(len,sizeof(double));
07246         phi     =       (double*) calloc(len,sizeof(double));
07247         weight  =       (double*) calloc(len,sizeof(double));
07248         key     =       (int*) calloc(len,sizeof(int));
07249         const float *thptr, *phptr;
07250 
07251         thptr = &th[0];
07252         phptr = &ph[0];
07253         for(i=1;i<=len;i++){
07254                 key(i) = i;
07255                 weight(i) = 0.0;
07256         }
07257 
07258         for(i = 0;i<len;i++){
07259                 theta[i] = thptr[i];
07260                 phi[i]   = phptr[i];
07261         }
07262 
07263         //  sort by theta
07264         Util::hsortd(theta, phi, key, len, 1);
07265 
07266         //Util::voronoidiag(theta,phi, weight, len);
07267         Util::voronoi(phi, theta, weight, len);
07268 
07269         //sort by key
07270         Util::hsortd(weight, weight, key, len, 2);
07271 
07272         free(theta);
07273         free(phi);
07274         free(key);
07275         vector<double> wt;
07276         double count = 0;
07277         for(i=1; i<= len; i++)
07278         {
07279                 wt.push_back(weight(i));
07280                 count += weight(i);
07281         }
07282 
07283         //if( abs(count-6.28) > 0.1 )
07284         //{
07285         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07286         //}
07287 
07288         free(weight);
07289 
07290         EXITFUNC;
07291         return wt;
07292 
07293 }
07294 
07295 struct  tmpstruct{
07296         double theta1,phi1;
07297         int key1;
07298         };
07299 
07300 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07301 {
07302         ENTERFUNC;
07303         vector<tmpstruct> tmp(len);
07304         int i;
07305         for(i = 1;i<=len;i++)
07306         {
07307                 tmp[i-1].theta1 = theta(i);
07308                 tmp[i-1].phi1 = phi(i);
07309                 tmp[i-1].key1 = key(i);
07310         }
07311 
07312         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07313         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07314 
07315         for(i = 1;i<=len;i++)
07316         {
07317                 theta(i) = tmp[i-1].theta1;
07318                 phi(i)   = tmp[i-1].phi1;
07319                 key(i)   = tmp[i-1].key1;
07320         }
07321         EXITFUNC;
07322 }
07323 
07324 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07325 {
07326         return(tmp1.theta1 < tmp2.theta1);
07327 }
07328 
07329 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07330 {
07331         return(tmp1.key1 < tmp2.key1);
07332 }
07333 
07334 /******************  VORONOI DIAGRAM **********************************/
07335 /*
07336 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07337 {
07338         ENTERFUNC;
07339 
07340         int     *lband;
07341         double  aat=0.0f,*ts;
07342         double  aa,acum,area;
07343         int     last;
07344         int numth       =       1;
07345         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07346 
07347         int i,it,l,k;
07348         int nband,lb,low,medium,lhigh,lbw,lenw;
07349 
07350 
07351         lband   =       (int*)calloc(nbt,sizeof(int));
07352         ts      =       (double*)calloc(nbt,sizeof(double));
07353 
07354         if(lband == NULL || ts == NULL ){
07355                 fprintf(stderr,"memory allocation failure!\n");
07356                 exit(1);
07357         }
07358 
07359         nband=nbt;
07360 
07361         while(nband>0){
07362                 Util::angstep(ts,nband);
07363 
07364                 l=1;
07365                 for(i=1;i<=n;i++){
07366                         if(theta(i)>ts(l)){
07367                                 lband(l)=i;
07368                                 l=l+1;
07369                                 if(l>nband)  exit(1);
07370                         }
07371                 }
07372 
07373                 l=1;
07374                 for(i=1;i<=n;i++){
07375                         if(theta(i)>ts(l)){
07376                                 lband(l)=i;
07377                                 l=l+1;
07378                                 if(l>nband)  exit(1);
07379                         }
07380                 }
07381 
07382                 lband(l)=n+1;
07383                 acum=0.0;
07384                 for(it=l;it>=1;it-=numth){
07385                         for(i=it;i>=mymax(1,it-numth+1);i--){
07386                         if(i==l) last   =        TRUE;
07387                         else     last   =        FALSE;
07388 
07389                         if(l==1){
07390                                 lb=1;
07391                                 low=1;
07392                                 medium=n+1;
07393                                 lhigh=n-lb+1;
07394                                 lbw=1;
07395                         }
07396                         else if(i==1){
07397                                 lb=1;
07398                                 low=1;
07399                                 medium=lband(1);
07400                                 lhigh=lband(2)-1;
07401                                 lbw=1;
07402                         }
07403                         else if(i==l){
07404                                 if(l==2)        lb=1;
07405                                 else            lb=lband(l-2);
07406                                 low=lband(l-1)-lb+1;
07407                                 medium=lband(l)-lb+1;
07408                                 lhigh=n-lb+1;
07409                                 lbw=lband(i-1);
07410                         }
07411                         else{
07412                                 if(i==2)        lb=1;
07413                                 else            lb=lband(i-2);
07414                                 low=lband(i-1)-lb+1;
07415                                 medium=lband(i)-lb+1;
07416                                 lhigh=lband(i+1)-1-lb+1;
07417                                 lbw=lband(i-1);
07418                         }
07419                         lenw=medium-low;
07420 
07421 
07422                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07423 
07424 
07425                         if(nband>1){
07426                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07427                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07428 
07429                                 aa = 0.0;
07430                                 for(k = lbw;k<=lbw+lenw-1;k++)
07431                                         aa = aa+weight(k);
07432 
07433                                 acum=acum+aa;
07434                                 aat=aa/area;
07435                                 }
07436 
07437                         }
07438                         for(i=it;mymax(1,it-numth+1);i--){
07439                         if(fabs(aat-1.0)>0.02){
07440                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07441                                 goto  label2;
07442                                 }
07443                         }
07444                 acum=acum/quadpi/2.0;
07445                 exit(1);
07446 label2:
07447 
07448                 continue;
07449                 }
07450 
07451         free(ts);
07452         free(lband);
07453 
07454         }
07455 
07456         EXITFUNC;
07457 }
07458 
07459 
07460 void Util::angstep(double* thetast,int len){
07461 
07462         ENTERFUNC;
07463 
07464         double t1,t2,tmp;
07465         int i;
07466         if(len>1){
07467                 t1=0;
07468                 for(i=1;i<=len-1;i++){
07469                         tmp=cos(t1)-1.0/((float)len);
07470                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07471                         thetast(i)=t2 * rad_to_deg;
07472                         t1=t2;
07473                 }
07474         }
07475         thetast(len)=90.0;
07476 
07477         EXITFUNC;
07478 }
07479 */
07480 /*
07481 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07482 {
07483 
07484         ENTERFUNC;
07485         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07486         int nt6, n, ier,nout,lnew,mdup,nd;
07487         int i,k,mt,status;
07488 
07489 
07490         double *ds, *x, *y, *z;
07491         double tol=1.0e-8;
07492         double a;
07493 
07494         if(last){
07495                 if(medium>nt)  n = nt+nt;
07496                 else           n = nt+nt-medium+1;
07497         }
07498         else{
07499                 n=nt;
07500         }
07501 
07502         nt6 = n*6;
07503 
07504         list = (int*)calloc(nt6,sizeof(int));
07505         lptr = (int*)calloc(nt6,sizeof(int));
07506         lend = (int*)calloc(n  ,sizeof(int));
07507         iwk  = (int*)calloc(n  ,sizeof(int));
07508         good = (int*)calloc(n  ,sizeof(int));
07509         key  = (int*)calloc(n  ,sizeof(int));
07510         indx = (int*)calloc(n  ,sizeof(int));
07511         lcnt = (int*)calloc(n  ,sizeof(int));
07512 
07513         ds      =       (double*) calloc(n,sizeof(double));
07514         x       =       (double*) calloc(n,sizeof(double));
07515         y       =       (double*) calloc(n,sizeof(double));
07516         z       =       (double*) calloc(n,sizeof(double));
07517 
07518         if (list == NULL ||
07519         lptr == NULL ||
07520         lend == NULL ||
07521         iwk  == NULL ||
07522         good == NULL ||
07523         key  == NULL ||
07524         indx == NULL ||
07525         lcnt == NULL ||
07526         x    == NULL ||
07527         y    == NULL ||
07528         z    == NULL ||
07529         ds   == NULL) {
07530                 printf("memory allocation failure!\n");
07531                 exit(1);
07532         }
07533 
07534 
07535 
07536         for(i = 1;i<=nt;i++){
07537                 x[i-1] = theta(i);
07538                 y[i-1] = phi(i);
07539         }
07540 
07541 
07542 
07543         if (last) {
07544                 for(i=nt+1;i<=n;i++){
07545                         x[i-1]=180.0-x[2*nt-i];
07546                         y[i-1]=180.0+y[2*nt-i];
07547                 }
07548         }
07549 
07550 
07551         Util::disorder2(x,y,key,n);
07552 
07553         Util::ang_to_xyz(x,y,z,n);
07554 
07555 
07556         //  Make sure that first three are no colinear
07557         label1:
07558         for(k=0; k<2; k++){
07559                 for(i=k+1; i<3; i++){
07560                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07561                                 Util::flip23(x, y, z, key, k, n);
07562                                 goto label1;
07563                         }
07564                 }
07565         }
07566 
07567 
07568         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07569 
07570 
07571         if (status != 0) {
07572                 printf(" error in trmsh3 \n");
07573                 exit(1);
07574         }
07575 
07576 
07577         mdup=n-nout;
07578         if (ier == -2) {
07579                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07580                 exit(1);
07581         }
07582         else if (ier > 0) {
07583                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07584                 exit(1);
07585         }
07586 
07587         nd=0;
07588         for (k=1;k<=n;k++){
07589                 if (indx[k-1]>0){
07590                         nd++;
07591                         good[nd-1]=k;
07592                 }
07593         }
07594 
07595 
07596         for(i = 1;i<=nout;i++) {
07597                 k=good[i-1];
07598                 if (key[k-1] >= low && key[k-1]<medium){
07599                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07600                         if (ier != 0){
07601                                 weight[key[k-1]-low] =-1.0;
07602                         }
07603                         else {
07604                                 weight[key[k-1]-low]=a/lcnt[i-1];
07605                         }
07606                 }
07607         }
07608 
07609 // Fill out the duplicated weights
07610         for(i = 1;i<=n;i++){
07611                 mt=-indx[i-1];
07612                 if (mt>0){
07613                         k=good[mt-1];
07614 //  This is a duplicated entry, get the already calculated
07615 //   weight and assign it.
07616                         if (key[i-1]>=low && key[i-1]<medium){
07617 //  Is it already calculated weight??
07618                                 if(key[k-1]>=low && key[k-1]<medium){
07619                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07620                                 }
07621                                 else{
07622 //  No, the weight is from the outside of valid region, calculate it anyway
07623                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07624                                         if (ier != 0){
07625                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07626                                                 weight[key[i-1]-low] =-1.0;
07627                                         }
07628                                         else {
07629                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07630                                         }
07631                                 }
07632                         }
07633                 }
07634         }
07635 
07636 
07637         free(list);
07638         free(lend);
07639         free(iwk);
07640         free(good);
07641         free(key);
07642 
07643         free(indx);
07644         free(lcnt);
07645         free(ds);
07646         free(x);
07647         free(y);
07648         free(z);
07649         EXITFUNC;
07650 }
07651 */
07652 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07653 {
07654 
07655         ENTERFUNC;
07656 
07657         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07658         int nt6, n, ier, nout, lnew, mdup, nd;
07659         int i,k,mt,status;
07660 
07661 
07662         double *ds, *x, *y, *z;
07663         double tol  = 1.0e-8;
07664         double dtol = 15;
07665         double a;
07666 
07667         /*if(last){
07668                 if(medium>nt)  n = nt+nt;
07669                 else           n = nt+nt-medium+1;
07670         }
07671         else{
07672                 n=nt;
07673         }*/
07674 
07675         n = nt + nt;
07676 
07677         nt6 = n*6;
07678 
07679         list = (int*)calloc(nt6,sizeof(int));
07680         lptr = (int*)calloc(nt6,sizeof(int));
07681         lend = (int*)calloc(n  ,sizeof(int));
07682         iwk  = (int*)calloc(n  ,sizeof(int));
07683         good = (int*)calloc(n  ,sizeof(int));
07684         key  = (int*)calloc(n  ,sizeof(int));
07685         indx = (int*)calloc(n  ,sizeof(int));
07686         lcnt = (int*)calloc(n  ,sizeof(int));
07687 
07688         ds      =       (double*) calloc(n,sizeof(double));
07689         x       =       (double*) calloc(n,sizeof(double));
07690         y       =       (double*) calloc(n,sizeof(double));
07691         z       =       (double*) calloc(n,sizeof(double));
07692 
07693         if (list == NULL ||
07694         lptr == NULL ||
07695         lend == NULL ||
07696         iwk  == NULL ||
07697         good == NULL ||
07698         key  == NULL ||
07699         indx == NULL ||
07700         lcnt == NULL ||
07701         x    == NULL ||
07702         y    == NULL ||
07703         z    == NULL ||
07704         ds   == NULL) {
07705                 printf("memory allocation failure!\n");
07706                 exit(1);
07707         }
07708 
07709         bool colinear=true;
07710         while(colinear)
07711         {
07712 
07713         L1:
07714             for(i = 0; i<nt; i++){
07715                 x[i] = theta[i];
07716                 y[i] = phi[i];
07717                 x[nt+i] = 180.0 - x[i];
07718                 y[nt+i] = 180.0 + y[i];
07719             }
07720 
07721             Util::disorder2(x, y, key, n);
07722 
07723             // check if the first three angles are not close, else shuffle
07724             double val;
07725             for(k=0; k<2; k++){
07726                 for(i=k+1; i<3; i++){
07727                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07728                     if( val  < dtol) {
07729                         goto L1;
07730                     }
07731                 }
07732             }
07733 
07734             Util::ang_to_xyz(x, y, z, n);
07735 
07736             //  Make sure that first three has no duplication
07737             bool dupnode=true;
07738             dupnode=true;
07739             while(dupnode)
07740             {
07741                 for(k=0; k<2; k++){
07742                     for(i=k+1; i<3; i++){
07743                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07744                                 Util::flip23(x, y, z, key, k, n);
07745                                 continue;
07746                         }
07747                     }
07748                 }
07749                 dupnode = false;
07750             }
07751 
07752 
07753             ier = 0;
07754 
07755             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07756 
07757             if (status != 0) {
07758                 printf(" error in trmsh3 \n");
07759                 exit(1);
07760             }
07761 
07762             if (ier > 0) {
07763                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07764                 exit(1);
07765             }
07766 
07767             mdup=n-nout;
07768             if (ier == -2) {
07769                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07770             }
07771             else
07772             {
07773                 colinear=false;
07774             }
07775         }
07776 
07777 
07778         Assert( ier != -2 );
07779 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07780 //  INDX contains node numbers from the squeezed list
07781         nd=0;
07782         for (k=1; k<=n; k++){
07783                 if (indx[k-1]>0) {
07784                         nd++;
07785                         good[nd-1]=k;
07786                 }
07787         }
07788 
07789 //
07790 // *** Compute the Voronoi region areas.
07791 //
07792         for(i = 1; i<=nout; i++) {
07793                 k=good[i-1];
07794                 //  We only need n weights from hemisphere
07795                 if (key[k-1] <= nt) {
07796 //  CALCULATE THE AREA
07797                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07798                         if (ier != 0){
07799 //  We set the weight to -1, this will signal the error in the calling
07800 //   program, as the area will turn out incorrect
07801                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07802                                 weight[key[k-1]-1] =-1.0;
07803                         } else {
07804 //  Assign the weight
07805                                 weight[key[k-1]-1]=a/lcnt[i-1];
07806                         }
07807                 }
07808         }
07809 
07810 
07811 // Fill out the duplicated weights
07812         for(i = 1; i<=n; i++){
07813                 mt =- indx[i-1];
07814                 if (mt>0){
07815                         k = good[mt-1];
07816 //  This is a duplicated entry, get the already calculated
07817 //   weight and assign it.
07818                 //  We only need n weights from hemisphere
07819                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07820                         }
07821         }
07822 
07823         free(list);
07824         free(lend);
07825         free(iwk);
07826         free(good);
07827         free(key);
07828         free(lptr);
07829         free(indx);
07830         free(lcnt);
07831         free(ds);
07832         free(x);
07833         free(y);
07834         free(z);
07835 
07836 
07837         EXITFUNC;
07838 }
07839 
07840 void Util::disorder2(double *x,double *y, int *key, int len)
07841 {
07842         ENTERFUNC;
07843         int k, i;
07844         for(i=0; i<len; i++) key[i]=i+1;
07845 
07846         for(i = 0; i<len;i++){
07847                 k = rand()%len;
07848                 std::swap(key[k], key[i]);
07849                 std::swap(x[k], x[i]);
07850                 std::swap(y[k], y[i]);
07851         }
07852         EXITFUNC;
07853 }
07854 
07855 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07856 {
07857         ENTERFUNC;
07858         double costheta,sintheta,cosphi,sinphi;
07859         for(int i = 0;  i<len;  i++)
07860         {
07861                 cosphi = cos(y[i]*dgr_to_rad);
07862                 sinphi = sin(y[i]*dgr_to_rad);
07863                 if(fabs(x[i]-90.0)< 1.0e-5){
07864                         x[i] = cosphi;
07865                         y[i] = sinphi;
07866                         z[i] = 0.0;
07867                 }
07868                 else{
07869                         costheta = cos(x[i]*dgr_to_rad);
07870                         sintheta = sin(x[i]*dgr_to_rad);
07871                         x[i] = cosphi*sintheta;
07872                         y[i] = sinphi*sintheta;
07873                         z[i] = costheta;
07874                 }
07875         }
07876         EXITFUNC;
07877 }
07878 
07879 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07880 {
07881         ENTERFUNC;
07882         int i = k;
07883         while( i == k )  i = rand()%len;
07884         std::swap(key[i], key[k]);
07885         std::swap(x[i], x[k]);
07886         std::swap(y[i], y[k]);
07887         std::swap(z[i], z[k]);
07888         EXITFUNC;
07889 }
07890 
07891 
07892 #undef  mymax
07893 #undef  mymin
07894 #undef  sign
07895 #undef  quadpi
07896 #undef  dgr_to_rad
07897 #undef  deg_to_rad
07898 #undef  rad_to_deg
07899 #undef  rad_to_dgr
07900 #undef  TRUE
07901 #undef  FALSE
07902 #undef  theta
07903 #undef  phi
07904 #undef  weight
07905 #undef  lband
07906 #undef  ts
07907 #undef  thetast
07908 #undef  key
07909 
07910 
07911 /*################################################################################################
07912 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07913 ######   You must link the resulting object file with the libraries: #############################
07914 ####################    -lf2c -lm   (in that order)   ############################################
07915 ################################################################################################*/
07916 
07917 /* Common Block Declarations */
07918 
07919 
07920 #define TRUE_ (1)
07921 #define FALSE_ (0)
07922 #define abs(x) ((x) >= 0 ? (x) : -(x))
07923 
07924 struct stcom_{
07925     double y;
07926 };
07927 stcom_ stcom_1;
07928 #ifdef KR_headers
07929 double floor();
07930 int i_dnnt(x) double *x;
07931 #else
07932 int i_dnnt(double *x)
07933 #endif
07934 {
07935         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07936 }
07937 
07938 
07939 
07940 
07941 /* ____________________STRID______________________________________ */
07942 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07943         double *y, double *z__, int *n, int *list, int *
07944         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07945         int *near__, int *next, double *dist, int *ier)
07946 {
07947     /* System generated locals */
07948     int i__1, i__2;
07949 
07950     /* Local variables */
07951     static double d__;
07952     static int i__, j;
07953     static double d1, d2, d3;
07954     static int i0, lp, kt, ku, lpl, nku;
07955     static int nexti;
07956 
07957 
07958 /* *********************************************************** */
07959 
07960 /*                                              From STRIPACK */
07961 /*                                            Robert J. Renka */
07962 /*                                  Dept. of Computer Science */
07963 /*                                       Univ. of North Texas */
07964 /*                                           renka@cs.unt.edu */
07965 /*                                                   01/20/03 */
07966 
07967 /*   This is an alternative to TRMESH with the inclusion of */
07968 /* an efficient means of removing duplicate or nearly dupli- */
07969 /* cate nodes. */
07970 
07971 /*   This subroutine creates a Delaunay triangulation of a */
07972 /* set of N arbitrarily distributed points, referred to as */
07973 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07974 /* routine TRMESH for definitions and a list of additional */
07975 /* subroutines.  This routine is an alternative to TRMESH */
07976 /* with the inclusion of an efficient means of removing dup- */
07977 /* licate or nearly duplicate nodes. */
07978 
07979 /*   The algorithm has expected time complexity O(N*log(N)) */
07980 /* for random nodal distributions. */
07981 
07982 
07983 /* On input: */
07984 
07985 /*       N0 = Number of nodes, possibly including duplicates. */
07986 /*            N0 .GE. 3. */
07987 
07988 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07989 /*             bound on the deviation from 1 of the cosine of */
07990 /*             the angle between the nodes.  Note that */
07991 /*             |1-cos(A)| is approximately A*A/2. */
07992 
07993 /* The above parameters are not altered by this routine. */
07994 
07995 /*       X,Y,Z = Arrays of length at least N0 containing the */
07996 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07997 /*               Z(K)) is referred to as node K, and K is re- */
07998 /*               ferred to as a nodal index.  It is required */
07999 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
08000 /*               K.  The first three nodes must not be col- */
08001 /*               linear (lie on a common great circle). */
08002 
08003 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
08004 
08005 /*       LEND = Array of length at least N0. */
08006 
08007 /*       INDX = Array of length at least N0. */
08008 
08009 /*       LCNT = Array of length at least N0 (length N is */
08010 /*              sufficient). */
08011 
08012 /*       NEAR,NEXT,DIST = Work space arrays of length at */
08013 /*                        least N0.  The space is used to */
08014 /*                        efficiently determine the nearest */
08015 /*                        triangulation node to each un- */
08016 /*                        processed node for use by ADDNOD. */
08017 
08018 /* On output: */
08019 
08020 /*       N = Number of nodes in the triangulation.  3 .LE. N */
08021 /*           .LE. N0, or N = 0 if IER < 0. */
08022 
08023 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
08024 /*               of the triangulation nodes in the first N */
08025 /*               locations.  The original array elements are */
08026 /*               shifted down as necessary to eliminate dup- */
08027 /*               licate nodes. */
08028 
08029 /*       LIST = Set of nodal indexes which, along with LPTR, */
08030 /*              LEND, and LNEW, define the triangulation as a */
08031 /*              set of N adjacency lists -- counterclockwise- */
08032 /*              ordered sequences of neighboring nodes such */
08033 /*              that the first and last neighbors of a bound- */
08034 /*              ary node are boundary nodes (the first neigh- */
08035 /*              bor of an interior node is arbitrary).  In */
08036 /*              order to distinguish between interior and */
08037 /*              boundary nodes, the last neighbor of each */
08038 /*              boundary node is represented by the negative */
08039 /*              of its index. */
08040 
08041 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
08042 /*              correspondence with the elements of LIST. */
08043 /*              LIST(LPTR(I)) indexes the node which follows */
08044 /*              LIST(I) in cyclical counterclockwise order */
08045 /*              (the first neighbor follows the last neigh- */
08046 /*              bor). */
08047 
08048 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
08049 /*              points to the last neighbor of node K for */
08050 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
08051 /*              only if K is a boundary node. */
08052 
08053 /*       LNEW = Pointer to the first empty location in LIST */
08054 /*              and LPTR (list length plus one).  LIST, LPTR, */
08055 /*              LEND, and LNEW are not altered if IER < 0, */
08056 /*              and are incomplete if IER > 0. */
08057 
08058 /*       INDX = Array of output (triangulation) nodal indexes */
08059 /*              associated with input nodes.  For I = 1 to */
08060 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
08061 /*              the triangulation node with the same (or */
08062 /*              nearly the same) coordinates as input node I. */
08063 
08064 /*       LCNT = Array of int weights (counts) associated */
08065 /*              with the triangulation nodes.  For I = 1 to */
08066 /*              N, LCNT(I) is the number of occurrences of */
08067 /*              node I in the input node set, and thus the */
08068 /*              number of duplicates is LCNT(I)-1. */
08069 
08070 /*       NEAR,NEXT,DIST = Garbage. */
08071 
08072 /*       IER = Error indicator: */
08073 /*             IER =  0 if no errors were encountered. */
08074 /*             IER = -1 if N0 < 3 on input. */
08075 /*             IER = -2 if the first three nodes are */
08076 /*                      collinear. */
08077 /*             IER = -3 if Subroutine ADDNOD returns an error */
08078 /*                      flag.  This should not occur. */
08079 
08080 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
08081 /*                                INSERT, INTADD, JRAND, */
08082 /*                                LEFT, LSTPTR, STORE, SWAP, */
08083 /*                                SWPTST, TRFIND */
08084 
08085 /* Intrinsic function called by TRMSH3:  ABS */
08086 
08087 /* *********************************************************** */
08088 
08089 
08090 /* Local parameters: */
08091 
08092 /* D =        (Negative cosine of) distance from node KT to */
08093 /*              node I */
08094 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
08095 /*              respectively */
08096 /* I,J =      Nodal indexes */
08097 /* I0 =       Index of the node preceding I in a sequence of */
08098 /*              unprocessed nodes:  I = NEXT(I0) */
08099 /* KT =       Index of a triangulation node */
08100 /* KU =       Index of an unprocessed node and DO-loop index */
08101 /* LP =       LIST index (pointer) of a neighbor of KT */
08102 /* LPL =      Pointer to the last neighbor of KT */
08103 /* NEXTI =    NEXT(I) */
08104 /* NKU =      NEAR(KU) */
08105 
08106     /* Parameter adjustments */
08107     --dist;
08108     --next;
08109     --near__;
08110     --indx;
08111     --lend;
08112     --z__;
08113     --y;
08114     --x;
08115     --list;
08116     --lptr;
08117     --lcnt;
08118 
08119     /* Function Body */
08120     if (*n0 < 3) {
08121         *n = 0;
08122         *ier = -1;
08123         return 0;
08124     }
08125 
08126 /* Store the first triangle in the linked list. */
08127 
08128     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08129             z__[3])) {
08130 
08131 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
08132 
08133         list[1] = 3;
08134         lptr[1] = 2;
08135         list[2] = -2;
08136         lptr[2] = 1;
08137         lend[1] = 2;
08138 
08139         list[3] = 1;
08140         lptr[3] = 4;
08141         list[4] = -3;
08142         lptr[4] = 3;
08143         lend[2] = 4;
08144 
08145         list[5] = 2;
08146         lptr[5] = 6;
08147         list[6] = -1;
08148         lptr[6] = 5;
08149         lend[3] = 6;
08150 
08151     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08152             y[3], &z__[3])) {
08153 
08154 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
08155 /*     i.e., node 3 lies in the left hemisphere defined by */
08156 /*     arc 1->2. */
08157 
08158         list[1] = 2;
08159         lptr[1] = 2;
08160         list[2] = -3;
08161         lptr[2] = 1;
08162         lend[1] = 2;
08163 
08164         list[3] = 3;
08165         lptr[3] = 4;
08166         list[4] = -1;
08167         lptr[4] = 3;
08168         lend[2] = 4;
08169 
08170         list[5] = 1;
08171         lptr[5] = 6;
08172         list[6] = -2;
08173         lptr[6] = 5;
08174         lend[3] = 6;
08175 
08176 
08177     } else {
08178 
08179 /*   The first three nodes are collinear. */
08180 
08181         *n = 0;
08182         *ier = -2;
08183         return 0;
08184     }
08185 
08186     //printf("pass check colinear\n");
08187 
08188 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08189 
08190     *lnew = 7;
08191     indx[1] = 1;
08192     indx[2] = 2;
08193     indx[3] = 3;
08194     lcnt[1] = 1;
08195     lcnt[2] = 1;
08196     lcnt[3] = 1;
08197     if (*n0 == 3) {
08198         *n = 3;
08199         *ier = 0;
08200         return 0;
08201     }
08202 
08203 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08204 /*   used to obtain an expected-time (N*log(N)) incremental */
08205 /*   algorithm by enabling constant search time for locating */
08206 /*   each new node in the triangulation. */
08207 
08208 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08209 /*   triangulation node closest to KU (used as the starting */
08210 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08211 /*   is an increasing function of the arc length (angular */
08212 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08213 /*   arc length a. */
08214 
08215 /* Since it is necessary to efficiently find the subset of */
08216 /*   unprocessed nodes associated with each triangulation */
08217 /*   node J (those that have J as their NEAR entries), the */
08218 /*   subsets are stored in NEAR and NEXT as follows:  for */
08219 /*   each node J in the triangulation, I = NEAR(J) is the */
08220 /*   first unprocessed node in J's set (with I = 0 if the */
08221 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08222 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08223 /*   set are initially ordered by increasing indexes (which */
08224 /*   maximizes efficiency) but that ordering is not main- */
08225 /*   tained as the data structure is updated. */
08226 
08227 /* Initialize the data structure for the single triangle. */
08228 
08229     near__[1] = 0;
08230     near__[2] = 0;
08231     near__[3] = 0;
08232     for (ku = *n0; ku >= 4; --ku) {
08233         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08234         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08235         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08236         if (d1 <= d2 && d1 <= d3) {
08237             near__[ku] = 1;
08238             dist[ku] = d1;
08239             next[ku] = near__[1];
08240             near__[1] = ku;
08241         } else if (d2 <= d1 && d2 <= d3) {
08242             near__[ku] = 2;
08243             dist[ku] = d2;
08244             next[ku] = near__[2];
08245             near__[2] = ku;
08246         } else {
08247             near__[ku] = 3;
08248             dist[ku] = d3;
08249             next[ku] = near__[3];
08250             near__[3] = ku;
08251         }
08252 /* L1: */
08253     }
08254 
08255 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08256 /*   in the triangulation, and NKU = NEAR(KU). */
08257 
08258     kt = 3;
08259     i__1 = *n0;
08260     for (ku = 4; ku <= i__1; ++ku) {
08261         nku = near__[ku];
08262 
08263 /* Remove KU from the set of unprocessed nodes associated */
08264 /*   with NEAR(KU). */
08265         i__ = nku;
08266         if (near__[i__] == ku) {
08267             near__[i__] = next[ku];
08268         } else {
08269             i__ = near__[i__];
08270 L2:
08271             i0 = i__;
08272             i__ = next[i0];
08273             if (i__ != ku) {
08274                 goto L2;
08275             }
08276             next[i0] = next[ku];
08277         }
08278         near__[ku] = 0;
08279 
08280 /* Bypass duplicate nodes. */
08281 
08282         if (dist[ku] <= *tol - 1.) {
08283             indx[ku] = -nku;
08284             ++lcnt[nku];
08285             goto L6;
08286         }
08287 
08288 
08289 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08290         ++kt;
08291         x[kt] = x[ku];
08292         y[kt] = y[ku];
08293         z__[kt] = z__[ku];
08294         indx[ku] = kt;
08295         lcnt[kt] = 1;
08296         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08297                 , lnew, ier);
08298         if (*ier != 0) {
08299             *n = 0;
08300             *ier = -3;
08301             return 0;
08302         }
08303 
08304 /* Loop on neighbors J of node KT. */
08305 
08306         lpl = lend[kt];
08307         lp = lpl;
08308 L3:
08309         lp = lptr[lp];
08310         j = (i__2 = list[lp], abs(i__2));
08311 
08312 /* Loop on elements I in the sequence of unprocessed nodes */
08313 /*   associated with J:  KT is a candidate for replacing J */
08314 /*   as the nearest triangulation node to I.  The next value */
08315 /*   of I in the sequence, NEXT(I), must be saved before I */
08316 /*   is moved because it is altered by adding I to KT's set. */
08317 
08318         i__ = near__[j];
08319 L4:
08320         if (i__ == 0) {
08321             goto L5;
08322         }
08323         nexti = next[i__];
08324 
08325 /* Test for the distance from I to KT less than the distance */
08326 /*   from I to J. */
08327 
08328         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08329         if (d__ < dist[i__]) {
08330 
08331 /* Replace J by KT as the nearest triangulation node to I: */
08332 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08333 /*   of unprocessed nodes and add it to KT's set. */
08334 
08335             near__[i__] = kt;
08336             dist[i__] = d__;
08337             if (i__ == near__[j]) {
08338                 near__[j] = nexti;
08339             } else {
08340                 next[i0] = nexti;
08341             }
08342             next[i__] = near__[kt];
08343             near__[kt] = i__;
08344         } else {
08345             i0 = i__;
08346         }
08347 
08348 /* Bottom of loop on I. */
08349 
08350         i__ = nexti;
08351         goto L4;
08352 
08353 /* Bottom of loop on neighbors J. */
08354 
08355 L5:
08356         if (lp != lpl) {
08357             goto L3;
08358         }
08359 L6:
08360         ;
08361     }
08362     *n = kt;
08363     *ier = 0;
08364     return 0;
08365 } /* trmsh3_ */
08366 
08367 /* stripack.dbl sent by Robert on 06/03/03 */
08368 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08369         double *y, double *z__, int *list, int *lptr, int
08370         *lend, int *lnew, int *ier)
08371 {
08372     /* Initialized data */
08373 
08374     static double tol = 0.;
08375 
08376     /* System generated locals */
08377     int i__1;
08378 
08379     /* Local variables */
08380     static int l;
08381     static double p[3], b1, b2, b3;
08382     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08383     /* Subroutine */ int swap_(int *, int *, int *,
08384             int *, int *, int *, int *, int *);
08385     static int lpo1s;
08386     /* Subroutine */ int bdyadd_(int *, int *, int *,
08387             int *, int *, int *, int *), intadd_(int *,
08388             int *, int *, int *, int *, int *, int *,
08389             int *), trfind_(int *, double *, int *,
08390             double *, double *, double *, int *, int *,
08391             int *, double *, double *, double *, int *,
08392             int *, int *), covsph_(int *, int *, int *,
08393             int *, int *, int *);
08394     int lstptr_(int *, int *, int *, int *);
08395     long int swptst_(int *, int *, int *, int *,
08396             double *, double *, double *);
08397 
08398 
08399 /* *********************************************************** */
08400 
08401 /*                                              From STRIPACK */
08402 /*                                            Robert J. Renka */
08403 /*                                  Dept. of Computer Science */
08404 /*                                       Univ. of North Texas */
08405 /*                                           renka@cs.unt.edu */
08406 /*                                                   01/08/03 */
08407 
08408 /*   This subroutine adds node K to a triangulation of the */
08409 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08410 /* of the convex hull of nodes 1,...,K. */
08411 
08412 /*   The algorithm consists of the following steps:  node K */
08413 /* is located relative to the triangulation (TRFIND), its */
08414 /* index is added to the data structure (INTADD or BDYADD), */
08415 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08416 /* the arcs opposite K so that all arcs incident on node K */
08417 /* and opposite node K are locally optimal (satisfy the cir- */
08418 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08419 /* input, a Delaunay triangulation will result. */
08420 
08421 
08422 /* On input: */
08423 
08424 /*       NST = Index of a node at which TRFIND begins its */
08425 /*             search.  Search time depends on the proximity */
08426 /*             of this node to K.  If NST < 1, the search is */
08427 /*             begun at node K-1. */
08428 
08429 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08430 /*           new node to be added.  K .GE. 4. */
08431 
08432 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08433 /*               tesian coordinates of the nodes. */
08434 /*               (X(I),Y(I),Z(I)) defines node I for */
08435 /*               I = 1,...,K. */
08436 
08437 /* The above parameters are not altered by this routine. */
08438 
08439 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08440 /*                             the triangulation of nodes 1 */
08441 /*                             to K-1.  The array lengths are */
08442 /*                             assumed to be large enough to */
08443 /*                             add node K.  Refer to Subrou- */
08444 /*                             tine TRMESH. */
08445 
08446 /* On output: */
08447 
08448 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08449 /*                             the addition of node K as the */
08450 /*                             last entry unless IER .NE. 0 */
08451 /*                             and IER .NE. -3, in which case */
08452 /*                             the arrays are not altered. */
08453 
08454 /*       IER = Error indicator: */
08455 /*             IER =  0 if no errors were encountered. */
08456 /*             IER = -1 if K is outside its valid range */
08457 /*                      on input. */
08458 /*             IER = -2 if all nodes (including K) are col- */
08459 /*                      linear (lie on a common geodesic). */
08460 /*             IER =  L if nodes L and K coincide for some */
08461 /*                      L < K.  Refer to TOL below. */
08462 
08463 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08464 /*                                INTADD, JRAND, LSTPTR, */
08465 /*                                STORE, SWAP, SWPTST, */
08466 /*                                TRFIND */
08467 
08468 /* Intrinsic function called by ADDNOD:  ABS */
08469 
08470 /* *********************************************************** */
08471 
08472 
08473 /* Local parameters: */
08474 
08475 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08476 /*              by TRFIND. */
08477 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08478 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08479 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08480 /*              counterclockwise order. */
08481 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08482 /*              be tested for a swap */
08483 /* IST =      Index of node at which TRFIND begins its search */
08484 /* KK =       Local copy of K */
08485 /* KM1 =      K-1 */
08486 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08487 /*              if node K coincides with a vertex */
08488 /* LP =       LIST pointer */
08489 /* LPF =      LIST pointer to the first neighbor of K */
08490 /* LPO1 =     LIST pointer to IO1 */
08491 /* LPO1S =    Saved value of LPO1 */
08492 /* P =        Cartesian coordinates of node K */
08493 /* TOL =      Tolerance defining coincident nodes:  bound on */
08494 /*              the deviation from 1 of the cosine of the */
08495 /*              angle between the nodes. */
08496 /*              Note that |1-cos(A)| is approximately A*A/2. */
08497 
08498     /* Parameter adjustments */
08499     --lend;
08500     --z__;
08501     --y;
08502     --x;
08503     --list;
08504     --lptr;
08505 
08506     /* Function Body */
08507 
08508     kk = *k;
08509     if (kk < 4) {
08510         goto L3;
08511     }
08512 
08513 /* Initialization: */
08514     km1 = kk - 1;
08515     ist = *nst;
08516     if (ist < 1) {
08517         ist = km1;
08518     }
08519     p[0] = x[kk];
08520     p[1] = y[kk];
08521     p[2] = z__[kk];
08522 
08523 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08524 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08525 /*   from node K. */
08526     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08527             , &b1, &b2, &b3, &i1, &i2, &i3);
08528 
08529 /*   Test for collinear or (nearly) duplicate nodes. */
08530 
08531     if (i1 == 0) {
08532         goto L4;
08533     }
08534     l = i1;
08535     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08536         goto L5;
08537     }
08538     l = i2;
08539     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08540         goto L5;
08541     }
08542     if (i3 != 0) {
08543         l = i3;
08544         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08545             goto L5;
08546         }
08547         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08548     } else {
08549         if (i1 != i2) {
08550             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08551         } else {
08552             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08553         }
08554     }
08555     *ier = 0;
08556 
08557 /* Initialize variables for optimization of the */
08558 /*   triangulation. */
08559     lp = lend[kk];
08560     lpf = lptr[lp];
08561     io2 = list[lpf];
08562     lpo1 = lptr[lpf];
08563     io1 = (i__1 = list[lpo1], abs(i__1));
08564 
08565 /* Begin loop:  find the node opposite K. */
08566 
08567 L1:
08568     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08569     if (list[lp] < 0) {
08570         goto L2;
08571     }
08572     lp = lptr[lp];
08573     in1 = (i__1 = list[lp], abs(i__1));
08574 
08575 /* Swap test:  if a swap occurs, two new arcs are */
08576 /*             opposite K and must be tested. */
08577 
08578     lpo1s = lpo1;
08579     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08580         goto L2;
08581     }
08582     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08583     if (lpo1 == 0) {
08584 
08585 /*   A swap is not possible because KK and IN1 are already */
08586 /*     adjacent.  This error in SWPTST only occurs in the */
08587 /*     neutral case and when there are nearly duplicate */
08588 /*     nodes. */
08589 
08590         lpo1 = lpo1s;
08591         goto L2;
08592     }
08593     io1 = in1;
08594     goto L1;
08595 
08596 /* No swap occurred.  Test for termination and reset */
08597 /*   IO2 and IO1. */
08598 
08599 L2:
08600     if (lpo1 == lpf || list[lpo1] < 0) {
08601         return 0;
08602     }
08603     io2 = io1;
08604     lpo1 = lptr[lpo1];
08605     io1 = (i__1 = list[lpo1], abs(i__1));
08606     goto L1;
08607 
08608 /* KK < 4. */
08609 
08610 L3:
08611     *ier = -1;
08612     return 0;
08613 
08614 /* All nodes are collinear. */
08615 
08616 L4:
08617     *ier = -2;
08618     return 0;
08619 
08620 /* Nodes L and K coincide. */
08621 
08622 L5:
08623     *ier = l;
08624     return 0;
08625 } /* addnod_ */
08626 
08627 double angle_(double *v1, double *v2, double *v3)
08628 {
08629     /* System generated locals */
08630     double ret_val;
08631 
08632     /* Builtin functions */
08633     //double sqrt(double), acos(double);
08634 
08635     /* Local variables */
08636     static double a;
08637     static int i__;
08638     static double ca, s21, s23, u21[3], u23[3];
08639 
08640 
08641 /* *********************************************************** */
08642 
08643 /*                                              From STRIPACK */
08644 /*                                            Robert J. Renka */
08645 /*                                  Dept. of Computer Science */
08646 /*                                       Univ. of North Texas */
08647 /*                                           renka@cs.unt.edu */
08648 /*                                                   06/03/03 */
08649 
08650 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08651 /* face of the unit sphere, this function returns the */
08652 /* interior angle at V2 -- the dihedral angle between the */
08653 /* plane defined by V2 and V3 (and the origin) and the plane */
08654 /* defined by V2 and V1 or, equivalently, the angle between */
08655 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08656 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08657 /* wise.  The surface area of a spherical polygon with CCW- */
08658 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08659 /* Asum is the sum of the m interior angles computed from the */
08660 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08661 /* (Vm-1,Vm,V1). */
08662 
08663 
08664 /* On input: */
08665 
08666 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08667 /*                  sian coordinates of unit vectors.  These */
08668 /*                  vectors, if nonzero, are implicitly */
08669 /*                  scaled to have length 1. */
08670 
08671 /* Input parameters are not altered by this function. */
08672 
08673 /* On output: */
08674 
08675 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08676 /*               V2 X V3 = 0. */
08677 
08678 /* Module required by ANGLE:  LEFT */
08679 
08680 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08681 
08682 /* *********************************************************** */
08683 
08684 
08685 /* Local parameters: */
08686 
08687 /* A =       Interior angle at V2 */
08688 /* CA =      cos(A) */
08689 /* I =       DO-loop index and index for U21 and U23 */
08690 /* S21,S23 = Sum of squared components of U21 and U23 */
08691 /* U21,U23 = Unit normal vectors to the planes defined by */
08692 /*             pairs of triangle vertices */
08693 
08694 
08695 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08696 
08697     /* Parameter adjustments */
08698     --v3;
08699     --v2;
08700     --v1;
08701 
08702     /* Function Body */
08703     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08704     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08705     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08706 
08707     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08708     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08709     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08710 
08711 /* Normalize U21 and U23 to unit vectors. */
08712 
08713     s21 = 0.;
08714     s23 = 0.;
08715     for (i__ = 1; i__ <= 3; ++i__) {
08716         s21 += u21[i__ - 1] * u21[i__ - 1];
08717         s23 += u23[i__ - 1] * u23[i__ - 1];
08718 /* L1: */
08719     }
08720 
08721 /* Test for a degenerate triangle associated with collinear */
08722 /*   vertices. */
08723 
08724     if (s21 == 0. || s23 == 0.) {
08725         ret_val = 0.;
08726         return ret_val;
08727     }
08728     s21 = sqrt(s21);
08729     s23 = sqrt(s23);
08730     for (i__ = 1; i__ <= 3; ++i__) {
08731         u21[i__ - 1] /= s21;
08732         u23[i__ - 1] /= s23;
08733 /* L2: */
08734     }
08735 
08736 /* Compute the angle A between normals: */
08737 
08738 /*   CA = cos(A) = <U21,U23> */
08739 
08740     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08741     if (ca < -1.) {
08742         ca = -1.;
08743     }
08744     if (ca > 1.) {
08745         ca = 1.;
08746     }
08747     a = acos(ca);
08748 
08749 /* Adjust A to the interior angle:  A > Pi iff */
08750 /*   V3 Right V1->V2. */
08751 
08752     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08753             , &v3[3])) {
08754         a = acos(-1.) * 2. - a;
08755     }
08756     ret_val = a;
08757     return ret_val;
08758 } /* angle_ */
08759 
08760 double areas_(double *v1, double *v2, double *v3)
08761 {
08762     /* System generated locals */
08763     double ret_val;
08764 
08765     /* Builtin functions */
08766     //double sqrt(double), acos(double);
08767 
08768     /* Local variables */
08769     static int i__;
08770     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08771             ca2, ca3;
08772 
08773 
08774 /* *********************************************************** */
08775 
08776 /*                                              From STRIPACK */
08777 /*                                            Robert J. Renka */
08778 /*                                  Dept. of Computer Science */
08779 /*                                       Univ. of North Texas */
08780 /*                                           renka@cs.unt.edu */
08781 /*                                                   06/22/98 */
08782 
08783 /*   This function returns the area of a spherical triangle */
08784 /* on the unit sphere. */
08785 
08786 
08787 /* On input: */
08788 
08789 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08790 /*                  sian coordinates of unit vectors (the */
08791 /*                  three triangle vertices in any order). */
08792 /*                  These vectors, if nonzero, are implicitly */
08793 /*                  scaled to have length 1. */
08794 
08795 /* Input parameters are not altered by this function. */
08796 
08797 /* On output: */
08798 
08799 /*       AREAS = Area of the spherical triangle defined by */
08800 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08801 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08802 /*               if and only if V1, V2, and V3 lie in (or */
08803 /*               close to) a plane containing the origin. */
08804 
08805 /* Modules required by AREAS:  None */
08806 
08807 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08808 
08809 /* *********************************************************** */
08810 
08811 
08812 /* Local parameters: */
08813 
08814 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08815 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08816 /* I =           DO-loop index and index for Uij */
08817 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08818 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08819 /*                 pairs of triangle vertices */
08820 
08821 
08822 /* Compute cross products Uij = Vi X Vj. */
08823 
08824     /* Parameter adjustments */
08825     --v3;
08826     --v2;
08827     --v1;
08828 
08829     /* Function Body */
08830     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08831     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08832     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08833 
08834     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08835     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08836     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08837 
08838     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08839     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08840     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08841 
08842 /* Normalize Uij to unit vectors. */
08843 
08844     s12 = 0.;
08845     s23 = 0.;
08846     s31 = 0.;
08847     for (i__ = 1; i__ <= 3; ++i__) {
08848         s12 += u12[i__ - 1] * u12[i__ - 1];
08849         s23 += u23[i__ - 1] * u23[i__ - 1];
08850         s31 += u31[i__ - 1] * u31[i__ - 1];
08851 /* L2: */
08852     }
08853 
08854 /* Test for a degenerate triangle associated with collinear */
08855 /*   vertices. */
08856 
08857     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08858         ret_val = 0.;
08859         return ret_val;
08860     }
08861     s12 = sqrt(s12);
08862     s23 = sqrt(s23);
08863     s31 = sqrt(s31);
08864     for (i__ = 1; i__ <= 3; ++i__) {
08865         u12[i__ - 1] /= s12;
08866         u23[i__ - 1] /= s23;
08867         u31[i__ - 1] /= s31;
08868 /* L3: */
08869     }
08870 
08871 /* Compute interior angles Ai as the dihedral angles between */
08872 /*   planes: */
08873 /*           CA1 = cos(A1) = -<U12,U31> */
08874 /*           CA2 = cos(A2) = -<U23,U12> */
08875 /*           CA3 = cos(A3) = -<U31,U23> */
08876 
08877     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08878     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08879     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08880     if (ca1 < -1.) {
08881         ca1 = -1.;
08882     }
08883     if (ca1 > 1.) {
08884         ca1 = 1.;
08885     }
08886     if (ca2 < -1.) {
08887         ca2 = -1.;
08888     }
08889     if (ca2 > 1.) {
08890         ca2 = 1.;
08891     }
08892     if (ca3 < -1.) {
08893         ca3 = -1.;
08894     }
08895     if (ca3 > 1.) {
08896         ca3 = 1.;
08897     }
08898     a1 = acos(ca1);
08899     a2 = acos(ca2);
08900     a3 = acos(ca3);
08901 
08902 /* Compute AREAS = A1 + A2 + A3 - PI. */
08903 
08904     ret_val = a1 + a2 + a3 - acos(-1.);
08905     if (ret_val < 0.) {
08906         ret_val = 0.;
08907     }
08908     return ret_val;
08909 } /* areas_ */
08910 
08911 //double areas_(double *, double *, double *);
08912 
08913 double Util::areav_(int *k, int *n, double *x, double *y,
08914         double *z__, int *list, int *lptr, int *lend, int
08915         *ier)
08916 {
08917     /* Initialized data */
08918 
08919     static double amax = 6.28;
08920 
08921     /* System generated locals */
08922     double ret_val;
08923 
08924     /* Local variables */
08925     static double a, c0[3], c2[3], c3[3];
08926     static int n1, n2, n3;
08927     static double v1[3], v2[3], v3[3];
08928     static int lp, lpl, ierr;
08929     static double asum;
08930     static long int first;
08931 
08932 
08933 /* *********************************************************** */
08934 
08935 /*                                            Robert J. Renka */
08936 /*                                  Dept. of Computer Science */
08937 /*                                       Univ. of North Texas */
08938 /*                                           renka@cs.unt.edu */
08939 /*                                                   10/25/02 */
08940 
08941 /*   Given a Delaunay triangulation and the index K of an */
08942 /* interior node, this subroutine returns the (surface) area */
08943 /* of the Voronoi region associated with node K.  The Voronoi */
08944 /* region is the polygon whose vertices are the circumcenters */
08945 /* of the triangles that contain node K, where a triangle */
08946 /* circumcenter is the point (unit vector) lying at the same */
08947 /* angular distance from the three vertices and contained in */
08948 /* the same hemisphere as the vertices. */
08949 
08950 
08951 /* On input: */
08952 
08953 /*       K = Nodal index in the range 1 to N. */
08954 
08955 /*       N = Number of nodes in the triangulation.  N > 3. */
08956 
08957 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08958 /*               coordinates of the nodes (unit vectors). */
08959 
08960 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08961 /*                        gulation.  Refer to Subroutine */
08962 /*                        TRMESH. */
08963 
08964 /* Input parameters are not altered by this function. */
08965 
08966 /* On output: */
08967 
08968 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08969 /*               in which case AREAV = 0. */
08970 
08971 /*       IER = Error indicator: */
08972 /*             IER = 0 if no errors were encountered. */
08973 /*             IER = 1 if K or N is outside its valid range */
08974 /*                     on input. */
08975 /*             IER = 2 if K indexes a boundary node. */
08976 /*             IER = 3 if an error flag is returned by CIRCUM */
08977 /*                     (null triangle). */
08978 /*             IER = 4 if AREAS returns a value greater than */
08979 /*                     AMAX (defined below). */
08980 
08981 /* Modules required by AREAV:  AREAS, CIRCUM */
08982 
08983 /* *********************************************************** */
08984 
08985 
08986 /* Maximum valid triangle area is less than 2*Pi: */
08987 
08988     /* Parameter adjustments */
08989     --lend;
08990     --z__;
08991     --y;
08992     --x;
08993     --list;
08994     --lptr;
08995 
08996     /* Function Body */
08997 
08998 /* Test for invalid input. */
08999 
09000     if (*k < 1 || *k > *n || *n <= 3) {
09001         goto L11;
09002     }
09003 
09004 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09005 /*   FIRST = TRUE only for the first triangle. */
09006 /*   The Voronoi region area is accumulated in ASUM. */
09007 
09008     n1 = *k;
09009     v1[0] = x[n1];
09010     v1[1] = y[n1];
09011     v1[2] = z__[n1];
09012     lpl = lend[n1];
09013     n3 = list[lpl];
09014     if (n3 < 0) {
09015         goto L12;
09016     }
09017     lp = lpl;
09018     first = TRUE_;
09019     asum = 0.;
09020 
09021 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09022 
09023 L1:
09024     n2 = n3;
09025     lp = lptr[lp];
09026     n3 = list[lp];
09027     v2[0] = x[n2];
09028     v2[1] = y[n2];
09029     v2[2] = z__[n2];
09030     v3[0] = x[n3];
09031     v3[1] = y[n3];
09032     v3[2] = z__[n3];
09033     if (first) {
09034 
09035 /* First triangle:  compute the circumcenter C3 and save a */
09036 /*   copy in C0. */
09037 
09038         circum_(v1, v2, v3, c3, &ierr);
09039         if (ierr != 0) {
09040             goto L13;
09041         }
09042         c0[0] = c3[0];
09043         c0[1] = c3[1];
09044         c0[2] = c3[2];
09045         first = FALSE_;
09046     } else {
09047 
09048 /* Set C2 to C3, compute the new circumcenter C3, and compute */
09049 /*   the area A of triangle (V1,C2,C3). */
09050 
09051         c2[0] = c3[0];
09052         c2[1] = c3[1];
09053         c2[2] = c3[2];
09054         circum_(v1, v2, v3, c3, &ierr);
09055         if (ierr != 0) {
09056             goto L13;
09057         }
09058         a = areas_(v1, c2, c3);
09059         if (a > amax) {
09060             goto L14;
09061         }
09062         asum += a;
09063     }
09064 
09065 /* Bottom on loop on neighbors of K. */
09066 
09067     if (lp != lpl) {
09068         goto L1;
09069     }
09070 
09071 /* Compute the area of triangle (V1,C3,C0). */
09072 
09073     a = areas_(v1, c3, c0);
09074     if (a > amax) {
09075         goto L14;
09076     }
09077     asum += a;
09078 
09079 /* No error encountered. */
09080 
09081     *ier = 0;
09082     ret_val = asum;
09083     return ret_val;
09084 
09085 /* Invalid input. */
09086 
09087 L11:
09088     *ier = 1;
09089     ret_val = 0.;
09090     return ret_val;
09091 
09092 /* K indexes a boundary node. */
09093 
09094 L12:
09095     *ier = 2;
09096     ret_val = 0.;
09097     return ret_val;
09098 
09099 /* Error in CIRCUM. */
09100 
09101 L13:
09102     *ier = 3;
09103     ret_val = 0.;
09104     return ret_val;
09105 
09106 /* AREAS value larger than AMAX. */
09107 
09108 L14:
09109     *ier = 4;
09110     ret_val = 0.;
09111     return ret_val;
09112 } /* areav_ */
09113 
09114 double areav_new__(int *k, int *n, double *x, double *y,
09115         double *z__, int *list, int *lptr, int *lend, int
09116         *ier)
09117 {
09118     /* System generated locals */
09119     double ret_val = 0;
09120 
09121     /* Builtin functions */
09122     //double acos(double);
09123 
09124     /* Local variables */
09125     static int m;
09126     static double c1[3], c2[3], c3[3];
09127     static int n1, n2, n3;
09128     static double v1[3], v2[3], v3[3];
09129     static int lp;
09130     static double c1s[3], c2s[3];
09131     static int lpl, ierr;
09132     static double asum;
09133     double angle_(double *, double *, double *);
09134     static float areav;
09135 
09136 
09137 /* *********************************************************** */
09138 
09139 /*                                            Robert J. Renka */
09140 /*                                  Dept. of Computer Science */
09141 /*                                       Univ. of North Texas */
09142 /*                                           renka@cs.unt.edu */
09143 /*                                                   06/03/03 */
09144 
09145 /*   Given a Delaunay triangulation and the index K of an */
09146 /* interior node, this subroutine returns the (surface) area */
09147 /* of the Voronoi region associated with node K.  The Voronoi */
09148 /* region is the polygon whose vertices are the circumcenters */
09149 /* of the triangles that contain node K, where a triangle */
09150 /* circumcenter is the point (unit vector) lying at the same */
09151 /* angular distance from the three vertices and contained in */
09152 /* the same hemisphere as the vertices.  The Voronoi region */
09153 /* area is computed as Asum-(m-2)*Pi, where m is the number */
09154 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
09155 /* of interior angles at the vertices. */
09156 
09157 
09158 /* On input: */
09159 
09160 /*       K = Nodal index in the range 1 to N. */
09161 
09162 /*       N = Number of nodes in the triangulation.  N > 3. */
09163 
09164 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09165 /*               coordinates of the nodes (unit vectors). */
09166 
09167 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09168 /*                        gulation.  Refer to Subroutine */
09169 /*                        TRMESH. */
09170 
09171 /* Input parameters are not altered by this function. */
09172 
09173 /* On output: */
09174 
09175 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09176 /*               in which case AREAV = 0. */
09177 
09178 /*       IER = Error indicator: */
09179 /*             IER = 0 if no errors were encountered. */
09180 /*             IER = 1 if K or N is outside its valid range */
09181 /*                     on input. */
09182 /*             IER = 2 if K indexes a boundary node. */
09183 /*             IER = 3 if an error flag is returned by CIRCUM */
09184 /*                     (null triangle). */
09185 
09186 /* Modules required by AREAV:  ANGLE, CIRCUM */
09187 
09188 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09189 
09190 /* *********************************************************** */
09191 
09192 
09193 /* Test for invalid input. */
09194 
09195     /* Parameter adjustments */
09196     --lend;
09197     --z__;
09198     --y;
09199     --x;
09200     --list;
09201     --lptr;
09202 
09203     /* Function Body */
09204     if (*k < 1 || *k > *n || *n <= 3) {
09205         goto L11;
09206     }
09207 
09208 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09209 /*   The number of neighbors and the sum of interior angles */
09210 /*   are accumulated in M and ASUM, respectively. */
09211 
09212     n1 = *k;
09213     v1[0] = x[n1];
09214     v1[1] = y[n1];
09215     v1[2] = z__[n1];
09216     lpl = lend[n1];
09217     n3 = list[lpl];
09218     if (n3 < 0) {
09219         goto L12;
09220     }
09221     lp = lpl;
09222     m = 0;
09223     asum = 0.;
09224 
09225 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09226 
09227 L1:
09228     ++m;
09229     n2 = n3;
09230     lp = lptr[lp];
09231     n3 = list[lp];
09232     v2[0] = x[n2];
09233     v2[1] = y[n2];
09234     v2[2] = z__[n2];
09235     v3[0] = x[n3];
09236     v3[1] = y[n3];
09237     v3[2] = z__[n3];
09238     if (m == 1) {
09239 
09240 /* First triangle:  compute the circumcenter C2 and save a */
09241 /*   copy in C1S. */
09242 
09243         circum_(v1, v2, v3, c2, &ierr);
09244         if (ierr != 0) {
09245             goto L13;
09246         }
09247         c1s[0] = c2[0];
09248         c1s[1] = c2[1];
09249         c1s[2] = c2[2];
09250     } else if (m == 2) {
09251 
09252 /* Second triangle:  compute the circumcenter C3 and save a */
09253 /*   copy in C2S. */
09254 
09255         circum_(v1, v2, v3, c3, &ierr);
09256         if (ierr != 0) {
09257             goto L13;
09258         }
09259         c2s[0] = c3[0];
09260         c2s[1] = c3[1];
09261         c2s[2] = c3[2];
09262     } else {
09263 
09264 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09265 /*   C3, and compute the interior angle at C2 from the */
09266 /*   sequence of vertices (C1,C2,C3). */
09267 
09268         c1[0] = c2[0];
09269         c1[1] = c2[1];
09270         c1[2] = c2[2];
09271         c2[0] = c3[0];
09272         c2[1] = c3[1];
09273         c2[2] = c3[2];
09274         circum_(v1, v2, v3, c3, &ierr);
09275         if (ierr != 0) {
09276             goto L13;
09277         }
09278         asum += angle_(c1, c2, c3);
09279     }
09280 
09281 /* Bottom on loop on neighbors of K. */
09282 
09283     if (lp != lpl) {
09284         goto L1;
09285     }
09286 
09287 /* C3 is the last vertex.  Compute its interior angle from */
09288 /*   the sequence (C2,C3,C1S). */
09289 
09290     asum += angle_(c2, c3, c1s);
09291 
09292 /* Compute the interior angle at C1S from */
09293 /*   the sequence (C3,C1S,C2S). */
09294 
09295     asum += angle_(c3, c1s, c2s);
09296 
09297 /* No error encountered. */
09298 
09299     *ier = 0;
09300     ret_val = asum - (double) (m - 2) * acos(-1.);
09301     return ret_val;
09302 
09303 /* Invalid input. */
09304 
09305 L11:
09306     *ier = 1;
09307     areav = 0.f;
09308     return ret_val;
09309 
09310 /* K indexes a boundary node. */
09311 
09312 L12:
09313     *ier = 2;
09314     areav = 0.f;
09315     return ret_val;
09316 
09317 /* Error in CIRCUM. */
09318 
09319 L13:
09320     *ier = 3;
09321     areav = 0.f;
09322     return ret_val;
09323 } /* areav_new__ */
09324 
09325 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09326         list, int *lptr, int *lend, int *lnew)
09327 {
09328     static int k, n1, n2, lp, lsav, nsav, next;
09329     /* Subroutine */ int insert_(int *, int *, int *,
09330             int *, int *);
09331 
09332 
09333 /* *********************************************************** */
09334 
09335 /*                                              From STRIPACK */
09336 /*                                            Robert J. Renka */
09337 /*                                  Dept. of Computer Science */
09338 /*                                       Univ. of North Texas */
09339 /*                                           renka@cs.unt.edu */
09340 /*                                                   07/11/96 */
09341 
09342 /*   This subroutine adds a boundary node to a triangulation */
09343 /* of a set of KK-1 points on the unit sphere.  The data */
09344 /* structure is updated with the insertion of node KK, but no */
09345 /* optimization is performed. */
09346 
09347 /*   This routine is identical to the similarly named routine */
09348 /* in TRIPACK. */
09349 
09350 
09351 /* On input: */
09352 
09353 /*       KK = Index of a node to be connected to the sequence */
09354 /*            of all visible boundary nodes.  KK .GE. 1 and */
09355 /*            KK must not be equal to I1 or I2. */
09356 
09357 /*       I1 = First (rightmost as viewed from KK) boundary */
09358 /*            node in the triangulation that is visible from */
09359 /*            node KK (the line segment KK-I1 intersects no */
09360 /*            arcs. */
09361 
09362 /*       I2 = Last (leftmost) boundary node that is visible */
09363 /*            from node KK.  I1 and I2 may be determined by */
09364 /*            Subroutine TRFIND. */
09365 
09366 /* The above parameters are not altered by this routine. */
09367 
09368 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09369 /*                             created by Subroutine TRMESH. */
09370 /*                             Nodes I1 and I2 must be in- */
09371 /*                             cluded in the triangulation. */
09372 
09373 /* On output: */
09374 
09375 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09376 /*                             the addition of node KK.  Node */
09377 /*                             KK is connected to I1, I2, and */
09378 /*                             all boundary nodes in between. */
09379 
09380 /* Module required by BDYADD:  INSERT */
09381 
09382 /* *********************************************************** */
09383 
09384 
09385 /* Local parameters: */
09386 
09387 /* K =     Local copy of KK */
09388 /* LP =    LIST pointer */
09389 /* LSAV =  LIST pointer */
09390 /* N1,N2 = Local copies of I1 and I2, respectively */
09391 /* NEXT =  Boundary node visible from K */
09392 /* NSAV =  Boundary node visible from K */
09393 
09394     /* Parameter adjustments */
09395     --lend;
09396     --lptr;
09397     --list;
09398 
09399     /* Function Body */
09400     k = *kk;
09401     n1 = *i1;
09402     n2 = *i2;
09403 
09404 /* Add K as the last neighbor of N1. */
09405 
09406     lp = lend[n1];
09407     lsav = lptr[lp];
09408     lptr[lp] = *lnew;
09409     list[*lnew] = -k;
09410     lptr[*lnew] = lsav;
09411     lend[n1] = *lnew;
09412     ++(*lnew);
09413     next = -list[lp];
09414     list[lp] = next;
09415     nsav = next;
09416 
09417 /* Loop on the remaining boundary nodes between N1 and N2, */
09418 /*   adding K as the first neighbor. */
09419 
09420 L1:
09421     lp = lend[next];
09422     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09423     if (next == n2) {
09424         goto L2;
09425     }
09426     next = -list[lp];
09427     list[lp] = next;
09428     goto L1;
09429 
09430 /* Add the boundary nodes between N1 and N2 as neighbors */
09431 /*   of node K. */
09432 
09433 L2:
09434     lsav = *lnew;
09435     list[*lnew] = n1;
09436     lptr[*lnew] = *lnew + 1;
09437     ++(*lnew);
09438     next = nsav;
09439 
09440 L3:
09441     if (next == n2) {
09442         goto L4;
09443     }
09444     list[*lnew] = next;
09445     lptr[*lnew] = *lnew + 1;
09446     ++(*lnew);
09447     lp = lend[next];
09448     next = list[lp];
09449     goto L3;
09450 
09451 L4:
09452     list[*lnew] = -n2;
09453     lptr[*lnew] = lsav;
09454     lend[k] = *lnew;
09455     ++(*lnew);
09456     return 0;
09457 } /* bdyadd_ */
09458 
09459 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09460         int *lend, int *nodes, int *nb, int *na, int *nt)
09461 {
09462     /* System generated locals */
09463     int i__1;
09464 
09465     /* Local variables */
09466     static int k, n0, lp, nn, nst;
09467 
09468 
09469 /* *********************************************************** */
09470 
09471 /*                                              From STRIPACK */
09472 /*                                            Robert J. Renka */
09473 /*                                  Dept. of Computer Science */
09474 /*                                       Univ. of North Texas */
09475 /*                                           renka@cs.unt.edu */
09476 /*                                                   06/26/96 */
09477 
09478 /*   Given a triangulation of N nodes on the unit sphere */
09479 /* created by Subroutine TRMESH, this subroutine returns an */
09480 /* array containing the indexes (if any) of the counterclock- */
09481 /* wise-ordered sequence of boundary nodes -- the nodes on */
09482 /* the boundary of the convex hull of the set of nodes.  (The */
09483 /* boundary is empty if the nodes do not lie in a single */
09484 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09485 /* triangles are also returned. */
09486 
09487 
09488 /* On input: */
09489 
09490 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09491 
09492 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09493 /*                        gulation.  Refer to Subroutine */
09494 /*                        TRMESH. */
09495 
09496 /* The above parameters are not altered by this routine. */
09497 
09498 /*       NODES = int array of length at least NB */
09499 /*               (NB .LE. N). */
09500 
09501 /* On output: */
09502 
09503 /*       NODES = Ordered sequence of boundary node indexes */
09504 /*               in the range 1 to N (in the first NB loca- */
09505 /*               tions). */
09506 
09507 /*       NB = Number of boundary nodes. */
09508 
09509 /*       NA,NT = Number of arcs and triangles, respectively, */
09510 /*               in the triangulation. */
09511 
09512 /* Modules required by BNODES:  None */
09513 
09514 /* *********************************************************** */
09515 
09516 
09517 /* Local parameters: */
09518 
09519 /* K =   NODES index */
09520 /* LP =  LIST pointer */
09521 /* N0 =  Boundary node to be added to NODES */
09522 /* NN =  Local copy of N */
09523 /* NST = First element of nodes (arbitrarily chosen to be */
09524 /*         the one with smallest index) */
09525 
09526     /* Parameter adjustments */
09527     --lend;
09528     --list;
09529     --lptr;
09530     --nodes;
09531 
09532     /* Function Body */
09533     nn = *n;
09534 
09535 /* Search for a boundary node. */
09536 
09537     i__1 = nn;
09538     for (nst = 1; nst <= i__1; ++nst) {
09539         lp = lend[nst];
09540         if (list[lp] < 0) {
09541             goto L2;
09542         }
09543 /* L1: */
09544     }
09545 
09546 /* The triangulation contains no boundary nodes. */
09547 
09548     *nb = 0;
09549     *na = (nn - 2) * 3;
09550     *nt = nn - (2<<1);
09551     return 0;
09552 
09553 /* NST is the first boundary node encountered.  Initialize */
09554 /*   for traversal of the boundary. */
09555 
09556 L2:
09557     nodes[1] = nst;
09558     k = 1;
09559     n0 = nst;
09560 
09561 /* Traverse the boundary in counterclockwise order. */
09562 
09563 L3:
09564     lp = lend[n0];
09565     lp = lptr[lp];
09566     n0 = list[lp];
09567     if (n0 == nst) {
09568         goto L4;
09569     }
09570     ++k;
09571     nodes[k] = n0;
09572     goto L3;
09573 
09574 /* Store the counts. */
09575 
09576 L4:
09577     *nb = k;
09578     *nt = (*n << 1) - *nb - 2;
09579     *na = *nt + *n - 1;
09580     return 0;
09581 } /* bnodes_ */
09582 
09583 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09584         int *ier)
09585 {
09586     /* System generated locals */
09587     int i__1;
09588 
09589     /* Builtin functions */
09590     //double atan(double), cos(double), sin(double);
09591 
09592     /* Local variables */
09593     static double a, c__;
09594     static int i__;
09595     static double s;
09596     static int k2, k3;
09597     static double x0, y0;
09598     static int kk, np1;
09599 
09600 
09601 /* *********************************************************** */
09602 
09603 /*                                              From STRIPACK */
09604 /*                                            Robert J. Renka */
09605 /*                                  Dept. of Computer Science */
09606 /*                                       Univ. of North Texas */
09607 /*                                           renka@cs.unt.edu */
09608 /*                                                   04/06/90 */
09609 
09610 /*   This subroutine computes the coordinates of a sequence */
09611 /* of N equally spaced points on the unit circle centered at */
09612 /* (0,0).  An N-sided polygonal approximation to the circle */
09613 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09614 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09615 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09616 /* is 2*PI*R, where R is the radius of the circle in device */
09617 /* coordinates. */
09618 
09619 
09620 /* On input: */
09621 
09622 /*       K = Number of points in each quadrant, defining N as */
09623 /*           4K.  K .GE. 1. */
09624 
09625 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09626 
09627 /* K is not altered by this routine. */
09628 
09629 /* On output: */
09630 
09631 /*       XC,YC = Cartesian coordinates of the points on the */
09632 /*               unit circle in the first N+1 locations. */
09633 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09634 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09635 /*               and YC(N+1) = YC(1). */
09636 
09637 /*       IER = Error indicator: */
09638 /*             IER = 0 if no errors were encountered. */
09639 /*             IER = 1 if K < 1 on input. */
09640 
09641 /* Modules required by CIRCLE:  None */
09642 
09643 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09644 /*                                          SIN */
09645 
09646 /* *********************************************************** */
09647 
09648 
09649 /* Local parameters: */
09650 
09651 /* I =     DO-loop index and index for XC and YC */
09652 /* KK =    Local copy of K */
09653 /* K2 =    K*2 */
09654 /* K3 =    K*3 */
09655 /* NP1 =   N+1 = 4*K + 1 */
09656 /* A =     Angular separation between adjacent points */
09657 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09658 /*           rotation through angle A */
09659 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09660 /*           circle in the first quadrant */
09661 
09662     /* Parameter adjustments */
09663     --yc;
09664     --xc;
09665 
09666     /* Function Body */
09667     kk = *k;
09668     k2 = kk << 1;
09669     k3 = kk * 3;
09670     np1 = (kk << 2) + 1;
09671 
09672 /* Test for invalid input, compute A, C, and S, and */
09673 /*   initialize (X0,Y0) to (1,0). */
09674 
09675     if (kk < 1) {
09676         goto L2;
09677     }
09678     a = atan(1.) * 2. / (double) kk;
09679     c__ = cos(a);
09680     s = sin(a);
09681     x0 = 1.;
09682     y0 = 0.;
09683 
09684 /* Loop on points (X0,Y0) in the first quadrant, storing */
09685 /*   the point and its reflections about the x axis, the */
09686 /*   y axis, and the line y = -x. */
09687 
09688     i__1 = kk;
09689     for (i__ = 1; i__ <= i__1; ++i__) {
09690         xc[i__] = x0;
09691         yc[i__] = y0;
09692         xc[i__ + kk] = -y0;
09693         yc[i__ + kk] = x0;
09694         xc[i__ + k2] = -x0;
09695         yc[i__ + k2] = -y0;
09696         xc[i__ + k3] = y0;
09697         yc[i__ + k3] = -x0;
09698 
09699 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09700 
09701         x0 = c__ * x0 - s * y0;
09702         y0 = s * x0 + c__ * y0;
09703 /* L1: */
09704     }
09705 
09706 /* Store the coordinates of the first point as the last */
09707 /*   point. */
09708 
09709     xc[np1] = xc[1];
09710     yc[np1] = yc[1];
09711     *ier = 0;
09712     return 0;
09713 
09714 /* K < 1. */
09715 
09716 L2:
09717     *ier = 1;
09718     return 0;
09719 } /* circle_ */
09720 
09721 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09722         double *c__, int *ier)
09723 {
09724     /* Builtin functions */
09725     //double sqrt(double);
09726 
09727     /* Local variables */
09728     static int i__;
09729     static double e1[3], e2[3], cu[3], cnorm;
09730 
09731 
09732 /* *********************************************************** */
09733 
09734 /*                                              From STRIPACK */
09735 /*                                            Robert J. Renka */
09736 /*                                  Dept. of Computer Science */
09737 /*                                       Univ. of North Texas */
09738 /*                                           renka@cs.unt.edu */
09739 /*                                                   10/27/02 */
09740 
09741 /*   This subroutine returns the circumcenter of a spherical */
09742 /* triangle on the unit sphere:  the point on the sphere sur- */
09743 /* face that is equally distant from the three triangle */
09744 /* vertices and lies in the same hemisphere, where distance */
09745 /* is taken to be arc-length on the sphere surface. */
09746 
09747 
09748 /* On input: */
09749 
09750 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09751 /*                  sian coordinates of the three triangle */
09752 /*                  vertices (unit vectors) in CCW order. */
09753 
09754 /* The above parameters are not altered by this routine. */
09755 
09756 /*       C = Array of length 3. */
09757 
09758 /* On output: */
09759 
09760 /*       C = Cartesian coordinates of the circumcenter unless */
09761 /*           IER > 0, in which case C is not defined.  C = */
09762 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09763 
09764 /*       IER = Error indicator: */
09765 /*             IER = 0 if no errors were encountered. */
09766 /*             IER = 1 if V1, V2, and V3 lie on a common */
09767 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09768 /*             (The vertices are not tested for validity.) */
09769 
09770 /* Modules required by CIRCUM:  None */
09771 
09772 /* Intrinsic function called by CIRCUM:  SQRT */
09773 
09774 /* *********************************************************** */
09775 
09776 
09777 /* Local parameters: */
09778 
09779 /* CNORM = Norm of CU:  used to compute C */
09780 /* CU =    Scalar multiple of C:  E1 X E2 */
09781 /* E1,E2 = Edges of the underlying planar triangle: */
09782 /*           V2-V1 and V3-V1, respectively */
09783 /* I =     DO-loop index */
09784 
09785     /* Parameter adjustments */
09786     --c__;
09787     --v3;
09788     --v2;
09789     --v1;
09790 
09791     /* Function Body */
09792     for (i__ = 1; i__ <= 3; ++i__) {
09793         e1[i__ - 1] = v2[i__] - v1[i__];
09794         e2[i__ - 1] = v3[i__] - v1[i__];
09795 /* L1: */
09796     }
09797 
09798 /* Compute CU = E1 X E2 and CNORM**2. */
09799 
09800     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09801     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09802     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09803     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09804 
09805 /* The vertices lie on a common line if and only if CU is */
09806 /*   the zero vector. */
09807 
09808     if (cnorm != 0.) {
09809 
09810 /*   No error:  compute C. */
09811 
09812         cnorm = sqrt(cnorm);
09813         for (i__ = 1; i__ <= 3; ++i__) {
09814             c__[i__] = cu[i__ - 1] / cnorm;
09815 /* L2: */
09816         }
09817 
09818 /* If the vertices are nearly identical, the problem is */
09819 /*   ill-conditioned and it is possible for the computed */
09820 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09821 /*   when it should be positive. */
09822 
09823         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09824             c__[1] = -c__[1];
09825             c__[2] = -c__[2];
09826             c__[3] = -c__[3];
09827         }
09828         *ier = 0;
09829     } else {
09830 
09831 /*   CU = 0. */
09832 
09833         *ier = 1;
09834     }
09835     return 0;
09836 } /* circum_ */
09837 
09838 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09839         *lptr, int *lend, int *lnew)
09840 {
09841     static int k, lp, nst, lsav, next;
09842     /* Subroutine */ int insert_(int *, int *, int *,
09843             int *, int *);
09844 
09845 
09846 /* *********************************************************** */
09847 
09848 /*                                              From STRIPACK */
09849 /*                                            Robert J. Renka */
09850 /*                                  Dept. of Computer Science */
09851 /*                                       Univ. of North Texas */
09852 /*                                           renka@cs.unt.edu */
09853 /*                                                   07/17/96 */
09854 
09855 /*   This subroutine connects an exterior node KK to all */
09856 /* boundary nodes of a triangulation of KK-1 points on the */
09857 /* unit sphere, producing a triangulation that covers the */
09858 /* sphere.  The data structure is updated with the addition */
09859 /* of node KK, but no optimization is performed.  All boun- */
09860 /* dary nodes must be visible from node KK. */
09861 
09862 
09863 /* On input: */
09864 
09865 /*       KK = Index of the node to be connected to the set of */
09866 /*            all boundary nodes.  KK .GE. 4. */
09867 
09868 /*       N0 = Index of a boundary node (in the range 1 to */
09869 /*            KK-1).  N0 may be determined by Subroutine */
09870 /*            TRFIND. */
09871 
09872 /* The above parameters are not altered by this routine. */
09873 
09874 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09875 /*                             created by Subroutine TRMESH. */
09876 /*                             Node N0 must be included in */
09877 /*                             the triangulation. */
09878 
09879 /* On output: */
09880 
09881 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09882 /*                             the addition of node KK as the */
09883 /*                             last entry.  The updated */
09884 /*                             triangulation contains no */
09885 /*                             boundary nodes. */
09886 
09887 /* Module required by COVSPH:  INSERT */
09888 
09889 /* *********************************************************** */
09890 
09891 
09892 /* Local parameters: */
09893 
09894 /* K =     Local copy of KK */
09895 /* LP =    LIST pointer */
09896 /* LSAV =  LIST pointer */
09897 /* NEXT =  Boundary node visible from K */
09898 /* NST =   Local copy of N0 */
09899 
09900     /* Parameter adjustments */
09901     --lend;
09902     --lptr;
09903     --list;
09904 
09905     /* Function Body */
09906     k = *kk;
09907     nst = *n0;
09908 
09909 /* Traverse the boundary in clockwise order, inserting K as */
09910 /*   the first neighbor of each boundary node, and converting */
09911 /*   the boundary node to an interior node. */
09912 
09913     next = nst;
09914 L1:
09915     lp = lend[next];
09916     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09917     next = -list[lp];
09918     list[lp] = next;
09919     if (next != nst) {
09920         goto L1;
09921     }
09922 
09923 /* Traverse the boundary again, adding each node to K's */
09924 /*   adjacency list. */
09925 
09926     lsav = *lnew;
09927 L2:
09928     lp = lend[next];
09929     list[*lnew] = next;
09930     lptr[*lnew] = *lnew + 1;
09931     ++(*lnew);
09932     next = list[lp];
09933     if (next != nst) {
09934         goto L2;
09935     }
09936 
09937     lptr[*lnew - 1] = lsav;
09938     lend[k] = *lnew - 1;
09939     return 0;
09940 } /* covsph_ */
09941 
09942 
09943 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09944         double *y, double *z__, int *list, int *lend, int
09945         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09946         double *xc, double *yc, double *zc, double *rc,
09947         int *ier)
09948 {
09949     /* System generated locals */
09950     int i__1, i__2;
09951 
09952     /* Builtin functions */
09953     //double acos(double);
09954 
09955     /* Local variables */
09956     static double c__[3], t;
09957     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09958     static double v1[3], v2[3], v3[3];
09959     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09960              lpn;
09961     static long int swp;
09962     static int ierr;
09963     int lstptr_(int *, int *, int *, int *);
09964     long int swptst_(int *, int *, int *, int *,
09965             double *, double *, double *);
09966 
09967 
09968 /* *********************************************************** */
09969 
09970 /*                                              From STRIPACK */
09971 /*                                            Robert J. Renka */
09972 /*                                  Dept. of Computer Science */
09973 /*                                       Univ. of North Texas */
09974 /*                                           renka@cs.unt.edu */
09975 /*                                                   03/05/03 */
09976 
09977 /*   Given a Delaunay triangulation of nodes on the surface */
09978 /* of the unit sphere, this subroutine returns the set of */
09979 /* triangle circumcenters corresponding to Voronoi vertices, */
09980 /* along with the circumradii and a list of triangle indexes */
09981 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09982 /* entries. */
09983 
09984 /*   A triangle circumcenter is the point (unit vector) lying */
09985 /* at the same angular distance from the three vertices and */
09986 /* contained in the same hemisphere as the vertices.  (Note */
09987 /* that the negative of a circumcenter is also equidistant */
09988 /* from the vertices.)  If the triangulation covers the sur- */
09989 /* face, the Voronoi vertices are the circumcenters of the */
09990 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09991 /* LNEW are not altered in this case. */
09992 
09993 /*   On the other hand, if the nodes are contained in a sin- */
09994 /* gle hemisphere, the triangulation is implicitly extended */
09995 /* to the entire surface by adding pseudo-arcs (of length */
09996 /* greater than 180 degrees) between boundary nodes forming */
09997 /* pseudo-triangles whose 'circumcenters' are included in the */
09998 /* list.  This extension to the triangulation actually con- */
09999 /* sists of a triangulation of the set of boundary nodes in */
10000 /* which the swap test is reversed (a non-empty circumcircle */
10001 /* test).  The negative circumcenters are stored as the */
10002 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
10003 /* LNEW contain a data structure corresponding to the ex- */
10004 /* tended triangulation (Voronoi diagram), but LIST is not */
10005 /* altered in this case.  Thus, if it is necessary to retain */
10006 /* the original (unextended) triangulation data structure, */
10007 /* copies of LPTR and LNEW must be saved before calling this */
10008 /* routine. */
10009 
10010 
10011 /* On input: */
10012 
10013 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10014 /*           Note that, if N = 3, there are only two Voronoi */
10015 /*           vertices separated by 180 degrees, and the */
10016 /*           Voronoi regions are not well defined. */
10017 
10018 /*       NCOL = Number of columns reserved for LTRI.  This */
10019 /*              must be at least NB-2, where NB is the number */
10020 /*              of boundary nodes. */
10021 
10022 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10023 /*               coordinates of the nodes (unit vectors). */
10024 
10025 /*       LIST = int array containing the set of adjacency */
10026 /*              lists.  Refer to Subroutine TRMESH. */
10027 
10028 /*       LEND = Set of pointers to ends of adjacency lists. */
10029 /*              Refer to Subroutine TRMESH. */
10030 
10031 /* The above parameters are not altered by this routine. */
10032 
10033 /*       LPTR = Array of pointers associated with LIST.  Re- */
10034 /*              fer to Subroutine TRMESH. */
10035 
10036 /*       LNEW = Pointer to the first empty location in LIST */
10037 /*              and LPTR (list length plus one). */
10038 
10039 /*       LTRI = int work space array dimensioned 6 by */
10040 /*              NCOL, or unused dummy parameter if NB = 0. */
10041 
10042 /*       LISTC = int array of length at least 3*NT, where */
10043 /*               NT = 2*N-4 is the number of triangles in the */
10044 /*               triangulation (after extending it to cover */
10045 /*               the entire surface if necessary). */
10046 
10047 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
10048 
10049 /* On output: */
10050 
10051 /*       LPTR = Array of pointers associated with LISTC: */
10052 /*              updated for the addition of pseudo-triangles */
10053 /*              if the original triangulation contains */
10054 /*              boundary nodes (NB > 0). */
10055 
10056 /*       LNEW = Pointer to the first empty location in LISTC */
10057 /*              and LPTR (list length plus one).  LNEW is not */
10058 /*              altered if NB = 0. */
10059 
10060 /*       LTRI = Triangle list whose first NB-2 columns con- */
10061 /*              tain the indexes of a clockwise-ordered */
10062 /*              sequence of vertices (first three rows) */
10063 /*              followed by the LTRI column indexes of the */
10064 /*              triangles opposite the vertices (or 0 */
10065 /*              denoting the exterior region) in the last */
10066 /*              three rows.  This array is not generally of */
10067 /*              any use. */
10068 
10069 /*       LISTC = Array containing triangle indexes (indexes */
10070 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
10071 /*               pondence with LIST/LPTR entries (or entries */
10072 /*               that would be stored in LIST for the */
10073 /*               extended triangulation):  the index of tri- */
10074 /*               angle (N1,N2,N3) is stored in LISTC(K), */
10075 /*               LISTC(L), and LISTC(M), where LIST(K), */
10076 /*               LIST(L), and LIST(M) are the indexes of N2 */
10077 /*               as a neighbor of N1, N3 as a neighbor of N2, */
10078 /*               and N1 as a neighbor of N3.  The Voronoi */
10079 /*               region associated with a node is defined by */
10080 /*               the CCW-ordered sequence of circumcenters in */
10081 /*               one-to-one correspondence with its adjacency */
10082 /*               list (in the extended triangulation). */
10083 
10084 /*       NB = Number of boundary nodes unless IER = 1. */
10085 
10086 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
10087 /*                  nates of the triangle circumcenters */
10088 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
10089 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
10090 /*                  correspond to pseudo-triangles if NB > 0. */
10091 
10092 /*       RC = Array containing circumradii (the arc lengths */
10093 /*            or angles between the circumcenters and associ- */
10094 /*            ated triangle vertices) in 1-1 correspondence */
10095 /*            with circumcenters. */
10096 
10097 /*       IER = Error indicator: */
10098 /*             IER = 0 if no errors were encountered. */
10099 /*             IER = 1 if N < 3. */
10100 /*             IER = 2 if NCOL < NB-2. */
10101 /*             IER = 3 if a triangle is degenerate (has ver- */
10102 /*                     tices lying on a common geodesic). */
10103 
10104 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
10105 
10106 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
10107 
10108 /* *********************************************************** */
10109 
10110 
10111 /* Local parameters: */
10112 
10113 /* C =         Circumcenter returned by Subroutine CIRCUM */
10114 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
10115 /* I4 =        LTRI row index in the range 1 to 3 */
10116 /* IERR =      Error flag for calls to CIRCUM */
10117 /* KT =        Triangle index */
10118 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
10119 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
10120 /*               and N2 as vertices of KT1 */
10121 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
10122 /*               and N2 as vertices of KT2 */
10123 /* LP,LPN =    LIST pointers */
10124 /* LPL =       LIST pointer of the last neighbor of N1 */
10125 /* N0 =        Index of the first boundary node (initial */
10126 /*               value of N1) in the loop on boundary nodes */
10127 /*               used to store the pseudo-triangle indexes */
10128 /*               in LISTC */
10129 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
10130 /*               or pseudo-triangle (clockwise order) */
10131 /* N4 =        Index of the node opposite N2 -> N1 */
10132 /* NM2 =       N-2 */
10133 /* NN =        Local copy of N */
10134 /* NT =        Number of pseudo-triangles:  NB-2 */
10135 /* SWP =       long int variable set to TRUE in each optimiza- */
10136 /*               tion loop (loop on pseudo-arcs) iff a swap */
10137 /*               is performed */
10138 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
10139 /*               Subroutine CIRCUM */
10140 
10141     /* Parameter adjustments */
10142     --lend;
10143     --z__;
10144     --y;
10145     --x;
10146     ltri -= 7;
10147     --list;
10148     --lptr;
10149     --listc;
10150     --xc;
10151     --yc;
10152     --zc;
10153     --rc;
10154 
10155     /* Function Body */
10156     nn = *n;
10157     *nb = 0;
10158     nt = 0;
10159     if (nn < 3) {
10160         goto L21;
10161     }
10162 
10163 /* Search for a boundary node N1. */
10164 
10165     i__1 = nn;
10166     for (n1 = 1; n1 <= i__1; ++n1) {
10167         lp = lend[n1];
10168         if (list[lp] < 0) {
10169             goto L2;
10170         }
10171 /* L1: */
10172     }
10173 
10174 /* The triangulation already covers the sphere. */
10175 
10176     goto L9;
10177 
10178 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
10179 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
10180 /*   boundary nodes to which it is not already adjacent. */
10181 
10182 /*   Set N3 and N2 to the first and last neighbors, */
10183 /*     respectively, of N1. */
10184 
10185 L2:
10186     n2 = -list[lp];
10187     lp = lptr[lp];
10188     n3 = list[lp];
10189 
10190 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
10191 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
10192 /*     along with the indexes of the triangles opposite */
10193 /*     the vertices. */
10194 
10195 L3:
10196     ++nt;
10197     if (nt <= *ncol) {
10198         ltri[nt * 6 + 1] = n1;
10199         ltri[nt * 6 + 2] = n2;
10200         ltri[nt * 6 + 3] = n3;
10201         ltri[nt * 6 + 4] = nt + 1;
10202         ltri[nt * 6 + 5] = nt - 1;
10203         ltri[nt * 6 + 6] = 0;
10204     }
10205     n1 = n2;
10206     lp = lend[n1];
10207     n2 = -list[lp];
10208     if (n2 != n3) {
10209         goto L3;
10210     }
10211 
10212     *nb = nt + 2;
10213     if (*ncol < nt) {
10214         goto L22;
10215     }
10216     ltri[nt * 6 + 4] = 0;
10217     if (nt == 1) {
10218         goto L7;
10219     }
10220 
10221 /* Optimize the exterior triangulation (set of pseudo- */
10222 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
10223 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
10224 /*   The loop on pseudo-arcs is repeated until no swaps are */
10225 /*   performed. */
10226 
10227 L4:
10228     swp = FALSE_;
10229     i__1 = nt - 1;
10230     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10231         for (i3 = 1; i3 <= 3; ++i3) {
10232             kt2 = ltri[i3 + 3 + kt1 * 6];
10233             if (kt2 <= kt1) {
10234                 goto L5;
10235             }
10236 
10237 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10238 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10239 
10240             if (i3 == 1) {
10241                 i1 = 2;
10242                 i2 = 3;
10243             } else if (i3 == 2) {
10244                 i1 = 3;
10245                 i2 = 1;
10246             } else {
10247                 i1 = 1;
10248                 i2 = 2;
10249             }
10250             n1 = ltri[i1 + kt1 * 6];
10251             n2 = ltri[i2 + kt1 * 6];
10252             n3 = ltri[i3 + kt1 * 6];
10253 
10254 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10255 /*     LTRI(I+3,KT2) = KT1. */
10256 
10257             if (ltri[kt2 * 6 + 4] == kt1) {
10258                 i4 = 1;
10259             } else if (ltri[kt2 * 6 + 5] == kt1) {
10260                 i4 = 2;
10261             } else {
10262                 i4 = 3;
10263             }
10264             n4 = ltri[i4 + kt2 * 6];
10265 
10266 /*   The empty circumcircle test is reversed for the pseudo- */
10267 /*     triangles.  The reversal is implicit in the clockwise */
10268 /*     ordering of the vertices. */
10269 
10270             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10271                 goto L5;
10272             }
10273 
10274 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10275 /*     Nj as a vertex of KTi. */
10276 
10277             swp = TRUE_;
10278             kt11 = ltri[i1 + 3 + kt1 * 6];
10279             kt12 = ltri[i2 + 3 + kt1 * 6];
10280             if (i4 == 1) {
10281                 i2 = 2;
10282                 i1 = 3;
10283             } else if (i4 == 2) {
10284                 i2 = 3;
10285                 i1 = 1;
10286             } else {
10287                 i2 = 1;
10288                 i1 = 2;
10289             }
10290             kt21 = ltri[i1 + 3 + kt2 * 6];
10291             kt22 = ltri[i2 + 3 + kt2 * 6];
10292             ltri[kt1 * 6 + 1] = n4;
10293             ltri[kt1 * 6 + 2] = n3;
10294             ltri[kt1 * 6 + 3] = n1;
10295             ltri[kt1 * 6 + 4] = kt12;
10296             ltri[kt1 * 6 + 5] = kt22;
10297             ltri[kt1 * 6 + 6] = kt2;
10298             ltri[kt2 * 6 + 1] = n3;
10299             ltri[kt2 * 6 + 2] = n4;
10300             ltri[kt2 * 6 + 3] = n2;
10301             ltri[kt2 * 6 + 4] = kt21;
10302             ltri[kt2 * 6 + 5] = kt11;
10303             ltri[kt2 * 6 + 6] = kt1;
10304 
10305 /*   Correct the KT11 and KT22 entries that changed. */
10306 
10307             if (kt11 != 0) {
10308                 i4 = 4;
10309                 if (ltri[kt11 * 6 + 4] != kt1) {
10310                     i4 = 5;
10311                     if (ltri[kt11 * 6 + 5] != kt1) {
10312                         i4 = 6;
10313                     }
10314                 }
10315                 ltri[i4 + kt11 * 6] = kt2;
10316             }
10317             if (kt22 != 0) {
10318                 i4 = 4;
10319                 if (ltri[kt22 * 6 + 4] != kt2) {
10320                     i4 = 5;
10321                     if (ltri[kt22 * 6 + 5] != kt2) {
10322                         i4 = 6;
10323                     }
10324                 }
10325                 ltri[i4 + kt22 * 6] = kt1;
10326             }
10327 L5:
10328             ;
10329         }
10330 /* L6: */
10331     }
10332     if (swp) {
10333         goto L4;
10334     }
10335 
10336 /* Compute and store the negative circumcenters and radii of */
10337 /*   the pseudo-triangles in the first NT positions. */
10338 
10339 L7:
10340     i__1 = nt;
10341     for (kt = 1; kt <= i__1; ++kt) {
10342         n1 = ltri[kt * 6 + 1];
10343         n2 = ltri[kt * 6 + 2];
10344         n3 = ltri[kt * 6 + 3];
10345         v1[0] = x[n1];
10346         v1[1] = y[n1];
10347         v1[2] = z__[n1];
10348         v2[0] = x[n2];
10349         v2[1] = y[n2];
10350         v2[2] = z__[n2];
10351         v3[0] = x[n3];
10352         v3[1] = y[n3];
10353         v3[2] = z__[n3];
10354         circum_(v2, v1, v3, c__, &ierr);
10355         if (ierr != 0) {
10356             goto L23;
10357         }
10358 
10359 /*   Store the negative circumcenter and radius (computed */
10360 /*     from <V1,C>). */
10361 
10362         xc[kt] = -c__[0];
10363         yc[kt] = -c__[1];
10364         zc[kt] = -c__[2];
10365         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10366         if (t < -1.) {
10367             t = -1.;
10368         }
10369         if (t > 1.) {
10370             t = 1.;
10371         }
10372         rc[kt] = acos(t);
10373 /* L8: */
10374     }
10375 
10376 /* Compute and store the circumcenters and radii of the */
10377 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10378 /*   Also, store the triangle indexes KT in the appropriate */
10379 /*   LISTC positions. */
10380 
10381 L9:
10382     kt = nt;
10383 
10384 /*   Loop on nodes N1. */
10385 
10386     nm2 = nn - 2;
10387     i__1 = nm2;
10388     for (n1 = 1; n1 <= i__1; ++n1) {
10389         lpl = lend[n1];
10390         lp = lpl;
10391         n3 = list[lp];
10392 
10393 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10394 /*     and N3 > N1. */
10395 
10396 L10:
10397         lp = lptr[lp];
10398         n2 = n3;
10399         n3 = (i__2 = list[lp], abs(i__2));
10400         if (n2 <= n1 || n3 <= n1) {
10401             goto L11;
10402         }
10403         ++kt;
10404 
10405 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10406 
10407         v1[0] = x[n1];
10408         v1[1] = y[n1];
10409         v1[2] = z__[n1];
10410         v2[0] = x[n2];
10411         v2[1] = y[n2];
10412         v2[2] = z__[n2];
10413         v3[0] = x[n3];
10414         v3[1] = y[n3];
10415         v3[2] = z__[n3];
10416         circum_(v1, v2, v3, c__, &ierr);
10417         if (ierr != 0) {
10418             goto L23;
10419         }
10420 
10421 /*   Store the circumcenter, radius and triangle index. */
10422 
10423         xc[kt] = c__[0];
10424         yc[kt] = c__[1];
10425         zc[kt] = c__[2];
10426         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10427         if (t < -1.) {
10428             t = -1.;
10429         }
10430         if (t > 1.) {
10431             t = 1.;
10432         }
10433         rc[kt] = acos(t);
10434 
10435 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10436 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10437 /*     of N2, and N1 as a neighbor of N3. */
10438 
10439         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10440         listc[lpn] = kt;
10441         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10442         listc[lpn] = kt;
10443         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10444         listc[lpn] = kt;
10445 L11:
10446         if (lp != lpl) {
10447             goto L10;
10448         }
10449 /* L12: */
10450     }
10451     if (nt == 0) {
10452         goto L20;
10453     }
10454 
10455 /* Store the first NT triangle indexes in LISTC. */
10456 
10457 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10458 /*     boundary arc opposite N3. */
10459 
10460     kt1 = 0;
10461 L13:
10462     ++kt1;
10463     if (ltri[kt1 * 6 + 4] == 0) {
10464         i1 = 2;
10465         i2 = 3;
10466         i3 = 1;
10467         goto L14;
10468     } else if (ltri[kt1 * 6 + 5] == 0) {
10469         i1 = 3;
10470         i2 = 1;
10471         i3 = 2;
10472         goto L14;
10473     } else if (ltri[kt1 * 6 + 6] == 0) {
10474         i1 = 1;
10475         i2 = 2;
10476         i3 = 3;
10477         goto L14;
10478     }
10479     goto L13;
10480 L14:
10481     n1 = ltri[i1 + kt1 * 6];
10482     n0 = n1;
10483 
10484 /*   Loop on boundary nodes N1 in CCW order, storing the */
10485 /*     indexes of the clockwise-ordered sequence of triangles */
10486 /*     that contain N1.  The first triangle overwrites the */
10487 /*     last neighbor position, and the remaining triangles, */
10488 /*     if any, are appended to N1's adjacency list. */
10489 
10490 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10491 
10492 L15:
10493     lp = lend[n1];
10494     lpn = lptr[lp];
10495     listc[lp] = kt1;
10496 
10497 /*   Loop on triangles KT2 containing N1. */
10498 
10499 L16:
10500     kt2 = ltri[i2 + 3 + kt1 * 6];
10501     if (kt2 != 0) {
10502 
10503 /*   Append KT2 to N1's triangle list. */
10504 
10505         lptr[lp] = *lnew;
10506         lp = *lnew;
10507         listc[lp] = kt2;
10508         ++(*lnew);
10509 
10510 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10511 /*     LTRI(I1,KT1) = N1. */
10512 
10513         kt1 = kt2;
10514         if (ltri[kt1 * 6 + 1] == n1) {
10515             i1 = 1;
10516             i2 = 2;
10517             i3 = 3;
10518         } else if (ltri[kt1 * 6 + 2] == n1) {
10519             i1 = 2;
10520             i2 = 3;
10521             i3 = 1;
10522         } else {
10523             i1 = 3;
10524             i2 = 1;
10525             i3 = 2;
10526         }
10527         goto L16;
10528     }
10529 
10530 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10531 /*     N1 to the next boundary node, test for termination, */
10532 /*     and permute the indexes:  the last triangle containing */
10533 /*     a boundary node is the first triangle containing the */
10534 /*     next boundary node. */
10535 
10536     lptr[lp] = lpn;
10537     n1 = ltri[i3 + kt1 * 6];
10538     if (n1 != n0) {
10539         i4 = i3;
10540         i3 = i2;
10541         i2 = i1;
10542         i1 = i4;
10543         goto L15;
10544     }
10545 
10546 /* No errors encountered. */
10547 
10548 L20:
10549     *ier = 0;
10550     return 0;
10551 
10552 /* N < 3. */
10553 
10554 L21:
10555     *ier = 1;
10556     return 0;
10557 
10558 /* Insufficient space reserved for LTRI. */
10559 
10560 L22:
10561     *ier = 2;
10562     return 0;
10563 
10564 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10565 
10566 L23:
10567     *ier = 3;
10568     return 0;
10569 } /* crlist_ */
10570 
10571 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10572         list, int *lptr, int *lend, int *lnew, int *ier)
10573 {
10574     /* System generated locals */
10575     int i__1;
10576 
10577     /* Local variables */
10578     static int n1, n2, n3, lp, lph, lpl;
10579     /* Subroutine */ int delnb_(int *, int *, int *,
10580             int *, int *, int *, int *, int *);
10581     int lstptr_(int *, int *, int *, int *);
10582 
10583 
10584 /* *********************************************************** */
10585 
10586 /*                                              From STRIPACK */
10587 /*                                            Robert J. Renka */
10588 /*                                  Dept. of Computer Science */
10589 /*                                       Univ. of North Texas */
10590 /*                                           renka@cs.unt.edu */
10591 /*                                                   07/17/96 */
10592 
10593 /*   This subroutine deletes a boundary arc from a triangula- */
10594 /* tion.  It may be used to remove a null triangle from the */
10595 /* convex hull boundary.  Note, however, that if the union of */
10596 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10597 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10598 /* NEARND should not be called following an arc deletion. */
10599 
10600 /*   This routine is identical to the similarly named routine */
10601 /* in TRIPACK. */
10602 
10603 
10604 /* On input: */
10605 
10606 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10607 
10608 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10609 /*                 adjacent boundary nodes defining the arc */
10610 /*                 to be removed. */
10611 
10612 /* The above parameters are not altered by this routine. */
10613 
10614 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10615 /*                             created by Subroutine TRMESH. */
10616 
10617 /* On output: */
10618 
10619 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10620 /*                             the removal of arc IO1-IO2 */
10621 /*                             unless IER > 0. */
10622 
10623 /*       IER = Error indicator: */
10624 /*             IER = 0 if no errors were encountered. */
10625 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10626 /*                     range, or IO1 = IO2. */
10627 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10628 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10629 /*                     ready a boundary node, and thus IO1 */
10630 /*                     or IO2 has only two neighbors or a */
10631 /*                     deletion would result in two triangu- */
10632 /*                     lations sharing a single node. */
10633 /*             IER = 4 if one of the nodes is a neighbor of */
10634 /*                     the other, but not vice versa, imply- */
10635 /*                     ing an invalid triangulation data */
10636 /*                     structure. */
10637 
10638 /* Module required by DELARC:  DELNB, LSTPTR */
10639 
10640 /* Intrinsic function called by DELARC:  ABS */
10641 
10642 /* *********************************************************** */
10643 
10644 
10645 /* Local parameters: */
10646 
10647 /* LP =       LIST pointer */
10648 /* LPH =      LIST pointer or flag returned by DELNB */
10649 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10650 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10651 /*              is the directed boundary edge associated */
10652 /*              with IO1-IO2 */
10653 
10654     /* Parameter adjustments */
10655     --lend;
10656     --list;
10657     --lptr;
10658 
10659     /* Function Body */
10660     n1 = *io1;
10661     n2 = *io2;
10662 
10663 /* Test for errors, and set N1->N2 to the directed boundary */
10664 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10665 /*   for some N3. */
10666 
10667     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10668         *ier = 1;
10669         return 0;
10670     }
10671 
10672     lpl = lend[n2];
10673     if (-list[lpl] != n1) {
10674         n1 = n2;
10675         n2 = *io1;
10676         lpl = lend[n2];
10677         if (-list[lpl] != n1) {
10678             *ier = 2;
10679             return 0;
10680         }
10681     }
10682 
10683 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10684 /*   of N1), and test for error 3 (N3 already a boundary */
10685 /*   node). */
10686 
10687     lpl = lend[n1];
10688     lp = lptr[lpl];
10689     lp = lptr[lp];
10690     n3 = (i__1 = list[lp], abs(i__1));
10691     lpl = lend[n3];
10692     if (list[lpl] <= 0) {
10693         *ier = 3;
10694         return 0;
10695     }
10696 
10697 /* Delete N2 as a neighbor of N1, making N3 the first */
10698 /*   neighbor, and test for error 4 (N2 not a neighbor */
10699 /*   of N1).  Note that previously computed pointers may */
10700 /*   no longer be valid following the call to DELNB. */
10701 
10702     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10703     if (lph < 0) {
10704         *ier = 4;
10705         return 0;
10706     }
10707 
10708 /* Delete N1 as a neighbor of N2, making N3 the new last */
10709 /*   neighbor. */
10710 
10711     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10712 
10713 /* Make N3 a boundary node with first neighbor N2 and last */
10714 /*   neighbor N1. */
10715 
10716     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10717     lend[n3] = lp;
10718     list[lp] = -n1;
10719 
10720 /* No errors encountered. */
10721 
10722     *ier = 0;
10723     return 0;
10724 } /* delarc_ */
10725 
10726 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10727         list, int *lptr, int *lend, int *lnew, int *lph)
10728 {
10729     /* System generated locals */
10730     int i__1;
10731 
10732     /* Local variables */
10733     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10734 
10735 
10736 /* *********************************************************** */
10737 
10738 /*                                              From STRIPACK */
10739 /*                                            Robert J. Renka */
10740 /*                                  Dept. of Computer Science */
10741 /*                                       Univ. of North Texas */
10742 /*                                           renka@cs.unt.edu */
10743 /*                                                   07/29/98 */
10744 
10745 /*   This subroutine deletes a neighbor NB from the adjacency */
10746 /* list of node N0 (but N0 is not deleted from the adjacency */
10747 /* list of NB) and, if NB is a boundary node, makes N0 a */
10748 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10749 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10750 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10751 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10752 /* ted.  This requires a search of LEND and LPTR entailing an */
10753 /* expected operation count of O(N). */
10754 
10755 /*   This routine is identical to the similarly named routine */
10756 /* in TRIPACK. */
10757 
10758 
10759 /* On input: */
10760 
10761 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10762 /*               nodes such that NB is a neighbor of N0. */
10763 /*               (N0 need not be a neighbor of NB.) */
10764 
10765 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10766 
10767 /* The above parameters are not altered by this routine. */
10768 
10769 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10770 /*                             triangulation. */
10771 
10772 /* On output: */
10773 
10774 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10775 /*                             the removal of NB from the ad- */
10776 /*                             jacency list of N0 unless */
10777 /*                             LPH < 0. */
10778 
10779 /*       LPH = List pointer to the hole (NB as a neighbor of */
10780 /*             N0) filled in by the values at LNEW-1 or error */
10781 /*             indicator: */
10782 /*             LPH > 0 if no errors were encountered. */
10783 /*             LPH = -1 if N0, NB, or N is outside its valid */
10784 /*                      range. */
10785 /*             LPH = -2 if NB is not a neighbor of N0. */
10786 
10787 /* Modules required by DELNB:  None */
10788 
10789 /* Intrinsic function called by DELNB:  ABS */
10790 
10791 /* *********************************************************** */
10792 
10793 
10794 /* Local parameters: */
10795 
10796 /* I =   DO-loop index */
10797 /* LNW = LNEW-1 (output value of LNEW) */
10798 /* LP =  LIST pointer of the last neighbor of NB */
10799 /* LPB = Pointer to NB as a neighbor of N0 */
10800 /* LPL = Pointer to the last neighbor of N0 */
10801 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10802 /* NN =  Local copy of N */
10803 
10804     /* Parameter adjustments */
10805     --lend;
10806     --list;
10807     --lptr;
10808 
10809     /* Function Body */
10810     nn = *n;
10811 
10812 /* Test for error 1. */
10813 
10814     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10815         *lph = -1;
10816         return 0;
10817     }
10818 
10819 /*   Find pointers to neighbors of N0: */
10820 
10821 /*     LPL points to the last neighbor, */
10822 /*     LPP points to the neighbor NP preceding NB, and */
10823 /*     LPB points to NB. */
10824 
10825     lpl = lend[*n0];
10826     lpp = lpl;
10827     lpb = lptr[lpp];
10828 L1:
10829     if (list[lpb] == *nb) {
10830         goto L2;
10831     }
10832     lpp = lpb;
10833     lpb = lptr[lpp];
10834     if (lpb != lpl) {
10835         goto L1;
10836     }
10837 
10838 /*   Test for error 2 (NB not found). */
10839 
10840     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10841         *lph = -2;
10842         return 0;
10843     }
10844 
10845 /*   NB is the last neighbor of N0.  Make NP the new last */
10846 /*     neighbor and, if NB is a boundary node, then make N0 */
10847 /*     a boundary node. */
10848 
10849     lend[*n0] = lpp;
10850     lp = lend[*nb];
10851     if (list[lp] < 0) {
10852         list[lpp] = -list[lpp];
10853     }
10854     goto L3;
10855 
10856 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10857 /*     node and N0 is not, then make N0 a boundary node with */
10858 /*     last neighbor NP. */
10859 
10860 L2:
10861     lp = lend[*nb];
10862     if (list[lp] < 0 && list[lpl] > 0) {
10863         lend[*n0] = lpp;
10864         list[lpp] = -list[lpp];
10865     }
10866 
10867 /*   Update LPTR so that the neighbor following NB now fol- */
10868 /*     lows NP, and fill in the hole at location LPB. */
10869 
10870 L3:
10871     lptr[lpp] = lptr[lpb];
10872     lnw = *lnew - 1;
10873     list[lpb] = list[lnw];
10874     lptr[lpb] = lptr[lnw];
10875     for (i__ = nn; i__ >= 1; --i__) {
10876         if (lend[i__] == lnw) {
10877             lend[i__] = lpb;
10878             goto L5;
10879         }
10880 /* L4: */
10881     }
10882 
10883 L5:
10884     i__1 = lnw - 1;
10885     for (i__ = 1; i__ <= i__1; ++i__) {
10886         if (lptr[i__] == lnw) {
10887             lptr[i__] = lpb;
10888         }
10889 /* L6: */
10890     }
10891 
10892 /* No errors encountered. */
10893 
10894     *lnew = lnw;
10895     *lph = lpb;
10896     return 0;
10897 } /* delnb_ */
10898 
10899 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10900         double *y, double *z__, int *list, int *lptr, int
10901         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10902 {
10903     /* System generated locals */
10904     int i__1;
10905 
10906     /* Local variables */
10907     static int i__, j, n1, n2;
10908     static double x1, x2, y1, y2, z1, z2;
10909     static int nl, lp, nn, nr;
10910     static double xl, yl, zl, xr, yr, zr;
10911     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10912     static long int bdry;
10913     static int ierr, lwkl;
10914     /* Subroutine */ int swap_(int *, int *, int *,
10915             int *, int *, int *, int *, int *), delnb_(
10916             int *, int *, int *, int *, int *, int *,
10917             int *, int *);
10918     int nbcnt_(int *, int *);
10919     /* Subroutine */ int optim_(double *, double *, double
10920             *, int *, int *, int *, int *, int *, int
10921             *, int *);
10922     static int nfrst;
10923     int lstptr_(int *, int *, int *, int *);
10924 
10925 
10926 /* *********************************************************** */
10927 
10928 /*                                              From STRIPACK */
10929 /*                                            Robert J. Renka */
10930 /*                                  Dept. of Computer Science */
10931 /*                                       Univ. of North Texas */
10932 /*                                           renka@cs.unt.edu */
10933 /*                                                   11/30/99 */
10934 
10935 /*   This subroutine deletes node K (along with all arcs */
10936 /* incident on node K) from a triangulation of N nodes on the */
10937 /* unit sphere, and inserts arcs as necessary to produce a */
10938 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10939 /* triangulation is input, a Delaunay triangulation will */
10940 /* result, and thus, DELNOD reverses the effect of a call to */
10941 /* Subroutine ADDNOD. */
10942 
10943 
10944 /* On input: */
10945 
10946 /*       K = Index (for X, Y, and Z) of the node to be */
10947 /*           deleted.  1 .LE. K .LE. N. */
10948 
10949 /* K is not altered by this routine. */
10950 
10951 /*       N = Number of nodes in the triangulation on input. */
10952 /*           N .GE. 4.  Note that N will be decremented */
10953 /*           following the deletion. */
10954 
10955 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10956 /*               coordinates of the nodes in the triangula- */
10957 /*               tion. */
10958 
10959 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10960 /*                             triangulation.  Refer to Sub- */
10961 /*                             routine TRMESH. */
10962 
10963 /*       LWK = Number of columns reserved for IWK.  LWK must */
10964 /*             be at least NNB-3, where NNB is the number of */
10965 /*             neighbors of node K, including an extra */
10966 /*             pseudo-node if K is a boundary node. */
10967 
10968 /*       IWK = int work array dimensioned 2 by LWK (or */
10969 /*             array of length .GE. 2*LWK). */
10970 
10971 /* On output: */
10972 
10973 /*       N = Number of nodes in the triangulation on output. */
10974 /*           The input value is decremented unless 1 .LE. IER */
10975 /*           .LE. 4. */
10976 
10977 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10978 /*               (with elements K+1,...,N+1 shifted up one */
10979 /*               position, thus overwriting element K) unless */
10980 /*               1 .LE. IER .LE. 4. */
10981 
10982 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10983 /*                             structure reflecting the dele- */
10984 /*                             tion unless 1 .LE. IER .LE. 4. */
10985 /*                             Note that the data structure */
10986 /*                             may have been altered if IER > */
10987 /*                             3. */
10988 
10989 /*       LWK = Number of IWK columns required unless IER = 1 */
10990 /*             or IER = 3. */
10991 
10992 /*       IWK = Indexes of the endpoints of the new arcs added */
10993 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10994 /*             are associated with columns, or pairs of */
10995 /*             adjacent elements if IWK is declared as a */
10996 /*             singly-subscripted array.) */
10997 
10998 /*       IER = Error indicator: */
10999 /*             IER = 0 if no errors were encountered. */
11000 /*             IER = 1 if K or N is outside its valid range */
11001 /*                     or LWK < 0 on input. */
11002 /*             IER = 2 if more space is required in IWK. */
11003 /*                     Refer to LWK. */
11004 /*             IER = 3 if the triangulation data structure is */
11005 /*                     invalid on input. */
11006 /*             IER = 4 if K indexes an interior node with */
11007 /*                     four or more neighbors, none of which */
11008 /*                     can be swapped out due to collineari- */
11009 /*                     ty, and K cannot therefore be deleted. */
11010 /*             IER = 5 if an error flag (other than IER = 1) */
11011 /*                     was returned by OPTIM.  An error */
11012 /*                     message is written to the standard */
11013 /*                     output unit in this case. */
11014 /*             IER = 6 if error flag 1 was returned by OPTIM. */
11015 /*                     This is not necessarily an error, but */
11016 /*                     the arcs may not be optimal. */
11017 
11018 /*   Note that the deletion may result in all remaining nodes */
11019 /* being collinear.  This situation is not flagged. */
11020 
11021 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
11022 /*                                OPTIM, SWAP, SWPTST */
11023 
11024 /* Intrinsic function called by DELNOD:  ABS */
11025 
11026 /* *********************************************************** */
11027 
11028 
11029 /* Local parameters: */
11030 
11031 /* BDRY =    long int variable with value TRUE iff N1 is a */
11032 /*             boundary node */
11033 /* I,J =     DO-loop indexes */
11034 /* IERR =    Error flag returned by OPTIM */
11035 /* IWL =     Number of IWK columns containing arcs */
11036 /* LNW =     Local copy of LNEW */
11037 /* LP =      LIST pointer */
11038 /* LP21 =    LIST pointer returned by SWAP */
11039 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
11040 /* LPH =     Pointer (or flag) returned by DELNB */
11041 /* LPL2 =    Pointer to the last neighbor of N2 */
11042 /* LPN =     Pointer to a neighbor of N1 */
11043 /* LWKL =    Input value of LWK */
11044 /* N1 =      Local copy of K */
11045 /* N2 =      Neighbor of N1 */
11046 /* NFRST =   First neighbor of N1:  LIST(LPF) */
11047 /* NIT =     Number of iterations in OPTIM */
11048 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
11049 /*             following (to the left of) N2, respectively */
11050 /* NN =      Number of nodes in the triangulation */
11051 /* NNB =     Number of neighbors of N1 (including a pseudo- */
11052 /*             node representing the boundary if N1 is a */
11053 /*             boundary node) */
11054 /* X1,Y1,Z1 = Coordinates of N1 */
11055 /* X2,Y2,Z2 = Coordinates of N2 */
11056 /* XL,YL,ZL = Coordinates of NL */
11057 /* XR,YR,ZR = Coordinates of NR */
11058 
11059 
11060 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
11061 /*   one if N1 is a boundary node), and test for errors.  LPF */
11062 /*   and LPL are LIST indexes of the first and last neighbors */
11063 /*   of N1, IWL is the number of IWK columns containing arcs, */
11064 /*   and BDRY is TRUE iff N1 is a boundary node. */
11065 
11066     /* Parameter adjustments */
11067     iwk -= 3;
11068     --lend;
11069     --lptr;
11070     --list;
11071     --z__;
11072     --y;
11073     --x;
11074 
11075     /* Function Body */
11076     n1 = *k;
11077     nn = *n;
11078     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
11079         goto L21;
11080     }
11081     lpl = lend[n1];
11082     lpf = lptr[lpl];
11083     nnb = nbcnt_(&lpl, &lptr[1]);
11084     bdry = list[lpl] < 0;
11085     if (bdry) {
11086         ++nnb;
11087     }
11088     if (nnb < 3) {
11089         goto L23;
11090     }
11091     lwkl = *lwk;
11092     *lwk = nnb - 3;
11093     if (lwkl < *lwk) {
11094         goto L22;
11095     }
11096     iwl = 0;
11097     if (nnb == 3) {
11098         goto L3;
11099     }
11100 
11101 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
11102 /*   beginning with the second neighbor.  NR and NL are the */
11103 /*   neighbors preceding and following N2, respectively, and */
11104 /*   LP indexes NL.  The loop is exited when all possible */
11105 /*   swaps have been applied to arcs incident on N1. */
11106 
11107     x1 = x[n1];
11108     y1 = y[n1];
11109     z1 = z__[n1];
11110     nfrst = list[lpf];
11111     nr = nfrst;
11112     xr = x[nr];
11113     yr = y[nr];
11114     zr = z__[nr];
11115     lp = lptr[lpf];
11116     n2 = list[lp];
11117     x2 = x[n2];
11118     y2 = y[n2];
11119     z2 = z__[n2];
11120     lp = lptr[lp];
11121 
11122 /* Top of loop:  set NL to the neighbor following N2. */
11123 
11124 L1:
11125     nl = (i__1 = list[lp], abs(i__1));
11126     if (nl == nfrst && bdry) {
11127         goto L3;
11128     }
11129     xl = x[nl];
11130     yl = y[nl];
11131     zl = z__[nl];
11132 
11133 /*   Test for a convex quadrilateral.  To avoid an incorrect */
11134 /*     test caused by collinearity, use the fact that if N1 */
11135 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
11136 /*     a boundary node, then N2 LEFT NL->NR. */
11137 
11138     lpl2 = lend[n2];
11139     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
11140             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
11141             z2)))) {
11142 
11143 /*   Nonconvex quadrilateral -- no swap is possible. */
11144 
11145         nr = n2;
11146         xr = x2;
11147         yr = y2;
11148         zr = z2;
11149         goto L2;
11150     }
11151 
11152 /*   The quadrilateral defined by adjacent triangles */
11153 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
11154 /*     NL-NR and store it in IWK unless NL and NR are */
11155 /*     already adjacent, in which case the swap is not */
11156 /*     possible.  Indexes larger than N1 must be decremented */
11157 /*     since N1 will be deleted from X, Y, and Z. */
11158 
11159     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11160     if (lp21 == 0) {
11161         nr = n2;
11162         xr = x2;
11163         yr = y2;
11164         zr = z2;
11165         goto L2;
11166     }
11167     ++iwl;
11168     if (nl <= n1) {
11169         iwk[(iwl << 1) + 1] = nl;
11170     } else {
11171         iwk[(iwl << 1) + 1] = nl - 1;
11172     }
11173     if (nr <= n1) {
11174         iwk[(iwl << 1) + 2] = nr;
11175     } else {
11176         iwk[(iwl << 1) + 2] = nr - 1;
11177     }
11178 
11179 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
11180 
11181     lpl = lend[n1];
11182     --nnb;
11183     if (nnb == 3) {
11184         goto L3;
11185     }
11186     lpf = lptr[lpl];
11187     nfrst = list[lpf];
11188     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11189     if (nr == nfrst) {
11190         goto L2;
11191     }
11192 
11193 /*   NR is not the first neighbor of N1. */
11194 /*     Back up and test N1-NR for a swap again:  Set N2 to */
11195 /*     NR and NR to the previous neighbor of N1 -- the */
11196 /*     neighbor of NR which follows N1.  LP21 points to NL */
11197 /*     as a neighbor of NR. */
11198 
11199     n2 = nr;
11200     x2 = xr;
11201     y2 = yr;
11202     z2 = zr;
11203     lp21 = lptr[lp21];
11204     lp21 = lptr[lp21];
11205     nr = (i__1 = list[lp21], abs(i__1));
11206     xr = x[nr];
11207     yr = y[nr];
11208     zr = z__[nr];
11209     goto L1;
11210 
11211 /*   Bottom of loop -- test for termination of loop. */
11212 
11213 L2:
11214     if (n2 == nfrst) {
11215         goto L3;
11216     }
11217     n2 = nl;
11218     x2 = xl;
11219     y2 = yl;
11220     z2 = zl;
11221     lp = lptr[lp];
11222     goto L1;
11223 
11224 /* Delete N1 and all its incident arcs.  If N1 is an interior */
11225 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
11226 /*   then N1 must be separated from its neighbors by a plane */
11227 /*   containing the origin -- its removal reverses the effect */
11228 /*   of a call to COVSPH, and all its neighbors become */
11229 /*   boundary nodes.  This is achieved by treating it as if */
11230 /*   it were a boundary node (setting BDRY to TRUE, changing */
11231 /*   a sign in LIST, and incrementing NNB). */
11232 
11233 L3:
11234     if (! bdry) {
11235         if (nnb > 3) {
11236             bdry = TRUE_;
11237         } else {
11238             lpf = lptr[lpl];
11239             nr = list[lpf];
11240             lp = lptr[lpf];
11241             n2 = list[lp];
11242             nl = list[lpl];
11243             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11244                     x[n2], &y[n2], &z__[n2]);
11245         }
11246         if (bdry) {
11247 
11248 /*   IF a boundary node already exists, then N1 and its */
11249 /*     neighbors cannot be converted to boundary nodes. */
11250 /*     (They must be collinear.)  This is a problem if */
11251 /*     NNB > 3. */
11252 
11253             i__1 = nn;
11254             for (i__ = 1; i__ <= i__1; ++i__) {
11255                 if (list[lend[i__]] < 0) {
11256                     bdry = FALSE_;
11257                     goto L5;
11258                 }
11259 /* L4: */
11260             }
11261             list[lpl] = -list[lpl];
11262             ++nnb;
11263         }
11264     }
11265 L5:
11266     if (! bdry && nnb > 3) {
11267         goto L24;
11268     }
11269 
11270 /* Initialize for loop on neighbors.  LPL points to the last */
11271 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11272 
11273     lp = lpl;
11274     lnw = *lnew;
11275 
11276 /* Loop on neighbors N2 of N1, beginning with the first. */
11277 
11278 L6:
11279     lp = lptr[lp];
11280     n2 = (i__1 = list[lp], abs(i__1));
11281     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11282     if (lph < 0) {
11283         goto L23;
11284     }
11285 
11286 /*   LP and LPL may require alteration. */
11287 
11288     if (lpl == lnw) {
11289         lpl = lph;
11290     }
11291     if (lp == lnw) {
11292         lp = lph;
11293     }
11294     if (lp != lpl) {
11295         goto L6;
11296     }
11297 
11298 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11299 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11300 /*   which are larger than N1 must be decremented. */
11301 
11302     --nn;
11303     if (n1 > nn) {
11304         goto L9;
11305     }
11306     i__1 = nn;
11307     for (i__ = n1; i__ <= i__1; ++i__) {
11308         x[i__] = x[i__ + 1];
11309         y[i__] = y[i__ + 1];
11310         z__[i__] = z__[i__ + 1];
11311         lend[i__] = lend[i__ + 1];
11312 /* L7: */
11313     }
11314 
11315     i__1 = lnw - 1;
11316     for (i__ = 1; i__ <= i__1; ++i__) {
11317         if (list[i__] > n1) {
11318             --list[i__];
11319         }
11320         if (list[i__] < -n1) {
11321             ++list[i__];
11322         }
11323 /* L8: */
11324     }
11325 
11326 /*   For LPN = first to last neighbors of N1, delete the */
11327 /*     preceding neighbor (indexed by LP). */
11328 
11329 /*   Each empty LIST,LPTR location LP is filled in with the */
11330 /*     values at LNW-1, and LNW is decremented.  All pointers */
11331 /*     (including those in LPTR and LEND) with value LNW-1 */
11332 /*     must be changed to LP. */
11333 
11334 /*  LPL points to the last neighbor of N1. */
11335 
11336 L9:
11337     if (bdry) {
11338         --nnb;
11339     }
11340     lpn = lpl;
11341     i__1 = nnb;
11342     for (j = 1; j <= i__1; ++j) {
11343         --lnw;
11344         lp = lpn;
11345         lpn = lptr[lp];
11346         list[lp] = list[lnw];
11347         lptr[lp] = lptr[lnw];
11348         if (lptr[lpn] == lnw) {
11349             lptr[lpn] = lp;
11350         }
11351         if (lpn == lnw) {
11352             lpn = lp;
11353         }
11354         for (i__ = nn; i__ >= 1; --i__) {
11355             if (lend[i__] == lnw) {
11356                 lend[i__] = lp;
11357                 goto L11;
11358             }
11359 /* L10: */
11360         }
11361 
11362 L11:
11363         for (i__ = lnw - 1; i__ >= 1; --i__) {
11364             if (lptr[i__] == lnw) {
11365                 lptr[i__] = lp;
11366             }
11367 /* L12: */
11368         }
11369 /* L13: */
11370     }
11371 
11372 /* Update N and LNEW, and optimize the patch of triangles */
11373 /*   containing K (on input) by applying swaps to the arcs */
11374 /*   in IWK. */
11375 
11376     *n = nn;
11377     *lnew = lnw;
11378     if (iwl > 0) {
11379         nit = iwl << 2;
11380         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11381                 nit, &iwk[3], &ierr);
11382         if (ierr != 0 && ierr != 1) {
11383             goto L25;
11384         }
11385         if (ierr == 1) {
11386             goto L26;
11387         }
11388     }
11389 
11390 /* Successful termination. */
11391 
11392     *ier = 0;
11393     return 0;
11394 
11395 /* Invalid input parameter. */
11396 
11397 L21:
11398     *ier = 1;
11399     return 0;
11400 
11401 /* Insufficient space reserved for IWK. */
11402 
11403 L22:
11404     *ier = 2;
11405     return 0;
11406 
11407 /* Invalid triangulation data structure.  NNB < 3 on input or */
11408 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11409 
11410 L23:
11411     *ier = 3;
11412     return 0;
11413 
11414 /* N1 is interior but NNB could not be reduced to 3. */
11415 
11416 L24:
11417     *ier = 4;
11418     return 0;
11419 
11420 /* Error flag (other than 1) returned by OPTIM. */
11421 
11422 L25:
11423     *ier = 5;
11424 /*      WRITE (*,100) NIT, IERR */
11425 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11426 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11427     return 0;
11428 
11429 /* Error flag 1 returned by OPTIM. */
11430 
11431 L26:
11432     *ier = 6;
11433     return 0;
11434 } /* delnod_ */
11435 
11436 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11437         double *tol, int *nseg)
11438 {
11439     /* System generated locals */
11440     int i__1;
11441     double d__1;
11442 
11443     /* Builtin functions */
11444     //double sqrt(double);
11445 
11446     /* Local variables */
11447     static int i__, k;
11448     static double s, p1[3], p2[3], u1, u2, v1, v2;
11449     static int na;
11450     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11451 
11452 
11453 /* *********************************************************** */
11454 
11455 /*                                              From STRIPACK */
11456 /*                                            Robert J. Renka */
11457 /*                                  Dept. of Computer Science */
11458 /*                                       Univ. of North Texas */
11459 /*                                           renka@cs.unt.edu */
11460 /*                                                   03/04/03 */
11461 
11462 /*   Given unit vectors P and Q corresponding to northern */
11463 /* hemisphere points (with positive third components), this */
11464 /* subroutine draws a polygonal line which approximates the */
11465 /* projection of arc P-Q onto the plane containing the */
11466 /* equator. */
11467 
11468 /*   The line segment is drawn by writing a sequence of */
11469 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11470 /* is assumed that an open file is attached to the unit, */
11471 /* header comments have been written to the file, a window- */
11472 /* to-viewport mapping has been established, etc. */
11473 
11474 /* On input: */
11475 
11476 /*       LUN = long int unit number in the range 0 to 99. */
11477 
11478 /*       P,Q = Arrays of length 3 containing the endpoints of */
11479 /*             the arc to be drawn. */
11480 
11481 /*       TOL = Maximum distance in world coordinates between */
11482 /*             the projected arc and polygonal line. */
11483 
11484 /* Input parameters are not altered by this routine. */
11485 
11486 /* On output: */
11487 
11488 /*       NSEG = Number of line segments in the polygonal */
11489 /*              approximation to the projected arc.  This is */
11490 /*              a decreasing function of TOL.  NSEG = 0 and */
11491 /*              no drawing is performed if P = Q or P = -Q */
11492 /*              or an error is encountered in writing to unit */
11493 /*              LUN. */
11494 
11495 /* STRIPACK modules required by DRWARC:  None */
11496 
11497 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11498 
11499 /* *********************************************************** */
11500 
11501 
11502 /* Local parameters: */
11503 
11504 /* DP =    (Q-P)/NSEG */
11505 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11506 /*           onto the projection plane */
11507 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11508 /* ERR =   Orthogonal distance from the projected midpoint */
11509 /*           PM' to the line defined by P' and Q': */
11510 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11511 /* I,K =   DO-loop indexes */
11512 /* NA =    Number of arcs (segments) in the partition of P-Q */
11513 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11514 /*           arc P-Q into NSEG segments; obtained by normal- */
11515 /*           izing PM values */
11516 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11517 /*           uniform partition of the line segment P-Q into */
11518 /*           NSEG segments */
11519 /* S =     Scale factor 1/NA */
11520 /* U1,V1 = Components of P' */
11521 /* U2,V2 = Components of Q' */
11522 /* UM,VM = Components of the midpoint PM' */
11523 
11524 
11525 /* Compute the midpoint PM of arc P-Q. */
11526 
11527     /* Parameter adjustments */
11528     --q;
11529     --p;
11530 
11531     /* Function Body */
11532     enrm = 0.;
11533     for (i__ = 1; i__ <= 3; ++i__) {
11534         pm[i__ - 1] = p[i__] + q[i__];
11535         enrm += pm[i__ - 1] * pm[i__ - 1];
11536 /* L1: */
11537     }
11538     if (enrm == 0.) {
11539         goto L5;
11540     }
11541     enrm = sqrt(enrm);
11542     pm[0] /= enrm;
11543     pm[1] /= enrm;
11544     pm[2] /= enrm;
11545 
11546 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11547 /*   PM' = (UM,VM), respectively. */
11548 
11549     u1 = p[1];
11550     v1 = p[2];
11551     u2 = q[1];
11552     v2 = q[2];
11553     um = pm[0];
11554     vm = pm[1];
11555 
11556 /* Compute the orthogonal distance ERR from PM' to the line */
11557 /*   defined by P' and Q'.  This is the maximum deviation */
11558 /*   between the projected arc and the line segment.  It is */
11559 /*   undefined if P' = Q'. */
11560 
11561     du = u2 - u1;
11562     dv = v2 - v1;
11563     enrm = du * du + dv * dv;
11564     if (enrm == 0.) {
11565         goto L5;
11566     }
11567     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11568 
11569 /* Compute the number of arcs into which P-Q will be parti- */
11570 /*   tioned (the number of line segments to be drawn): */
11571 /*   NA = ERR/TOL. */
11572 
11573     na = (int) (err / *tol + 1.);
11574 
11575 /* Initialize for loop on arcs P1-P2, where the intermediate */
11576 /*   points are obtained by normalizing PM = P + k*DP for */
11577 /*   DP = (Q-P)/NA */
11578 
11579     s = 1. / (double) na;
11580     for (i__ = 1; i__ <= 3; ++i__) {
11581         dp[i__ - 1] = s * (q[i__] - p[i__]);
11582         pm[i__ - 1] = p[i__];
11583         p1[i__ - 1] = p[i__];
11584 /* L2: */
11585     }
11586 
11587 /* Loop on arcs P1-P2, drawing the line segments associated */
11588 /*   with the projected endpoints. */
11589 
11590     i__1 = na - 1;
11591     for (k = 1; k <= i__1; ++k) {
11592         enrm = 0.;
11593         for (i__ = 1; i__ <= 3; ++i__) {
11594             pm[i__ - 1] += dp[i__ - 1];
11595             enrm += pm[i__ - 1] * pm[i__ - 1];
11596 /* L3: */
11597         }
11598         if (enrm == 0.) {
11599             goto L5;
11600         }
11601         enrm = sqrt(enrm);
11602         p2[0] = pm[0] / enrm;
11603         p2[1] = pm[1] / enrm;
11604         p2[2] = pm[2] / enrm;
11605 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11606 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11607         p1[0] = p2[0];
11608         p1[1] = p2[1];
11609         p1[2] = p2[2];
11610 /* L4: */
11611     }
11612 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11613 
11614 /* No error encountered. */
11615 
11616     *nseg = na;
11617     return 0;
11618 
11619 /* Invalid input value of P or Q. */
11620 
11621 L5:
11622     *nseg = 0;
11623     return 0;
11624 } /* drwarc_ */
11625 
11626 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11627         double *y, double *z__, int *lwk, int *iwk, int *
11628         list, int *lptr, int *lend, int *ier)
11629 {
11630     /* System generated locals */
11631     int i__1;
11632 
11633     /* Local variables */
11634     static int i__, n0, n1, n2;
11635     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11636     static int nl, lp, nr;
11637     static double dp12;
11638     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11639     static double dp1l, dp2l, dp1r, dp2r;
11640     static int ierr;
11641     /* Subroutine */ int swap_(int *, int *, int *,
11642             int *, int *, int *, int *, int *);
11643     static int next, iwcp1, n1lst, iwend;
11644     /* Subroutine */ int optim_(double *, double *, double
11645             *, int *, int *, int *, int *, int *, int
11646             *, int *);
11647     static int n1frst;
11648 
11649 
11650 /* *********************************************************** */
11651 
11652 /*                                              From STRIPACK */
11653 /*                                            Robert J. Renka */
11654 /*                                  Dept. of Computer Science */
11655 /*                                       Univ. of North Texas */
11656 /*                                           renka@cs.unt.edu */
11657 /*                                                   07/30/98 */
11658 
11659 /*   Given a triangulation of N nodes and a pair of nodal */
11660 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11661 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11662 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11663 /* lation is input, the resulting triangulation is as close */
11664 /* as possible to a Delaunay triangulation in the sense that */
11665 /* all arcs other than IN1-IN2 are locally optimal. */
11666 
11667 /*   A sequence of calls to EDGE may be used to force the */
11668 /* presence of a set of edges defining the boundary of a non- */
11669 /* convex and/or multiply connected region, or to introduce */
11670 /* barriers into the triangulation.  Note that Subroutine */
11671 /* GETNP will not necessarily return closest nodes if the */
11672 /* triangulation has been constrained by a call to EDGE. */
11673 /* However, this is appropriate in some applications, such */
11674 /* as triangle-based interpolation on a nonconvex domain. */
11675 
11676 
11677 /* On input: */
11678 
11679 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11680 /*                 N defining a pair of nodes to be connected */
11681 /*                 by an arc. */
11682 
11683 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11684 /*               coordinates of the nodes. */
11685 
11686 /* The above parameters are not altered by this routine. */
11687 
11688 /*       LWK = Number of columns reserved for IWK.  This must */
11689 /*             be at least NI -- the number of arcs that */
11690 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11691 
11692 /*       IWK = int work array of length at least 2*LWK. */
11693 
11694 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11695 /*                        gulation.  Refer to Subroutine */
11696 /*                        TRMESH. */
11697 
11698 /* On output: */
11699 
11700 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11701 /*             not more than the input value of LWK) unless */
11702 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11703 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11704 
11705 /*       IWK = Array containing the indexes of the endpoints */
11706 /*             of the new arcs other than IN1-IN2 unless */
11707 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11708 /*             IN1->IN2 are stored in the first K-1 columns */
11709 /*             (left portion of IWK), column K contains */
11710 /*             zeros, and new arcs to the right of IN1->IN2 */
11711 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11712 /*             mined by searching IWK for the zeros.) */
11713 
11714 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11715 /*                        to reflect the presence of an arc */
11716 /*                        connecting IN1 and IN2 unless IER > */
11717 /*                        0.  The data structure has been */
11718 /*                        altered if IER >= 4. */
11719 
11720 /*       IER = Error indicator: */
11721 /*             IER = 0 if no errors were encountered. */
11722 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11723 /*                     or LWK < 0 on input. */
11724 /*             IER = 2 if more space is required in IWK. */
11725 /*                     Refer to LWK. */
11726 /*             IER = 3 if IN1 and IN2 could not be connected */
11727 /*                     due to either an invalid data struc- */
11728 /*                     ture or collinear nodes (and floating */
11729 /*                     point error). */
11730 /*             IER = 4 if an error flag other than IER = 1 */
11731 /*                     was returned by OPTIM. */
11732 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11733 /*                     This is not necessarily an error, but */
11734 /*                     the arcs other than IN1-IN2 may not */
11735 /*                     be optimal. */
11736 
11737 /*   An error message is written to the standard output unit */
11738 /* in the case of IER = 3 or IER = 4. */
11739 
11740 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11741 /*                              SWPTST */
11742 
11743 /* Intrinsic function called by EDGE:  ABS */
11744 
11745 /* *********************************************************** */
11746 
11747 
11748 /* Local parameters: */
11749 
11750 /* DPij =     Dot product <Ni,Nj> */
11751 /* I =        DO-loop index and column index for IWK */
11752 /* IERR =     Error flag returned by Subroutine OPTIM */
11753 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11754 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11755 /* IWCP1 =    IWC + 1 */
11756 /* IWEND =    Input or output value of LWK */
11757 /* IWF =      IWK (column) index of the first (leftmost) arc */
11758 /*              which intersects IN1->IN2 */
11759 /* IWL =      IWK (column) index of the last (rightmost) are */
11760 /*              which intersects IN1->IN2 */
11761 /* LFT =      Flag used to determine if a swap results in the */
11762 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11763 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11764 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11765 /* LP =       List pointer (index for LIST and LPTR) */
11766 /* LP21 =     Unused parameter returned by SWAP */
11767 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11768 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11769 /* N1,N2 =    Local copies of IN1 and IN2 */
11770 /* N1FRST =   First neighbor of IN1 */
11771 /* N1LST =    (Signed) last neighbor of IN1 */
11772 /* NEXT =     Node opposite NL->NR */
11773 /* NIT =      Flag or number of iterations employed by OPTIM */
11774 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11775 /*              with NL LEFT IN1->IN2 */
11776 /* X0,Y0,Z0 = Coordinates of N0 */
11777 /* X1,Y1,Z1 = Coordinates of IN1 */
11778 /* X2,Y2,Z2 = Coordinates of IN2 */
11779 
11780 
11781 /* Store IN1, IN2, and LWK in local variables and test for */
11782 /*   errors. */
11783 
11784     /* Parameter adjustments */
11785     --lend;
11786     --lptr;
11787     --list;
11788     iwk -= 3;
11789     --z__;
11790     --y;
11791     --x;
11792 
11793     /* Function Body */
11794     n1 = *in1;
11795     n2 = *in2;
11796     iwend = *lwk;
11797     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11798         goto L31;
11799     }
11800 
11801 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11802 /*   neighbor of N1. */
11803 
11804     lpl = lend[n1];
11805     n0 = (i__1 = list[lpl], abs(i__1));
11806     lp = lpl;
11807 L1:
11808     if (n0 == n2) {
11809         goto L30;
11810     }
11811     lp = lptr[lp];
11812     n0 = list[lp];
11813     if (lp != lpl) {
11814         goto L1;
11815     }
11816 
11817 /* Initialize parameters. */
11818 
11819     iwl = 0;
11820     nit = 0;
11821 
11822 /* Store the coordinates of N1 and N2. */
11823 
11824 L2:
11825     x1 = x[n1];
11826     y1 = y[n1];
11827     z1 = z__[n1];
11828     x2 = x[n2];
11829     y2 = y[n2];
11830     z2 = z__[n2];
11831 
11832 /* Set NR and NL to adjacent neighbors of N1 such that */
11833 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11834 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11835 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11836 
11837 /*   Initialization:  Set N1FRST and N1LST to the first and */
11838 /*     (signed) last neighbors of N1, respectively, and */
11839 /*     initialize NL to N1FRST. */
11840 
11841     lpl = lend[n1];
11842     n1lst = list[lpl];
11843     lp = lptr[lpl];
11844     n1frst = list[lp];
11845     nl = n1frst;
11846     if (n1lst < 0) {
11847         goto L4;
11848     }
11849 
11850 /*   N1 is an interior node.  Set NL to the first candidate */
11851 /*     for NR (NL LEFT N2->N1). */
11852 
11853 L3:
11854     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11855         goto L4;
11856     }
11857     lp = lptr[lp];
11858     nl = list[lp];
11859     if (nl != n1frst) {
11860         goto L3;
11861     }
11862 
11863 /*   All neighbors of N1 are strictly left of N1->N2. */
11864 
11865     goto L5;
11866 
11867 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11868 /*     following neighbor of N1. */
11869 
11870 L4:
11871     nr = nl;
11872     lp = lptr[lp];
11873     nl = (i__1 = list[lp], abs(i__1));
11874     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11875 
11876 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11877 /*     are employed to avoid an error associated with */
11878 /*     collinear nodes. */
11879 
11880         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11881         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11882         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11883         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11884         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11885         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11886                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11887             goto L6;
11888         }
11889 
11890 /*   NL-NR does not intersect N1-N2.  However, there is */
11891 /*     another candidate for the first arc if NL lies on */
11892 /*     the line N1-N2. */
11893 
11894         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11895             goto L5;
11896         }
11897     }
11898 
11899 /*   Bottom of loop. */
11900 
11901     if (nl != n1frst) {
11902         goto L4;
11903     }
11904 
11905 /* Either the triangulation is invalid or N1-N2 lies on the */
11906 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11907 /*   intersecting N1-N2) was not found due to floating point */
11908 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11909 /*   has already been done. */
11910 
11911 L5:
11912     if (nit > 0) {
11913         goto L33;
11914     }
11915     nit = 1;
11916     n1 = n2;
11917     n2 = *in1;
11918     goto L2;
11919 
11920 /* Store the ordered sequence of intersecting edges NL->NR in */
11921 /*   IWK(1,IWL)->IWK(2,IWL). */
11922 
11923 L6:
11924     ++iwl;
11925     if (iwl > iwend) {
11926         goto L32;
11927     }
11928     iwk[(iwl << 1) + 1] = nl;
11929     iwk[(iwl << 1) + 2] = nr;
11930 
11931 /*   Set NEXT to the neighbor of NL which follows NR. */
11932 
11933     lpl = lend[nl];
11934     lp = lptr[lpl];
11935 
11936 /*   Find NR as a neighbor of NL.  The search begins with */
11937 /*     the first neighbor. */
11938 
11939 L7:
11940     if (list[lp] == nr) {
11941         goto L8;
11942     }
11943     lp = lptr[lp];
11944     if (lp != lpl) {
11945         goto L7;
11946     }
11947 
11948 /*   NR must be the last neighbor, and NL->NR cannot be a */
11949 /*     boundary edge. */
11950 
11951     if (list[lp] != nr) {
11952         goto L33;
11953     }
11954 
11955 /*   Set NEXT to the neighbor following NR, and test for */
11956 /*     termination of the store loop. */
11957 
11958 L8:
11959     lp = lptr[lp];
11960     next = (i__1 = list[lp], abs(i__1));
11961     if (next == n2) {
11962         goto L9;
11963     }
11964 
11965 /*   Set NL or NR to NEXT. */
11966 
11967     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11968         nl = next;
11969     } else {
11970         nr = next;
11971     }
11972     goto L6;
11973 
11974 /* IWL is the number of arcs which intersect N1-N2. */
11975 /*   Store LWK. */
11976 
11977 L9:
11978     *lwk = iwl;
11979     iwend = iwl;
11980 
11981 /* Initialize for edge swapping loop -- all possible swaps */
11982 /*   are applied (even if the new arc again intersects */
11983 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11984 /*   left portion of IWK, and arcs to the right are stored in */
11985 /*   the right portion.  IWF and IWL index the first and last */
11986 /*   intersecting arcs. */
11987 
11988     iwf = 1;
11989 
11990 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11991 /*   IWC points to the arc currently being processed.  LFT */
11992 /*   .LE. 0 iff N0 LEFT N1->N2. */
11993 
11994 L10:
11995     lft = 0;
11996     n0 = n1;
11997     x0 = x1;
11998     y0 = y1;
11999     z0 = z1;
12000     nl = iwk[(iwf << 1) + 1];
12001     nr = iwk[(iwf << 1) + 2];
12002     iwc = iwf;
12003 
12004 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
12005 /*     last arc. */
12006 
12007 L11:
12008     if (iwc == iwl) {
12009         goto L21;
12010     }
12011     iwcp1 = iwc + 1;
12012     next = iwk[(iwcp1 << 1) + 1];
12013     if (next != nl) {
12014         goto L16;
12015     }
12016     next = iwk[(iwcp1 << 1) + 2];
12017 
12018 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
12019 /*     swap. */
12020 
12021     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12022             z__[next])) {
12023         goto L14;
12024     }
12025     if (lft >= 0) {
12026         goto L12;
12027     }
12028     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12029             z__[next])) {
12030         goto L14;
12031     }
12032 
12033 /*   Replace NL->NR with N0->NEXT. */
12034 
12035     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12036     iwk[(iwc << 1) + 1] = n0;
12037     iwk[(iwc << 1) + 2] = next;
12038     goto L15;
12039 
12040 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
12041 /*     the left, and store N0-NEXT in the right portion of */
12042 /*     IWK. */
12043 
12044 L12:
12045     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12046     i__1 = iwl;
12047     for (i__ = iwcp1; i__ <= i__1; ++i__) {
12048         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12049         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12050 /* L13: */
12051     }
12052     iwk[(iwl << 1) + 1] = n0;
12053     iwk[(iwl << 1) + 2] = next;
12054     --iwl;
12055     nr = next;
12056     goto L11;
12057 
12058 /*   A swap is not possible.  Set N0 to NR. */
12059 
12060 L14:
12061     n0 = nr;
12062     x0 = x[n0];
12063     y0 = y[n0];
12064     z0 = z__[n0];
12065     lft = 1;
12066 
12067 /*   Advance to the next arc. */
12068 
12069 L15:
12070     nr = next;
12071     ++iwc;
12072     goto L11;
12073 
12074 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
12075 /*     Test for a possible swap. */
12076 
12077 L16:
12078     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12079             z__[next])) {
12080         goto L19;
12081     }
12082     if (lft <= 0) {
12083         goto L17;
12084     }
12085     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12086             z__[next])) {
12087         goto L19;
12088     }
12089 
12090 /*   Replace NL->NR with NEXT->N0. */
12091 
12092     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12093     iwk[(iwc << 1) + 1] = next;
12094     iwk[(iwc << 1) + 2] = n0;
12095     goto L20;
12096 
12097 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
12098 /*     the right, and store N0-NEXT in the left portion of */
12099 /*     IWK. */
12100 
12101 L17:
12102     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12103     i__1 = iwf;
12104     for (i__ = iwc - 1; i__ >= i__1; --i__) {
12105         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12106         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12107 /* L18: */
12108     }
12109     iwk[(iwf << 1) + 1] = n0;
12110     iwk[(iwf << 1) + 2] = next;
12111     ++iwf;
12112     goto L20;
12113 
12114 /*   A swap is not possible.  Set N0 to NL. */
12115 
12116 L19:
12117     n0 = nl;
12118     x0 = x[n0];
12119     y0 = y[n0];
12120     z0 = z__[n0];
12121     lft = -1;
12122 
12123 /*   Advance to the next arc. */
12124 
12125 L20:
12126     nl = next;
12127     ++iwc;
12128     goto L11;
12129 
12130 /*   N2 is opposite NL->NR (IWC = IWL). */
12131 
12132 L21:
12133     if (n0 == n1) {
12134         goto L24;
12135     }
12136     if (lft < 0) {
12137         goto L22;
12138     }
12139 
12140 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
12141 
12142     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
12143         goto L10;
12144     }
12145 
12146 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
12147 /*     portion of IWK. */
12148 
12149     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12150     iwk[(iwl << 1) + 1] = n0;
12151     iwk[(iwl << 1) + 2] = n2;
12152     --iwl;
12153     goto L10;
12154 
12155 /*   N0 LEFT N1->N2.  Test for a possible swap. */
12156 
12157 L22:
12158     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12159         goto L10;
12160     }
12161 
12162 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
12163 /*     right, and store N0-N2 in the left portion of IWK. */
12164 
12165     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12166     i__ = iwl;
12167 L23:
12168     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12169     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12170     --i__;
12171     if (i__ > iwf) {
12172         goto L23;
12173     }
12174     iwk[(iwf << 1) + 1] = n0;
12175     iwk[(iwf << 1) + 2] = n2;
12176     ++iwf;
12177     goto L10;
12178 
12179 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
12180 /*   store zeros in IWK. */
12181 
12182 L24:
12183     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12184     iwk[(iwc << 1) + 1] = 0;
12185     iwk[(iwc << 1) + 2] = 0;
12186 
12187 /* Optimization procedure -- */
12188 
12189     *ier = 0;
12190     if (iwc > 1) {
12191 
12192 /*   Optimize the set of new arcs to the left of IN1->IN2. */
12193 
12194         nit = iwc - (1<<2);
12195         i__1 = iwc - 1;
12196         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12197                 nit, &iwk[3], &ierr);
12198         if (ierr != 0 && ierr != 1) {
12199             goto L34;
12200         }
12201         if (ierr == 1) {
12202             *ier = 5;
12203         }
12204     }
12205     if (iwc < iwend) {
12206 
12207 /*   Optimize the set of new arcs to the right of IN1->IN2. */
12208 
12209         nit = iwend - (iwc<<2);
12210         i__1 = iwend - iwc;
12211         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12212                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12213         if (ierr != 0 && ierr != 1) {
12214             goto L34;
12215         }
12216         if (ierr == 1) {
12217             goto L35;
12218         }
12219     }
12220     if (*ier == 5) {
12221         goto L35;
12222     }
12223 
12224 /* Successful termination (IER = 0). */
12225 
12226     return 0;
12227 
12228 /* IN1 and IN2 were adjacent on input. */
12229 
12230 L30:
12231     *ier = 0;
12232     return 0;
12233 
12234 /* Invalid input parameter. */
12235 
12236 L31:
12237     *ier = 1;
12238     return 0;
12239 
12240 /* Insufficient space reserved for IWK. */
12241 
12242 L32:
12243     *ier = 2;
12244     return 0;
12245 
12246 /* Invalid triangulation data structure or collinear nodes */
12247 /*   on convex hull boundary. */
12248 
12249 L33:
12250     *ier = 3;
12251 /*      WRITE (*,130) IN1, IN2 */
12252 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12253 /*     .        'tion or null triangles on boundary'/ */
12254 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12255     return 0;
12256 
12257 /* Error flag (other than 1) returned by OPTIM. */
12258 
12259 L34:
12260     *ier = 4;
12261 /*      WRITE (*,140) NIT, IERR */
12262 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12263 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12264     return 0;
12265 
12266 /* Error flag 1 returned by OPTIM. */
12267 
12268 L35:
12269     *ier = 5;
12270     return 0;
12271 } /* edge_ */
12272 
12273 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12274         int *list, int *lptr, int *lend, int *l, int *
12275         npts, double *df, int *ier)
12276 {
12277     /* System generated locals */
12278     int i__1, i__2;
12279 
12280     /* Local variables */
12281     static int i__, n1;
12282     static double x1, y1, z1;
12283     static int nb, ni, lp, np, lm1;
12284     static double dnb, dnp;
12285     static int lpl;
12286 
12287 
12288 /* *********************************************************** */
12289 
12290 /*                                              From STRIPACK */
12291 /*                                            Robert J. Renka */
12292 /*                                  Dept. of Computer Science */
12293 /*                                       Univ. of North Texas */
12294 /*                                           renka@cs.unt.edu */
12295 /*                                                   07/28/98 */
12296 
12297 /*   Given a Delaunay triangulation of N nodes on the unit */
12298 /* sphere and an array NPTS containing the indexes of L-1 */
12299 /* nodes ordered by angular distance from NPTS(1), this sub- */
12300 /* routine sets NPTS(L) to the index of the next node in the */
12301 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12302 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12303 /* of K closest nodes to N1 (including N1) may be determined */
12304 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12305 /* for K .GE. 2. */
12306 
12307 /*   The algorithm uses the property of a Delaunay triangula- */
12308 /* tion that the K-th closest node to N1 is a neighbor of one */
12309 /* of the K-1 closest nodes to N1. */
12310 
12311 
12312 /* On input: */
12313 
12314 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12315 /*               coordinates of the nodes. */
12316 
12317 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12318 /*                        fer to Subroutine TRMESH. */
12319 
12320 /*       L = Number of nodes in the sequence on output.  2 */
12321 /*           .LE. L .LE. N. */
12322 
12323 /* The above parameters are not altered by this routine. */
12324 
12325 /*       NPTS = Array of length .GE. L containing the indexes */
12326 /*              of the L-1 closest nodes to NPTS(1) in the */
12327 /*              first L-1 locations. */
12328 
12329 /* On output: */
12330 
12331 /*       NPTS = Array updated with the index of the L-th */
12332 /*              closest node to NPTS(1) in position L unless */
12333 /*              IER = 1. */
12334 
12335 /*       DF = Value of an increasing function (negative cos- */
12336 /*            ine) of the angular distance between NPTS(1) */
12337 /*            and NPTS(L) unless IER = 1. */
12338 
12339 /*       IER = Error indicator: */
12340 /*             IER = 0 if no errors were encountered. */
12341 /*             IER = 1 if L < 2. */
12342 
12343 /* Modules required by GETNP:  None */
12344 
12345 /* Intrinsic function called by GETNP:  ABS */
12346 
12347 /* *********************************************************** */
12348 
12349 
12350 /* Local parameters: */
12351 
12352 /* DNB,DNP =  Negative cosines of the angular distances from */
12353 /*              N1 to NB and to NP, respectively */
12354 /* I =        NPTS index and DO-loop index */
12355 /* LM1 =      L-1 */
12356 /* LP =       LIST pointer of a neighbor of NI */
12357 /* LPL =      Pointer to the last neighbor of NI */
12358 /* N1 =       NPTS(1) */
12359 /* NB =       Neighbor of NI and candidate for NP */
12360 /* NI =       NPTS(I) */
12361 /* NP =       Candidate for NPTS(L) */
12362 /* X1,Y1,Z1 = Coordinates of N1 */
12363 
12364     /* Parameter adjustments */
12365     --x;
12366     --y;
12367     --z__;
12368     --list;
12369     --lptr;
12370     --lend;
12371     --npts;
12372 
12373     /* Function Body */
12374     lm1 = *l - 1;
12375     if (lm1 < 1) {
12376         goto L6;
12377     }
12378     *ier = 0;
12379 
12380 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12381 
12382     n1 = npts[1];
12383     x1 = x[n1];
12384     y1 = y[n1];
12385     z1 = z__[n1];
12386     i__1 = lm1;
12387     for (i__ = 1; i__ <= i__1; ++i__) {
12388         ni = npts[i__];
12389         lend[ni] = -lend[ni];
12390 /* L1: */
12391     }
12392 
12393 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12394 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12395 /*   (the maximum distance). */
12396 
12397     dnp = 2.;
12398 
12399 /* Loop on nodes NI in NPTS. */
12400 
12401     i__1 = lm1;
12402     for (i__ = 1; i__ <= i__1; ++i__) {
12403         ni = npts[i__];
12404         lpl = -lend[ni];
12405         lp = lpl;
12406 
12407 /* Loop on neighbors NB of NI. */
12408 
12409 L2:
12410         nb = (i__2 = list[lp], abs(i__2));
12411         if (lend[nb] < 0) {
12412             goto L3;
12413         }
12414 
12415 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12416 /*   closer to N1. */
12417 
12418         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12419         if (dnb >= dnp) {
12420             goto L3;
12421         }
12422         np = nb;
12423         dnp = dnb;
12424 L3:
12425         lp = lptr[lp];
12426         if (lp != lpl) {
12427             goto L2;
12428         }
12429 /* L4: */
12430     }
12431     npts[*l] = np;
12432     *df = dnp;
12433 
12434 /* Unmark the elements of NPTS. */
12435 
12436     i__1 = lm1;
12437     for (i__ = 1; i__ <= i__1; ++i__) {
12438         ni = npts[i__];
12439         lend[ni] = -lend[ni];
12440 /* L5: */
12441     }
12442     return 0;
12443 
12444 /* L is outside its valid range. */
12445 
12446 L6:
12447     *ier = 1;
12448     return 0;
12449 } /* getnp_ */
12450 
12451 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12452         lptr, int *lnew)
12453 {
12454     static int lsav;
12455 
12456 
12457 /* *********************************************************** */
12458 
12459 /*                                              From STRIPACK */
12460 /*                                            Robert J. Renka */
12461 /*                                  Dept. of Computer Science */
12462 /*                                       Univ. of North Texas */
12463 /*                                           renka@cs.unt.edu */
12464 /*                                                   07/17/96 */
12465 
12466 /*   This subroutine inserts K as a neighbor of N1 following */
12467 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12468 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12469 /* become the first neighbor (even if N1 is a boundary node). */
12470 
12471 /*   This routine is identical to the similarly named routine */
12472 /* in TRIPACK. */
12473 
12474 
12475 /* On input: */
12476 
12477 /*       K = Index of the node to be inserted. */
12478 
12479 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12480 
12481 /* The above parameters are not altered by this routine. */
12482 
12483 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12484 /*                        gulation.  Refer to Subroutine */
12485 /*                        TRMESH. */
12486 
12487 /* On output: */
12488 
12489 /*       LIST,LPTR,LNEW = Data structure updated with the */
12490 /*                        addition of node K. */
12491 
12492 /* Modules required by INSERT:  None */
12493 
12494 /* *********************************************************** */
12495 
12496 
12497     /* Parameter adjustments */
12498     --lptr;
12499     --list;
12500 
12501     /* Function Body */
12502     lsav = lptr[*lp];
12503     lptr[*lp] = *lnew;
12504     list[*lnew] = *k;
12505     lptr[*lnew] = lsav;
12506     ++(*lnew);
12507     return 0;
12508 } /* insert_ */
12509 
12510 long int inside_(double *p, int *lv, double *xv, double *yv,
12511         double *zv, int *nv, int *listv, int *ier)
12512 {
12513     /* Initialized data */
12514 
12515     static double eps = .001;
12516 
12517     /* System generated locals */
12518     int i__1;
12519     long int ret_val = 0;
12520 
12521     /* Builtin functions */
12522     //double sqrt(double);
12523 
12524     /* Local variables */
12525     static double b[3], d__;
12526     static int k, n;
12527     static double q[3];
12528     static int i1, i2, k0;
12529     static double v1[3], v2[3], cn[3], bp, bq;
12530     static int ni;
12531     static double pn[3], qn[3], vn[3];
12532     static int imx;
12533     static long int lft1, lft2, even;
12534     static int ierr;
12535     static long int pinr, qinr;
12536     static double qnrm, vnrm;
12537     /* Subroutine */ int intrsc_(double *, double *,
12538             double *, double *, int *);
12539 
12540 
12541 /* *********************************************************** */
12542 
12543 /*                                              From STRIPACK */
12544 /*                                            Robert J. Renka */
12545 /*                                  Dept. of Computer Science */
12546 /*                                       Univ. of North Texas */
12547 /*                                           renka@cs.unt.edu */
12548 /*                                                   12/27/93 */
12549 
12550 /*   This function locates a point P relative to a polygonal */
12551 /* region R on the surface of the unit sphere, returning */
12552 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12553 /* defined by a cyclically ordered sequence of vertices which */
12554 /* form a positively-oriented simple closed curve.  Adjacent */
12555 /* vertices need not be distinct but the curve must not be */
12556 /* self-intersecting.  Also, while polygon edges are by defi- */
12557 /* nition restricted to a single hemisphere, R is not so */
12558 /* restricted.  Its interior is the region to the left as the */
12559 /* vertices are traversed in order. */
12560 
12561 /*   The algorithm consists of selecting a point Q in R and */
12562 /* then finding all points at which the great circle defined */
12563 /* by P and Q intersects the boundary of R.  P lies inside R */
12564 /* if and only if there is an even number of intersection */
12565 /* points between Q and P.  Q is taken to be a point immedi- */
12566 /* ately to the left of a directed boundary edge -- the first */
12567 /* one that results in no consistency-check failures. */
12568 
12569 /*   If P is close to the polygon boundary, the problem is */
12570 /* ill-conditioned and the decision may be incorrect.  Also, */
12571 /* an incorrect decision may result from a poor choice of Q */
12572 /* (if, for example, a boundary edge lies on the great cir- */
12573 /* cle defined by P and Q).  A more reliable result could be */
12574 /* obtained by a sequence of calls to INSIDE with the ver- */
12575 /* tices cyclically permuted before each call (to alter the */
12576 /* choice of Q). */
12577 
12578 
12579 /* On input: */
12580 
12581 /*       P = Array of length 3 containing the Cartesian */
12582 /*           coordinates of the point (unit vector) to be */
12583 /*           located. */
12584 
12585 /*       LV = Length of arrays XV, YV, and ZV. */
12586 
12587 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12588 /*                  sian coordinates of unit vectors (points */
12589 /*                  on the unit sphere).  These values are */
12590 /*                  not tested for validity. */
12591 
12592 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12593 /*            .LE. LV. */
12594 
12595 /*       LISTV = Array of length NV containing the indexes */
12596 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12597 /*               (and CCW-ordered) sequence of vertices that */
12598 /*               define R.  The last vertex (indexed by */
12599 /*               LISTV(NV)) is followed by the first (indexed */
12600 /*               by LISTV(1)).  LISTV entries must be in the */
12601 /*               range 1 to LV. */
12602 
12603 /* Input parameters are not altered by this function. */
12604 
12605 /* On output: */
12606 
12607 /*       INSIDE = TRUE if and only if P lies inside R unless */
12608 /*                IER .NE. 0, in which case the value is not */
12609 /*                altered. */
12610 
12611 /*       IER = Error indicator: */
12612 /*             IER = 0 if no errors were encountered. */
12613 /*             IER = 1 if LV or NV is outside its valid */
12614 /*                     range. */
12615 /*             IER = 2 if a LISTV entry is outside its valid */
12616 /*                     range. */
12617 /*             IER = 3 if the polygon boundary was found to */
12618 /*                     be self-intersecting.  This error will */
12619 /*                     not necessarily be detected. */
12620 /*             IER = 4 if every choice of Q (one for each */
12621 /*                     boundary edge) led to failure of some */
12622 /*                     internal consistency check.  The most */
12623 /*                     likely cause of this error is invalid */
12624 /*                     input:  P = (0,0,0), a null or self- */
12625 /*                     intersecting polygon, etc. */
12626 
12627 /* Module required by INSIDE:  INTRSC */
12628 
12629 /* Intrinsic function called by INSIDE:  SQRT */
12630 
12631 /* *********************************************************** */
12632 
12633 
12634 /* Local parameters: */
12635 
12636 /* B =         Intersection point between the boundary and */
12637 /*               the great circle defined by P and Q */
12638 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12639 /*               intersection points B that lie between P and */
12640 /*               Q (on the shorter arc) -- used to find the */
12641 /*               closest intersection points to P and Q */
12642 /* CN =        Q X P = normal to the plane of P and Q */
12643 /* D =         Dot product <B,P> or <B,Q> */
12644 /* EPS =       Parameter used to define Q as the point whose */
12645 /*               orthogonal distance to (the midpoint of) */
12646 /*               boundary edge V1->V2 is approximately EPS/ */
12647 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12648 /* EVEN =      TRUE iff an even number of intersection points */
12649 /*               lie between P and Q (on the shorter arc) */
12650 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12651 /*               boundary vertices (endpoints of a boundary */
12652 /*               edge) */
12653 /* IERR =      Error flag for calls to INTRSC (not tested) */
12654 /* IMX =       Local copy of LV and maximum value of I1 and */
12655 /*               I2 */
12656 /* K =         DO-loop index and LISTV index */
12657 /* K0 =        LISTV index of the first endpoint of the */
12658 /*               boundary edge used to compute Q */
12659 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12660 /*               the boundary traversal:  TRUE iff the vertex */
12661 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12662 /* N =         Local copy of NV */
12663 /* NI =        Number of intersections (between the boundary */
12664 /*               curve and the great circle P-Q) encountered */
12665 /* PINR =      TRUE iff P is to the left of the directed */
12666 /*               boundary edge associated with the closest */
12667 /*               intersection point to P that lies between P */
12668 /*               and Q (a left-to-right intersection as */
12669 /*               viewed from Q), or there is no intersection */
12670 /*               between P and Q (on the shorter arc) */
12671 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12672 /*               locate intersections B relative to arc Q->P */
12673 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12674 /*               the boundary edge indexed by LISTV(K0) -> */
12675 /*               LISTV(K0+1) */
12676 /* QINR =      TRUE iff Q is to the left of the directed */
12677 /*               boundary edge associated with the closest */
12678 /*               intersection point to Q that lies between P */
12679 /*               and Q (a right-to-left intersection as */
12680 /*               viewed from Q), or there is no intersection */
12681 /*               between P and Q (on the shorter arc) */
12682 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12683 /*               compute (normalize) Q */
12684 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12685 /*               traversal */
12686 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12687 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12688 /* VNRM =      Euclidean norm of VN */
12689 
12690     /* Parameter adjustments */
12691     --p;
12692     --zv;
12693     --yv;
12694     --xv;
12695     --listv;
12696 
12697     /* Function Body */
12698 
12699 /* Store local parameters, test for error 1, and initialize */
12700 /*   K0. */
12701 
12702     imx = *lv;
12703     n = *nv;
12704     if (n < 3 || n > imx) {
12705         goto L11;
12706     }
12707     k0 = 0;
12708     i1 = listv[1];
12709     if (i1 < 1 || i1 > imx) {
12710         goto L12;
12711     }
12712 
12713 /* Increment K0 and set Q to a point immediately to the left */
12714 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12715 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12716 
12717 L1:
12718     ++k0;
12719     if (k0 > n) {
12720         goto L14;
12721     }
12722     i1 = listv[k0];
12723     if (k0 < n) {
12724         i2 = listv[k0 + 1];
12725     } else {
12726         i2 = listv[1];
12727     }
12728     if (i2 < 1 || i2 > imx) {
12729         goto L12;
12730     }
12731     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12732     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12733     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12734     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12735     if (vnrm == 0.) {
12736         goto L1;
12737     }
12738     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12739     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12740     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12741     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12742     q[0] /= qnrm;
12743     q[1] /= qnrm;
12744     q[2] /= qnrm;
12745 
12746 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12747 
12748     cn[0] = q[1] * p[3] - q[2] * p[2];
12749     cn[1] = q[2] * p[1] - q[0] * p[3];
12750     cn[2] = q[0] * p[2] - q[1] * p[1];
12751     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12752         goto L1;
12753     }
12754     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12755     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12756     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12757     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12758     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12759     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12760 
12761 /* Initialize parameters for the boundary traversal. */
12762 
12763     ni = 0;
12764     even = TRUE_;
12765     bp = -2.;
12766     bq = -2.;
12767     pinr = TRUE_;
12768     qinr = TRUE_;
12769     i2 = listv[n];
12770     if (i2 < 1 || i2 > imx) {
12771         goto L12;
12772     }
12773     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12774 
12775 /* Loop on boundary arcs I1->I2. */
12776 
12777     i__1 = n;
12778     for (k = 1; k <= i__1; ++k) {
12779         i1 = i2;
12780         lft1 = lft2;
12781         i2 = listv[k];
12782         if (i2 < 1 || i2 > imx) {
12783             goto L12;
12784         }
12785         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12786         if (lft1 == lft2) {
12787             goto L2;
12788         }
12789 
12790 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12791 /*     point of intersection B. */
12792 
12793         ++ni;
12794         v1[0] = xv[i1];
12795         v1[1] = yv[i1];
12796         v1[2] = zv[i1];
12797         v2[0] = xv[i2];
12798         v2[1] = yv[i2];
12799         v2[2] = zv[i2];
12800         intrsc_(v1, v2, cn, b, &ierr);
12801 
12802 /*   B is between Q and P (on the shorter arc) iff */
12803 /*     B Forward Q->P and B Forward P->Q       iff */
12804 /*     <B,QN> > 0 and <B,PN> > 0. */
12805 
12806         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12807                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12808 
12809 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12810 
12811             even = ! even;
12812             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12813             if (d__ > bq) {
12814                 bq = d__;
12815                 qinr = lft2;
12816             }
12817             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12818             if (d__ > bp) {
12819                 bp = d__;
12820                 pinr = lft1;
12821             }
12822         }
12823 L2:
12824         ;
12825     }
12826 
12827 /* Test for consistency:  NI must be even and QINR must be */
12828 /*   TRUE. */
12829 
12830     if (ni != ni / 2 << 1 || ! qinr) {
12831         goto L1;
12832     }
12833 
12834 /* Test for error 3:  different values of PINR and EVEN. */
12835 
12836     if (pinr != even) {
12837         goto L13;
12838     }
12839 
12840 /* No error encountered. */
12841 
12842     *ier = 0;
12843     ret_val = even;
12844     return ret_val;
12845 
12846 /* LV or NV is outside its valid range. */
12847 
12848 L11:
12849     *ier = 1;
12850     return ret_val;
12851 
12852 /* A LISTV entry is outside its valid range. */
12853 
12854 L12:
12855     *ier = 2;
12856     return ret_val;
12857 
12858 /* The polygon boundary is self-intersecting. */
12859 
12860 L13:
12861     *ier = 3;
12862     return ret_val;
12863 
12864 /* Consistency tests failed for all values of Q. */
12865 
12866 L14:
12867     *ier = 4;
12868     return ret_val;
12869 } /* inside_ */
12870 
12871 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12872         i3, int *list, int *lptr, int *lend, int *lnew)
12873 {
12874     static int k, n1, n2, n3, lp;
12875     /* Subroutine */ int insert_(int *, int *, int *,
12876             int *, int *);
12877     int lstptr_(int *, int *, int *, int *);
12878 
12879 
12880 /* *********************************************************** */
12881 
12882 /*                                              From STRIPACK */
12883 /*                                            Robert J. Renka */
12884 /*                                  Dept. of Computer Science */
12885 /*                                       Univ. of North Texas */
12886 /*                                           renka@cs.unt.edu */
12887 /*                                                   07/17/96 */
12888 
12889 /*   This subroutine adds an interior node to a triangulation */
12890 /* of a set of points on the unit sphere.  The data structure */
12891 /* is updated with the insertion of node KK into the triangle */
12892 /* whose vertices are I1, I2, and I3.  No optimization of the */
12893 /* triangulation is performed. */
12894 
12895 /*   This routine is identical to the similarly named routine */
12896 /* in TRIPACK. */
12897 
12898 
12899 /* On input: */
12900 
12901 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12902 /*            and KK must not be equal to I1, I2, or I3. */
12903 
12904 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12905 /*                  sequence of vertices of a triangle which */
12906 /*                  contains node KK. */
12907 
12908 /* The above parameters are not altered by this routine. */
12909 
12910 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12911 /*                             triangulation.  Refer to Sub- */
12912 /*                             routine TRMESH.  Triangle */
12913 /*                             (I1,I2,I3) must be included */
12914 /*                             in the triangulation. */
12915 
12916 /* On output: */
12917 
12918 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12919 /*                             the addition of node KK.  KK */
12920 /*                             will be connected to nodes I1, */
12921 /*                             I2, and I3. */
12922 
12923 /* Modules required by INTADD:  INSERT, LSTPTR */
12924 
12925 /* *********************************************************** */
12926 
12927 
12928 /* Local parameters: */
12929 
12930 /* K =        Local copy of KK */
12931 /* LP =       LIST pointer */
12932 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12933 
12934     /* Parameter adjustments */
12935     --lend;
12936     --lptr;
12937     --list;
12938 
12939     /* Function Body */
12940     k = *kk;
12941 
12942 /* Initialization. */
12943 
12944     n1 = *i1;
12945     n2 = *i2;
12946     n3 = *i3;
12947 
12948 /* Add K as a neighbor of I1, I2, and I3. */
12949 
12950     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12951     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12952     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12953     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12954     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12955     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12956 
12957 /* Add I1, I2, and I3 as neighbors of K. */
12958 
12959     list[*lnew] = n1;
12960     list[*lnew + 1] = n2;
12961     list[*lnew + 2] = n3;
12962     lptr[*lnew] = *lnew + 1;
12963     lptr[*lnew + 1] = *lnew + 2;
12964     lptr[*lnew + 2] = *lnew;
12965     lend[k] = *lnew + 2;
12966     *lnew += 3;
12967     return 0;
12968 } /* intadd_ */
12969 
12970 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12971         double *p, int *ier)
12972 {
12973     /* Builtin functions */
12974     //double sqrt(double);
12975 
12976     /* Local variables */
12977     static int i__;
12978     static double t, d1, d2, pp[3], ppn;
12979 
12980 
12981 /* *********************************************************** */
12982 
12983 /*                                              From STRIPACK */
12984 /*                                            Robert J. Renka */
12985 /*                                  Dept. of Computer Science */
12986 /*                                       Univ. of North Texas */
12987 /*                                           renka@cs.unt.edu */
12988 /*                                                   07/19/90 */
12989 
12990 /*   Given a great circle C and points P1 and P2 defining an */
12991 /* arc A on the surface of the unit sphere, where A is the */
12992 /* shorter of the two portions of the great circle C12 assoc- */
12993 /* iated with P1 and P2, this subroutine returns the point */
12994 /* of intersection P between C and C12 that is closer to A. */
12995 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12996 /* C, P is the point of intersection of C with A. */
12997 
12998 
12999 /* On input: */
13000 
13001 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
13002 /*               coordinates of unit vectors. */
13003 
13004 /*       CN = Array of length 3 containing the Cartesian */
13005 /*            coordinates of a nonzero vector which defines C */
13006 /*            as the intersection of the plane whose normal */
13007 /*            is CN with the unit sphere.  Thus, if C is to */
13008 /*            be the great circle defined by P and Q, CN */
13009 /*            should be P X Q. */
13010 
13011 /* The above parameters are not altered by this routine. */
13012 
13013 /*       P = Array of length 3. */
13014 
13015 /* On output: */
13016 
13017 /*       P = Point of intersection defined above unless IER */
13018 /*           .NE. 0, in which case P is not altered. */
13019 
13020 /*       IER = Error indicator. */
13021 /*             IER = 0 if no errors were encountered. */
13022 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
13023 /*                     iff P1 = P2 or CN = 0 or there are */
13024 /*                     two intersection points at the same */
13025 /*                     distance from A. */
13026 /*             IER = 2 if P2 = -P1 and the definition of A is */
13027 /*                     therefore ambiguous. */
13028 
13029 /* Modules required by INTRSC:  None */
13030 
13031 /* Intrinsic function called by INTRSC:  SQRT */
13032 
13033 /* *********************************************************** */
13034 
13035 
13036 /* Local parameters: */
13037 
13038 /* D1 =  <CN,P1> */
13039 /* D2 =  <CN,P2> */
13040 /* I =   DO-loop index */
13041 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
13042 /*         line defined by P1 and P2 */
13043 /* PPN = Norm of PP */
13044 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
13045 /*         in the plane of C */
13046 
13047     /* Parameter adjustments */
13048     --p;
13049     --cn;
13050     --p2;
13051     --p1;
13052 
13053     /* Function Body */
13054     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
13055     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
13056 
13057     if (d1 == d2) {
13058         *ier = 1;
13059         return 0;
13060     }
13061 
13062 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
13063 
13064     t = d1 / (d1 - d2);
13065     ppn = 0.;
13066     for (i__ = 1; i__ <= 3; ++i__) {
13067         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
13068         ppn += pp[i__ - 1] * pp[i__ - 1];
13069 /* L1: */
13070     }
13071 
13072 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
13073 
13074     if (ppn == 0.) {
13075         *ier = 2;
13076         return 0;
13077     }
13078     ppn = sqrt(ppn);
13079 
13080 /* Compute P = PP/PPN. */
13081 
13082     for (i__ = 1; i__ <= 3; ++i__) {
13083         p[i__] = pp[i__ - 1] / ppn;
13084 /* L2: */
13085     }
13086     *ier = 0;
13087     return 0;
13088 } /* intrsc_ */
13089 
13090 int jrand_(int *n, int *ix, int *iy, int *iz)
13091 {
13092     /* System generated locals */
13093     int ret_val;
13094 
13095     /* Local variables */
13096     static float u, x;
13097 
13098 
13099 /* *********************************************************** */
13100 
13101 /*                                              From STRIPACK */
13102 /*                                            Robert J. Renka */
13103 /*                                  Dept. of Computer Science */
13104 /*                                       Univ. of North Texas */
13105 /*                                           renka@cs.unt.edu */
13106 /*                                                   07/28/98 */
13107 
13108 /*   This function returns a uniformly distributed pseudo- */
13109 /* random int in the range 1 to N. */
13110 
13111 
13112 /* On input: */
13113 
13114 /*       N = Maximum value to be returned. */
13115 
13116 /* N is not altered by this function. */
13117 
13118 /*       IX,IY,IZ = int seeds initialized to values in */
13119 /*                  the range 1 to 30,000 before the first */
13120 /*                  call to JRAND, and not altered between */
13121 /*                  subsequent calls (unless a sequence of */
13122 /*                  random numbers is to be repeated by */
13123 /*                  reinitializing the seeds). */
13124 
13125 /* On output: */
13126 
13127 /*       IX,IY,IZ = Updated int seeds. */
13128 
13129 /*       JRAND = Random int in the range 1 to N. */
13130 
13131 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
13132 /*             and Portable Pseudo-random Number Generator", */
13133 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
13134 /*             pp. 188-190. */
13135 
13136 /* Modules required by JRAND:  None */
13137 
13138 /* Intrinsic functions called by JRAND:  INT, MOD, float */
13139 
13140 /* *********************************************************** */
13141 
13142 
13143 /* Local parameters: */
13144 
13145 /* U = Pseudo-random number uniformly distributed in the */
13146 /*     interval (0,1). */
13147 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
13148 /*       tional part is U. */
13149 
13150     *ix = *ix * 171 % 30269;
13151     *iy = *iy * 172 % 30307;
13152     *iz = *iz * 170 % 30323;
13153     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13154             30323.f;
13155     u = x - (int) x;
13156     ret_val = (int) ((float) (*n) * u + 1.f);
13157     return ret_val;
13158 } /* jrand_ */
13159 
13160 long int left_(double *x1, double *y1, double *z1, double *x2,
13161         double *y2, double *z2, double *x0, double *y0,
13162         double *z0)
13163 {
13164     /* System generated locals */
13165     long int ret_val;
13166 
13167 
13168 /* *********************************************************** */
13169 
13170 /*                                              From STRIPACK */
13171 /*                                            Robert J. Renka */
13172 /*                                  Dept. of Computer Science */
13173 /*                                       Univ. of North Texas */
13174 /*                                           renka@cs.unt.edu */
13175 /*                                                   07/15/96 */
13176 
13177 /*   This function determines whether node N0 is in the */
13178 /* (closed) left hemisphere defined by the plane containing */
13179 /* N1, N2, and the origin, where left is defined relative to */
13180 /* an observer at N1 facing N2. */
13181 
13182 
13183 /* On input: */
13184 
13185 /*       X1,Y1,Z1 = Coordinates of N1. */
13186 
13187 /*       X2,Y2,Z2 = Coordinates of N2. */
13188 
13189 /*       X0,Y0,Z0 = Coordinates of N0. */
13190 
13191 /* Input parameters are not altered by this function. */
13192 
13193 /* On output: */
13194 
13195 /*       LEFT = TRUE if and only if N0 is in the closed */
13196 /*              left hemisphere. */
13197 
13198 /* Modules required by LEFT:  None */
13199 
13200 /* *********************************************************** */
13201 
13202 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
13203 
13204     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13205             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13206 
13207 
13208     return ret_val;
13209 } /* left_ */
13210 
13211 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13212 {
13213     /* System generated locals */
13214     int ret_val;
13215 
13216     /* Local variables */
13217     static int nd, lp;
13218 
13219 
13220 /* *********************************************************** */
13221 
13222 /*                                              From STRIPACK */
13223 /*                                            Robert J. Renka */
13224 /*                                  Dept. of Computer Science */
13225 /*                                       Univ. of North Texas */
13226 /*                                           renka@cs.unt.edu */
13227 /*                                                   07/15/96 */
13228 
13229 /*   This function returns the index (LIST pointer) of NB in */
13230 /* the adjacency list for N0, where LPL = LEND(N0). */
13231 
13232 /*   This function is identical to the similarly named */
13233 /* function in TRIPACK. */
13234 
13235 
13236 /* On input: */
13237 
13238 /*       LPL = LEND(N0) */
13239 
13240 /*       NB = Index of the node whose pointer is to be re- */
13241 /*            turned.  NB must be connected to N0. */
13242 
13243 /*       LIST,LPTR = Data structure defining the triangula- */
13244 /*                   tion.  Refer to Subroutine TRMESH. */
13245 
13246 /* Input parameters are not altered by this function. */
13247 
13248 /* On output: */
13249 
13250 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13251 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13252 /*                neighbor of N0, in which case LSTPTR = LPL. */
13253 
13254 /* Modules required by LSTPTR:  None */
13255 
13256 /* *********************************************************** */
13257 
13258 
13259 /* Local parameters: */
13260 
13261 /* LP = LIST pointer */
13262 /* ND = Nodal index */
13263 
13264     /* Parameter adjustments */
13265     --lptr;
13266     --list;
13267 
13268     /* Function Body */
13269     lp = lptr[*lpl];
13270 L1:
13271     nd = list[lp];
13272     if (nd == *nb) {
13273         goto L2;
13274     }
13275     lp = lptr[lp];
13276     if (lp != *lpl) {
13277         goto L1;
13278     }
13279 
13280 L2:
13281     ret_val = lp;
13282     return ret_val;
13283 } /* lstptr_ */
13284 
13285 int nbcnt_(int *lpl, int *lptr)
13286 {
13287     /* System generated locals */
13288     int ret_val;
13289 
13290     /* Local variables */
13291     static int k, lp;
13292 
13293 
13294 /* *********************************************************** */
13295 
13296 /*                                              From STRIPACK */
13297 /*                                            Robert J. Renka */
13298 /*                                  Dept. of Computer Science */
13299 /*                                       Univ. of North Texas */
13300 /*                                           renka@cs.unt.edu */
13301 /*                                                   07/15/96 */
13302 
13303 /*   This function returns the number of neighbors of a node */
13304 /* N0 in a triangulation created by Subroutine TRMESH. */
13305 
13306 /*   This function is identical to the similarly named */
13307 /* function in TRIPACK. */
13308 
13309 
13310 /* On input: */
13311 
13312 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13313 /*             LPL = LEND(N0). */
13314 
13315 /*       LPTR = Array of pointers associated with LIST. */
13316 
13317 /* Input parameters are not altered by this function. */
13318 
13319 /* On output: */
13320 
13321 /*       NBCNT = Number of neighbors of N0. */
13322 
13323 /* Modules required by NBCNT:  None */
13324 
13325 /* *********************************************************** */
13326 
13327 
13328 /* Local parameters: */
13329 
13330 /* K =  Counter for computing the number of neighbors */
13331 /* LP = LIST pointer */
13332 
13333     /* Parameter adjustments */
13334     --lptr;
13335 
13336     /* Function Body */
13337     lp = *lpl;
13338     k = 1;
13339 
13340 L1:
13341     lp = lptr[lp];
13342     if (lp == *lpl) {
13343         goto L2;
13344     }
13345     ++k;
13346     goto L1;
13347 
13348 L2:
13349     ret_val = k;
13350     return ret_val;
13351 } /* nbcnt_ */
13352 
13353 int nearnd_(double *p, int *ist, int *n, double *x,
13354         double *y, double *z__, int *list, int *lptr, int
13355         *lend, double *al)
13356 {
13357     /* System generated locals */
13358     int ret_val, i__1;
13359 
13360     /* Builtin functions */
13361     //double acos(double);
13362 
13363     /* Local variables */
13364     static int l;
13365     static double b1, b2, b3;
13366     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13367     static double ds1;
13368     static int lp1, lp2;
13369     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13370     static int lpl;
13371     static double dsr;
13372     static int nst, listp[25], lptrp[25];
13373     /* Subroutine */ int trfind_(int *, double *, int *,
13374             double *, double *, double *, int *, int *,
13375             int *, double *, double *, double *, int *,
13376             int *, int *);
13377     int lstptr_(int *, int *, int *, int *);
13378 
13379 
13380 /* *********************************************************** */
13381 
13382 /*                                              From STRIPACK */
13383 /*                                            Robert J. Renka */
13384 /*                                  Dept. of Computer Science */
13385 /*                                       Univ. of North Texas */
13386 /*                                           renka@cs.unt.edu */
13387 /*                                                   07/28/98 */
13388 
13389 /*   Given a point P on the surface of the unit sphere and a */
13390 /* Delaunay triangulation created by Subroutine TRMESH, this */
13391 /* function returns the index of the nearest triangulation */
13392 /* node to P. */
13393 
13394 /*   The algorithm consists of implicitly adding P to the */
13395 /* triangulation, finding the nearest neighbor to P, and */
13396 /* implicitly deleting P from the triangulation.  Thus, it */
13397 /* is based on the fact that, if P is a node in a Delaunay */
13398 /* triangulation, the nearest node to P is a neighbor of P. */
13399 
13400 
13401 /* On input: */
13402 
13403 /*       P = Array of length 3 containing the Cartesian coor- */
13404 /*           dinates of the point P to be located relative to */
13405 /*           the triangulation.  It is assumed without a test */
13406 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13407 
13408 /*       IST = Index of a node at which TRFIND begins the */
13409 /*             search.  Search time depends on the proximity */
13410 /*             of this node to P. */
13411 
13412 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13413 
13414 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13415 /*               coordinates of the nodes. */
13416 
13417 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13418 /*                        gulation.  Refer to TRMESH. */
13419 
13420 /* Input parameters are not altered by this function. */
13421 
13422 /* On output: */
13423 
13424 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13425 /*                if N < 3 or the triangulation data struc- */
13426 /*                ture is invalid. */
13427 
13428 /*       AL = Arc length (angular distance in radians) be- */
13429 /*            tween P and NEARND unless NEARND = 0. */
13430 
13431 /*       Note that the number of candidates for NEARND */
13432 /*       (neighbors of P) is limited to LMAX defined in */
13433 /*       the PARAMETER statement below. */
13434 
13435 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13436 
13437 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13438 
13439 /* *********************************************************** */
13440 
13441 
13442 /* Local parameters: */
13443 
13444 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13445 /*               by TRFIND */
13446 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13447 /* DSR =       (Negative cosine of the) distance from P to NR */
13448 /* DX1,..DZ3 = Components of vectors used by the swap test */
13449 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13450 /*               the rightmost (I1) and leftmost (I2) visible */
13451 /*               boundary nodes as viewed from P */
13452 /* L =         Length of LISTP/LPTRP and number of neighbors */
13453 /*               of P */
13454 /* LMAX =      Maximum value of L */
13455 /* LISTP =     Indexes of the neighbors of P */
13456 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13457 /*               LISTP elements */
13458 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13459 /*               pointer */
13460 /* LP1,LP2 =   LISTP indexes (pointers) */
13461 /* LPL =       Pointer to the last neighbor of N1 */
13462 /* N1 =        Index of a node visible from P */
13463 /* N2 =        Index of an endpoint of an arc opposite P */
13464 /* N3 =        Index of the node opposite N1->N2 */
13465 /* NN =        Local copy of N */
13466 /* NR =        Index of a candidate for the nearest node to P */
13467 /* NST =       Index of the node at which TRFIND begins the */
13468 /*               search */
13469 
13470 
13471 /* Store local parameters and test for N invalid. */
13472 
13473     /* Parameter adjustments */
13474     --p;
13475     --lend;
13476     --z__;
13477     --y;
13478     --x;
13479     --list;
13480     --lptr;
13481 
13482     /* Function Body */
13483     nn = *n;
13484     if (nn < 3) {
13485         goto L6;
13486     }
13487     nst = *ist;
13488     if (nst < 1 || nst > nn) {
13489         nst = 1;
13490     }
13491 
13492 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13493 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13494 /*   from P. */
13495 
13496     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13497             1], &b1, &b2, &b3, &i1, &i2, &i3);
13498 
13499 /* Test for collinear nodes. */
13500 
13501     if (i1 == 0) {
13502         goto L6;
13503     }
13504 
13505 /* Store the linked list of 'neighbors' of P in LISTP and */
13506 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13507 /*   the last neighbor if P is not contained in a triangle. */
13508 /*   L is the length of LISTP and LPTRP, and is limited to */
13509 /*   LMAX. */
13510 
13511     if (i3 != 0) {
13512         listp[0] = i1;
13513         lptrp[0] = 2;
13514         listp[1] = i2;
13515         lptrp[1] = 3;
13516         listp[2] = i3;
13517         lptrp[2] = 1;
13518         l = 3;
13519     } else {
13520         n1 = i1;
13521         l = 1;
13522         lp1 = 2;
13523         listp[l - 1] = n1;
13524         lptrp[l - 1] = lp1;
13525 
13526 /*   Loop on the ordered sequence of visible boundary nodes */
13527 /*     N1 from I1 to I2. */
13528 
13529 L1:
13530         lpl = lend[n1];
13531         n1 = -list[lpl];
13532         l = lp1;
13533         lp1 = l + 1;
13534         listp[l - 1] = n1;
13535         lptrp[l - 1] = lp1;
13536         if (n1 != i2 && lp1 < 25) {
13537             goto L1;
13538         }
13539         l = lp1;
13540         listp[l - 1] = 0;
13541         lptrp[l - 1] = 1;
13542     }
13543 
13544 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13545 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13546 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13547 /*   indexes of N1 and N2. */
13548 
13549     lp2 = 1;
13550     n2 = i1;
13551     lp1 = lptrp[0];
13552     n1 = listp[lp1 - 1];
13553 
13554 /* Begin loop:  find the node N3 opposite N1->N2. */
13555 
13556 L2:
13557     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13558     if (list[lp] < 0) {
13559         goto L3;
13560     }
13561     lp = lptr[lp];
13562     n3 = (i__1 = list[lp], abs(i__1));
13563 
13564 /* Swap test:  Exit the loop if L = LMAX. */
13565 
13566     if (l == 25) {
13567         goto L4;
13568     }
13569     dx1 = x[n1] - p[1];
13570     dy1 = y[n1] - p[2];
13571     dz1 = z__[n1] - p[3];
13572 
13573     dx2 = x[n2] - p[1];
13574     dy2 = y[n2] - p[2];
13575     dz2 = z__[n2] - p[3];
13576 
13577     dx3 = x[n3] - p[1];
13578     dy3 = y[n3] - p[2];
13579     dz3 = z__[n3] - p[3];
13580     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13581             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13582         goto L3;
13583     }
13584 
13585 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13586 /*        The two new arcs opposite P must be tested. */
13587 
13588     ++l;
13589     lptrp[lp2 - 1] = l;
13590     listp[l - 1] = n3;
13591     lptrp[l - 1] = lp1;
13592     lp1 = l;
13593     n1 = n3;
13594     goto L2;
13595 
13596 /* No swap:  Advance to the next arc and test for termination */
13597 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13598 
13599 L3:
13600     if (lp1 == 1) {
13601         goto L4;
13602     }
13603     lp2 = lp1;
13604     n2 = n1;
13605     lp1 = lptrp[lp1 - 1];
13606     n1 = listp[lp1 - 1];
13607     if (n1 == 0) {
13608         goto L4;
13609     }
13610     goto L2;
13611 
13612 /* Set NR and DSR to the index of the nearest node to P and */
13613 /*   an increasing function (negative cosine) of its distance */
13614 /*   from P, respectively. */
13615 
13616 L4:
13617     nr = i1;
13618     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13619     i__1 = l;
13620     for (lp = 2; lp <= i__1; ++lp) {
13621         n1 = listp[lp - 1];
13622         if (n1 == 0) {
13623             goto L5;
13624         }
13625         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13626         if (ds1 < dsr) {
13627             nr = n1;
13628             dsr = ds1;
13629         }
13630 L5:
13631         ;
13632     }
13633     dsr = -dsr;
13634     if (dsr > 1.) {
13635         dsr = 1.;
13636     }
13637     *al = acos(dsr);
13638     ret_val = nr;
13639     return ret_val;
13640 
13641 /* Invalid input. */
13642 
13643 L6:
13644     ret_val = 0;
13645     return ret_val;
13646 } /* nearnd_ */
13647 
13648 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13649         int *na, int *list, int *lptr, int *lend, int *
13650         nit, int *iwk, int *ier)
13651 {
13652     /* System generated locals */
13653     int i__1, i__2;
13654 
13655     /* Local variables */
13656     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13657     static long int swp;
13658     static int iter;
13659     /* Subroutine */ int swap_(int *, int *, int *,
13660             int *, int *, int *, int *, int *);
13661     static int maxit;
13662     long int swptst_(int *, int *, int *, int *,
13663             double *, double *, double *);
13664 
13665 
13666 /* *********************************************************** */
13667 
13668 /*                                              From STRIPACK */
13669 /*                                            Robert J. Renka */
13670 /*                                  Dept. of Computer Science */
13671 /*                                       Univ. of North Texas */
13672 /*                                           renka@cs.unt.edu */
13673 /*                                                   07/30/98 */
13674 
13675 /*   Given a set of NA triangulation arcs, this subroutine */
13676 /* optimizes the portion of the triangulation consisting of */
13677 /* the quadrilaterals (pairs of adjacent triangles) which */
13678 /* have the arcs as diagonals by applying the circumcircle */
13679 /* test and appropriate swaps to the arcs. */
13680 
13681 /*   An iteration consists of applying the swap test and */
13682 /* swaps to all NA arcs in the order in which they are */
13683 /* stored.  The iteration is repeated until no swap occurs */
13684 /* or NIT iterations have been performed.  The bound on the */
13685 /* number of iterations may be necessary to prevent an */
13686 /* infinite loop caused by cycling (reversing the effect of a */
13687 /* previous swap) due to floating point inaccuracy when four */
13688 /* or more nodes are nearly cocircular. */
13689 
13690 
13691 /* On input: */
13692 
13693 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13694 
13695 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13696 
13697 /* The above parameters are not altered by this routine. */
13698 
13699 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13700 /*                        gulation.  Refer to Subroutine */
13701 /*                        TRMESH. */
13702 
13703 /*       NIT = Maximum number of iterations to be performed. */
13704 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13705 
13706 /*       IWK = int array dimensioned 2 by NA containing */
13707 /*             the nodal indexes of the arc endpoints (pairs */
13708 /*             of endpoints are stored in columns). */
13709 
13710 /* On output: */
13711 
13712 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13713 /*                        ture reflecting the swaps. */
13714 
13715 /*       NIT = Number of iterations performed. */
13716 
13717 /*       IWK = Endpoint indexes of the new set of arcs */
13718 /*             reflecting the swaps. */
13719 
13720 /*       IER = Error indicator: */
13721 /*             IER = 0 if no errors were encountered. */
13722 /*             IER = 1 if a swap occurred on the last of */
13723 /*                     MAXIT iterations, where MAXIT is the */
13724 /*                     value of NIT on input.  The new set */
13725 /*                     of arcs is not necessarily optimal */
13726 /*                     in this case. */
13727 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13728 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13729 /*                     IWK(1,I) for some I in the range 1 */
13730 /*                     to NA.  A swap may have occurred in */
13731 /*                     this case. */
13732 /*             IER = 4 if a zero pointer was returned by */
13733 /*                     Subroutine SWAP. */
13734 
13735 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13736 
13737 /* Intrinsic function called by OPTIM:  ABS */
13738 
13739 /* *********************************************************** */
13740 
13741 
13742 /* Local parameters: */
13743 
13744 /* I =       Column index for IWK */
13745 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13746 /* ITER =    Iteration count */
13747 /* LP =      LIST pointer */
13748 /* LP21 =    Parameter returned by SWAP (not used) */
13749 /* LPL =     Pointer to the last neighbor of IO1 */
13750 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13751 /*             of IO1 */
13752 /* MAXIT =   Input value of NIT */
13753 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13754 /*             respectively */
13755 /* NNA =     Local copy of NA */
13756 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13757 /*             optimization loop */
13758 
13759     /* Parameter adjustments */
13760     --x;
13761     --y;
13762     --z__;
13763     iwk -= 3;
13764     --list;
13765     --lptr;
13766     --lend;
13767 
13768     /* Function Body */
13769     nna = *na;
13770     maxit = *nit;
13771     if (nna < 0 || maxit < 1) {
13772         goto L7;
13773     }
13774 
13775 /* Initialize iteration count ITER and test for NA = 0. */
13776 
13777     iter = 0;
13778     if (nna == 0) {
13779         goto L5;
13780     }
13781 
13782 /* Top of loop -- */
13783 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13784 
13785 L1:
13786     if (iter == maxit) {
13787         goto L6;
13788     }
13789     ++iter;
13790     swp = FALSE_;
13791 
13792 /*   Inner loop on arcs IO1-IO2 -- */
13793 
13794     i__1 = nna;
13795     for (i__ = 1; i__ <= i__1; ++i__) {
13796         io1 = iwk[(i__ << 1) + 1];
13797         io2 = iwk[(i__ << 1) + 2];
13798 
13799 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13800 /*     IO2->IO1, respectively.  Determine the following: */
13801 
13802 /*     LPL = pointer to the last neighbor of IO1, */
13803 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13804 /*     LPP = pointer to the node N2 preceding IO2. */
13805 
13806         lpl = lend[io1];
13807         lpp = lpl;
13808         lp = lptr[lpp];
13809 L2:
13810         if (list[lp] == io2) {
13811             goto L3;
13812         }
13813         lpp = lp;
13814         lp = lptr[lpp];
13815         if (lp != lpl) {
13816             goto L2;
13817         }
13818 
13819 /*   IO2 should be the last neighbor of IO1.  Test for no */
13820 /*     arc and bypass the swap test if IO1 is a boundary */
13821 /*     node. */
13822 
13823         if ((i__2 = list[lp], abs(i__2)) != io2) {
13824             goto L8;
13825         }
13826         if (list[lp] < 0) {
13827             goto L4;
13828         }
13829 
13830 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13831 /*     boundary node and IO2 is its first neighbor. */
13832 
13833 L3:
13834         n2 = list[lpp];
13835         if (n2 < 0) {
13836             goto L4;
13837         }
13838         lp = lptr[lp];
13839         n1 = (i__2 = list[lp], abs(i__2));
13840 
13841 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13842 
13843         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13844             goto L4;
13845         }
13846         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13847         if (lp21 == 0) {
13848             goto L9;
13849         }
13850         swp = TRUE_;
13851         iwk[(i__ << 1) + 1] = n1;
13852         iwk[(i__ << 1) + 2] = n2;
13853 L4:
13854         ;
13855     }
13856     if (swp) {
13857         goto L1;
13858     }
13859 
13860 /* Successful termination. */
13861 
13862 L5:
13863     *nit = iter;
13864     *ier = 0;
13865     return 0;
13866 
13867 /* MAXIT iterations performed without convergence. */
13868 
13869 L6:
13870     *nit = maxit;
13871     *ier = 1;
13872     return 0;
13873 
13874 /* Invalid input parameter. */
13875 
13876 L7:
13877     *nit = 0;
13878     *ier = 2;
13879     return 0;
13880 
13881 /* IO2 is not a neighbor of IO1. */
13882 
13883 L8:
13884     *nit = iter;
13885     *ier = 3;
13886     return 0;
13887 
13888 /* Zero pointer returned by SWAP. */
13889 
13890 L9:
13891     *nit = iter;
13892     *ier = 4;
13893     return 0;
13894 } /* optim_ */
13895 
13896 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13897         double *ox, double *oy, double *oz, double *ex,
13898         double *ey, double *ez, double *vx, double *vy,
13899         double *vz, long int *init, double *x, double *y,
13900         double *z__, int *ier)
13901 {
13902     /* Builtin functions */
13903     //double sqrt(double);
13904 
13905     /* Local variables */
13906     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13907             oes, xoe, yoe, zoe, xep, yep, zep;
13908 
13909 
13910 /* *********************************************************** */
13911 
13912 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13913 /*                                            Robert J. Renka */
13914 /*                                  Dept. of Computer Science */
13915 /*                                       Univ. of North Texas */
13916 /*                                           renka@cs.unt.edu */
13917 /*                                                   07/18/90 */
13918 
13919 /*   Given a projection plane and associated coordinate sys- */
13920 /* tem defined by an origin O, eye position E, and up-vector */
13921 /* V, this subroutine applies a perspective depth transform- */
13922 /* ation T to a point P = (PX,PY,PZ), returning the point */
13923 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13924 /* coordinates of the point that lies in the projection */
13925 /* plane and on the line defined by P and E, and Z is the */
13926 /* depth associated with P. */
13927 
13928 /*   The projection plane is defined to be the plane that */
13929 /* contains O and has normal defined by O and E. */
13930 
13931 /*   The depth Z is defined in such a way that Z < 1, T maps */
13932 /* lines to lines (and planes to planes), and if two distinct */
13933 /* points have the same projection plane coordinates, then */
13934 /* the one closer to E has a smaller depth.  (Z increases */
13935 /* monotonically with orthogonal distance from P to the plane */
13936 /* that is parallel to the projection plane and contains E.) */
13937 /* This depth value facilitates depth sorting and depth buf- */
13938 /* fer methods. */
13939 
13940 
13941 /* On input: */
13942 
13943 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13944 /*                  be mapped onto the projection plane.  The */
13945 /*                  half line that contains P and has end- */
13946 /*                  point at E must intersect the plane. */
13947 
13948 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13949 /*                  nate system in the projection plane).  A */
13950 /*                  reasonable value for O is a point near */
13951 /*                  the center of an object or scene to be */
13952 /*                  viewed. */
13953 
13954 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13955 /*                  ing the normal to the plane and the line */
13956 /*                  of sight for the projection.  E must not */
13957 /*                  coincide with O or P, and the angle be- */
13958 /*                  tween the vectors O-E and P-E must be */
13959 /*                  less than 90 degrees.  Note that E and P */
13960 /*                  may lie on opposite sides of the projec- */
13961 /*                  tion plane. */
13962 
13963 /*       VX,VY,VZ = Coordinates of a point V which defines */
13964 /*                  the positive Y axis of an X-Y coordinate */
13965 /*                  system in the projection plane as the */
13966 /*                  half-line containing O and the projection */
13967 /*                  of O+V onto the plane.  The positive X */
13968 /*                  axis has direction defined by the cross */
13969 /*                  product V X (E-O). */
13970 
13971 /* The above parameters are not altered by this routine. */
13972 
13973 /*       INIT = long int switch which must be set to TRUE on */
13974 /*              the first call and when the values of O, E, */
13975 /*              or V have been altered since a previous call. */
13976 /*              If INIT = FALSE, it is assumed that only the */
13977 /*              coordinates of P have changed since a previ- */
13978 /*              ous call.  Previously stored quantities are */
13979 /*              used for increased efficiency in this case. */
13980 
13981 /* On output: */
13982 
13983 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13984 
13985 /*       X,Y = Projection plane coordinates of the point */
13986 /*             that lies in the projection plane and on the */
13987 /*             line defined by E and P.  X and Y are not */
13988 /*             altered if IER .NE. 0. */
13989 
13990 /*       Z = Depth value defined above unless IER .NE. 0. */
13991 
13992 /*       IER = Error indicator. */
13993 /*             IER = 0 if no errors were encountered. */
13994 /*             IER = 1 if the inner product of O-E with P-E */
13995 /*                     is not positive, implying that E is */
13996 /*                     too close to the plane. */
13997 /*             IER = 2 if O, E, and O+V are collinear.  See */
13998 /*                     the description of VX,VY,VZ. */
13999 
14000 /* Modules required by PROJCT:  None */
14001 
14002 /* Intrinsic function called by PROJCT:  SQRT */
14003 
14004 /* *********************************************************** */
14005 
14006 
14007 /* Local parameters: */
14008 
14009 /* OES =         Norm squared of OE -- inner product (OE,OE) */
14010 /* S =           Scale factor for computing projections */
14011 /* SC =          Scale factor for normalizing VN and HN */
14012 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
14013 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
14014 /* XH,YH,ZH =    Components of a unit vector HN defining the */
14015 /*                 positive X-axis in the plane */
14016 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
14017 /* XV,YV,ZV =    Components of a unit vector VN defining the */
14018 /*                 positive Y-axis in the plane */
14019 /* XW,YW,ZW =    Components of the vector W from O to the */
14020 /*                 projection of P onto the plane */
14021 
14022     if (*init) {
14023 
14024 /* Compute parameters defining the transformation: */
14025 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
14026 /*   2 square roots. */
14027 
14028 /* Set the coordinates of E to local variables, compute */
14029 /*   OE = E-O and OES, and test for OE = 0. */
14030 
14031         xe = *ex;
14032         ye = *ey;
14033         ze = *ez;
14034         xoe = xe - *ox;
14035         yoe = ye - *oy;
14036         zoe = ze - *oz;
14037         oes = xoe * xoe + yoe * yoe + zoe * zoe;
14038         if (oes == 0.) {
14039             goto L1;
14040         }
14041 
14042 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
14043 
14044         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
14045         xv = *vx - s * xoe;
14046         yv = *vy - s * yoe;
14047         zv = *vz - s * zoe;
14048 
14049 /* Normalize VN to a unit vector. */
14050 
14051         sc = xv * xv + yv * yv + zv * zv;
14052         if (sc == 0.) {
14053             goto L2;
14054         }
14055         sc = 1. / sqrt(sc);
14056         xv = sc * xv;
14057         yv = sc * yv;
14058         zv = sc * zv;
14059 
14060 /* Compute HN = VN X OE (normalized). */
14061 
14062         xh = yv * zoe - yoe * zv;
14063         yh = xoe * zv - xv * zoe;
14064         zh = xv * yoe - xoe * yv;
14065         sc = sqrt(xh * xh + yh * yh + zh * zh);
14066         if (sc == 0.) {
14067             goto L2;
14068         }
14069         sc = 1. / sc;
14070         xh = sc * xh;
14071         yh = sc * yh;
14072         zh = sc * zh;
14073     }
14074 
14075 /* Apply the transformation:  13 adds, 12 multiplies, */
14076 /*                            1 divide, and 1 compare. */
14077 
14078 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
14079 
14080     xep = *px - xe;
14081     yep = *py - ye;
14082     zep = *pz - ze;
14083     s = xoe * xep + yoe * yep + zoe * zep;
14084     if (s >= 0.) {
14085         goto L1;
14086     }
14087     s = oes / s;
14088     xw = xoe - s * xep;
14089     yw = yoe - s * yep;
14090     zw = zoe - s * zep;
14091 
14092 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
14093 /*   reset INIT. */
14094 
14095     *x = xw * xh + yw * yh + zw * zh;
14096     *y = xw * xv + yw * yv + zw * zv;
14097     *z__ = s + 1.;
14098     *init = FALSE_;
14099     *ier = 0;
14100     return 0;
14101 
14102 /* (OE,EP) .GE. 0. */
14103 
14104 L1:
14105     *ier = 1;
14106     return 0;
14107 
14108 /* O, E, and O+V are collinear. */
14109 
14110 L2:
14111     *ier = 2;
14112     return 0;
14113 } /* projct_ */
14114 
14115 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
14116         double *plat, double *plon, double *pnrm)
14117 {
14118     /* Builtin functions */
14119     //double sqrt(double), atan2(double, double), asin(double);
14120 
14121 
14122 /* *********************************************************** */
14123 
14124 /*                                              From STRIPACK */
14125 /*                                            Robert J. Renka */
14126 /*                                  Dept. of Computer Science */
14127 /*                                       Univ. of North Texas */
14128 /*                                           renka@cs.unt.edu */
14129 /*                                                   08/27/90 */
14130 
14131 /*   This subroutine converts a point P from Cartesian coor- */
14132 /* dinates to spherical coordinates. */
14133 
14134 
14135 /* On input: */
14136 
14137 /*       PX,PY,PZ = Cartesian coordinates of P. */
14138 
14139 /* Input parameters are not altered by this routine. */
14140 
14141 /* On output: */
14142 
14143 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
14144 /*              0 if PNRM = 0.  PLAT should be scaled by */
14145 /*              180/PI to obtain the value in degrees. */
14146 
14147 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
14148 /*              if P lies on the Z-axis.  PLON should be */
14149 /*              scaled by 180/PI to obtain the value in */
14150 /*              degrees. */
14151 
14152 /*       PNRM = Magnitude (Euclidean norm) of P. */
14153 
14154 /* Modules required by SCOORD:  None */
14155 
14156 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
14157 
14158 /* *********************************************************** */
14159 
14160     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14161     if (*px != 0. || *py != 0.) {
14162         *plon = atan2(*py, *px);
14163     } else {
14164         *plon = 0.;
14165     }
14166     if (*pnrm != 0.) {
14167         *plat = asin(*pz / *pnrm);
14168     } else {
14169         *plat = 0.;
14170     }
14171     return 0;
14172 } /* scoord_ */
14173 
14174 double store_(double *x)
14175 {
14176     /* System generated locals */
14177     double ret_val;
14178 
14179 
14180 /* *********************************************************** */
14181 
14182 /*                                              From STRIPACK */
14183 /*                                            Robert J. Renka */
14184 /*                                  Dept. of Computer Science */
14185 /*                                       Univ. of North Texas */
14186 /*                                           renka@cs.unt.edu */
14187 /*                                                   05/09/92 */
14188 
14189 /*   This function forces its argument X to be stored in a */
14190 /* memory location, thus providing a means of determining */
14191 /* floating point number characteristics (such as the machine */
14192 /* precision) when it is necessary to avoid computation in */
14193 /* high precision registers. */
14194 
14195 
14196 /* On input: */
14197 
14198 /*       X = Value to be stored. */
14199 
14200 /* X is not altered by this function. */
14201 
14202 /* On output: */
14203 
14204 /*       STORE = Value of X after it has been stored and */
14205 /*               possibly truncated or rounded to the single */
14206 /*               precision word length. */
14207 
14208 /* Modules required by STORE:  None */
14209 
14210 /* *********************************************************** */
14211 
14212     stcom_1.y = *x;
14213     ret_val = stcom_1.y;
14214     return ret_val;
14215 } /* store_ */
14216 
14217 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
14218         io2, int *list, int *lptr, int *lend, int *lp21)
14219 {
14220     /* System generated locals */
14221     int i__1;
14222 
14223     /* Local variables */
14224     static int lp, lph, lpsav;
14225     int lstptr_(int *, int *, int *, int *);
14226 
14227 
14228 /* *********************************************************** */
14229 
14230 /*                                              From STRIPACK */
14231 /*                                            Robert J. Renka */
14232 /*                                  Dept. of Computer Science */
14233 /*                                       Univ. of North Texas */
14234 /*                                           renka@cs.unt.edu */
14235 /*                                                   06/22/98 */
14236 
14237 /*   Given a triangulation of a set of points on the unit */
14238 /* sphere, this subroutine replaces a diagonal arc in a */
14239 /* strictly convex quadrilateral (defined by a pair of adja- */
14240 /* cent triangles) with the other diagonal.  Equivalently, a */
14241 /* pair of adjacent triangles is replaced by another pair */
14242 /* having the same union. */
14243 
14244 
14245 /* On input: */
14246 
14247 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14248 /*                         the quadrilateral.  IO1-IO2 is re- */
14249 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14250 /*                         and (IO2,IO1,IN2) must be trian- */
14251 /*                         gles on input. */
14252 
14253 /* The above parameters are not altered by this routine. */
14254 
14255 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14256 /*                        gulation.  Refer to Subroutine */
14257 /*                        TRMESH. */
14258 
14259 /* On output: */
14260 
14261 /*       LIST,LPTR,LEND = Data structure updated with the */
14262 /*                        swap -- triangles (IO1,IO2,IN1) and */
14263 /*                        (IO2,IO1,IN2) are replaced by */
14264 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14265 /*                        unless LP21 = 0. */
14266 
14267 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14268 /*              swap is performed unless IN1 and IN2 are */
14269 /*              adjacent on input, in which case LP21 = 0. */
14270 
14271 /* Module required by SWAP:  LSTPTR */
14272 
14273 /* Intrinsic function called by SWAP:  ABS */
14274 
14275 /* *********************************************************** */
14276 
14277 
14278 /* Local parameters: */
14279 
14280 /* LP,LPH,LPSAV = LIST pointers */
14281 
14282 
14283 /* Test for IN1 and IN2 adjacent. */
14284 
14285     /* Parameter adjustments */
14286     --lend;
14287     --lptr;
14288     --list;
14289 
14290     /* Function Body */
14291     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14292     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14293         *lp21 = 0;
14294         return 0;
14295     }
14296 
14297 /* Delete IO2 as a neighbor of IO1. */
14298 
14299     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14300     lph = lptr[lp];
14301     lptr[lp] = lptr[lph];
14302 
14303 /* If IO2 is the last neighbor of IO1, make IN2 the */
14304 /*   last neighbor. */
14305 
14306     if (lend[*io1] == lph) {
14307         lend[*io1] = lp;
14308     }
14309 
14310 /* Insert IN2 as a neighbor of IN1 following IO1 */
14311 /*   using the hole created above. */
14312 
14313     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14314     lpsav = lptr[lp];
14315     lptr[lp] = lph;
14316     list[lph] = *in2;
14317     lptr[lph] = lpsav;
14318 
14319 /* Delete IO1 as a neighbor of IO2. */
14320 
14321     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14322     lph = lptr[lp];
14323     lptr[lp] = lptr[lph];
14324 
14325 /* If IO1 is the last neighbor of IO2, make IN1 the */
14326 /*   last neighbor. */
14327 
14328     if (lend[*io2] == lph) {
14329         lend[*io2] = lp;
14330     }
14331 
14332 /* Insert IN1 as a neighbor of IN2 following IO2. */
14333 
14334     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14335     lpsav = lptr[lp];
14336     lptr[lp] = lph;
14337     list[lph] = *in1;
14338     lptr[lph] = lpsav;
14339     *lp21 = lph;
14340     return 0;
14341 } /* swap_ */
14342 
14343 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14344         double *x, double *y, double *z__)
14345 {
14346     /* System generated locals */
14347     long int ret_val;
14348 
14349     /* Local variables */
14350     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14351 
14352 
14353 /* *********************************************************** */
14354 
14355 /*                                              From STRIPACK */
14356 /*                                            Robert J. Renka */
14357 /*                                  Dept. of Computer Science */
14358 /*                                       Univ. of North Texas */
14359 /*                                           renka@cs.unt.edu */
14360 /*                                                   03/29/91 */
14361 
14362 /*   This function decides whether or not to replace a */
14363 /* diagonal arc in a quadrilateral with the other diagonal. */
14364 /* The decision will be to swap (SWPTST = TRUE) if and only */
14365 /* if N4 lies above the plane (in the half-space not contain- */
14366 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14367 /* the projection of N4 onto this plane is interior to the */
14368 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14369 /* swap if the quadrilateral is not strictly convex. */
14370 
14371 
14372 /* On input: */
14373 
14374 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14375 /*                     quadrilateral with N1 adjacent to N2, */
14376 /*                     and (N1,N2,N3) in counterclockwise */
14377 /*                     order.  The arc connecting N1 to N2 */
14378 /*                     should be replaced by an arc connec- */
14379 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14380 /*                     to Subroutine SWAP. */
14381 
14382 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14383 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14384 /*               define node I for I = N1, N2, N3, and N4. */
14385 
14386 /* Input parameters are not altered by this routine. */
14387 
14388 /* On output: */
14389 
14390 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14391 /*                and N2 should be swapped for an arc con- */
14392 /*                necting N3 and N4. */
14393 
14394 /* Modules required by SWPTST:  None */
14395 
14396 /* *********************************************************** */
14397 
14398 
14399 /* Local parameters: */
14400 
14401 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14402 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14403 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14404 /* X4,Y4,Z4 =    Coordinates of N4 */
14405 
14406     /* Parameter adjustments */
14407     --z__;
14408     --y;
14409     --x;
14410 
14411     /* Function Body */
14412     x4 = x[*n4];
14413     y4 = y[*n4];
14414     z4 = z__[*n4];
14415     dx1 = x[*n1] - x4;
14416     dx2 = x[*n2] - x4;
14417     dx3 = x[*n3] - x4;
14418     dy1 = y[*n1] - y4;
14419     dy2 = y[*n2] - y4;
14420     dy3 = y[*n3] - y4;
14421     dz1 = z__[*n1] - z4;
14422     dz2 = z__[*n2] - z4;
14423     dz3 = z__[*n3] - z4;
14424 
14425 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14426 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14427 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14428 
14429     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14430             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14431     return ret_val;
14432 } /* swptst_ */
14433 
14434 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14435         double *x, double *y, double *z__)
14436 {
14437     /* System generated locals */
14438     int i__1;
14439 
14440     /* Builtin functions */
14441     //double cos(double), sin(double);
14442 
14443     /* Local variables */
14444     static int i__, nn;
14445     static double phi, theta, cosphi;
14446 
14447 
14448 /* *********************************************************** */
14449 
14450 /*                                              From STRIPACK */
14451 /*                                            Robert J. Renka */
14452 /*                                  Dept. of Computer Science */
14453 /*                                       Univ. of North Texas */
14454 /*                                           renka@cs.unt.edu */
14455 /*                                                   04/08/90 */
14456 
14457 /*   This subroutine transforms spherical coordinates into */
14458 /* Cartesian coordinates on the unit sphere for input to */
14459 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14460 /* storage for RLAT and RLON if the latter need not be saved. */
14461 
14462 
14463 /* On input: */
14464 
14465 /*       N = Number of nodes (points on the unit sphere) */
14466 /*           whose coordinates are to be transformed. */
14467 
14468 /*       RLAT = Array of length N containing latitudinal */
14469 /*              coordinates of the nodes in radians. */
14470 
14471 /*       RLON = Array of length N containing longitudinal */
14472 /*              coordinates of the nodes in radians. */
14473 
14474 /* The above parameters are not altered by this routine. */
14475 
14476 /*       X,Y,Z = Arrays of length at least N. */
14477 
14478 /* On output: */
14479 
14480 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14481 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14482 /*               to N. */
14483 
14484 /* Modules required by TRANS:  None */
14485 
14486 /* Intrinsic functions called by TRANS:  COS, SIN */
14487 
14488 /* *********************************************************** */
14489 
14490 
14491 /* Local parameters: */
14492 
14493 /* COSPHI = cos(PHI) */
14494 /* I =      DO-loop index */
14495 /* NN =     Local copy of N */
14496 /* PHI =    Latitude */
14497 /* THETA =  Longitude */
14498 
14499     /* Parameter adjustments */
14500     --z__;
14501     --y;
14502     --x;
14503     --rlon;
14504     --rlat;
14505 
14506     /* Function Body */
14507     nn = *n;
14508     i__1 = nn;
14509     for (i__ = 1; i__ <= i__1; ++i__) {
14510         phi = rlat[i__];
14511         theta = rlon[i__];
14512         cosphi = cos(phi);
14513         x[i__] = cosphi * cos(theta);
14514         y[i__] = cosphi * sin(theta);
14515         z__[i__] = sin(phi);
14516 /* L1: */
14517     }
14518     return 0;
14519 } /* trans_ */
14520 
14521 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14522         double *x, double *y, double *z__, int *list, int
14523         *lptr, int *lend, double *b1, double *b2, double *b3,
14524         int *i1, int *i2, int *i3)
14525 {
14526     /* Initialized data */
14527 
14528     static int ix = 1;
14529     static int iy = 2;
14530     static int iz = 3;
14531 
14532     /* System generated locals */
14533     int i__1;
14534     double d__1, d__2;
14535 
14536     /* Local variables */
14537     static double q[3];
14538     static int n0, n1, n2, n3, n4, nf;
14539     static double s12;
14540     static int nl, lp;
14541     static double xp, yp, zp;
14542     static int n1s, n2s;
14543     static double eps, tol, ptn1, ptn2;
14544     static int next;
14545     int jrand_(int *, int *, int *, int *);
14546     double store_(double *);
14547     int lstptr_(int *, int *, int *, int *);
14548 
14549 
14550 /* *********************************************************** */
14551 
14552 /*                                              From STRIPACK */
14553 /*                                            Robert J. Renka */
14554 /*                                  Dept. of Computer Science */
14555 /*                                       Univ. of North Texas */
14556 /*                                           renka@cs.unt.edu */
14557 /*                                                   11/30/99 */
14558 
14559 /*   This subroutine locates a point P relative to a triangu- */
14560 /* lation created by Subroutine TRMESH.  If P is contained in */
14561 /* a triangle, the three vertex indexes and barycentric coor- */
14562 /* dinates are returned.  Otherwise, the indexes of the */
14563 /* visible boundary nodes are returned. */
14564 
14565 
14566 /* On input: */
14567 
14568 /*       NST = Index of a node at which TRFIND begins its */
14569 /*             search.  Search time depends on the proximity */
14570 /*             of this node to P. */
14571 
14572 /*       P = Array of length 3 containing the x, y, and z */
14573 /*           coordinates (in that order) of the point P to be */
14574 /*           located. */
14575 
14576 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14577 
14578 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14579 /*               coordinates of the triangulation nodes (unit */
14580 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14581 /*               for I = 1 to N. */
14582 
14583 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14584 /*                        gulation.  Refer to Subroutine */
14585 /*                        TRMESH. */
14586 
14587 /* Input parameters are not altered by this routine. */
14588 
14589 /* On output: */
14590 
14591 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14592 /*                  the central projection of P onto the un- */
14593 /*                  derlying planar triangle if P is in the */
14594 /*                  convex hull of the nodes.  These parame- */
14595 /*                  ters are not altered if I1 = 0. */
14596 
14597 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14598 /*                  of a triangle containing P if P is con- */
14599 /*                  tained in a triangle.  If P is not in the */
14600 /*                  convex hull of the nodes, I1 and I2 are */
14601 /*                  the rightmost and leftmost (boundary) */
14602 /*                  nodes that are visible from P, and */
14603 /*                  I3 = 0.  (If all boundary nodes are vis- */
14604 /*                  ible from P, then I1 and I2 coincide.) */
14605 /*                  I1 = I2 = I3 = 0 if P and all of the */
14606 /*                  nodes are coplanar (lie on a common great */
14607 /*                  circle. */
14608 
14609 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14610 
14611 /* Intrinsic function called by TRFIND:  ABS */
14612 
14613 /* *********************************************************** */
14614 
14615 
14616     /* Parameter adjustments */
14617     --p;
14618     --lend;
14619     --z__;
14620     --y;
14621     --x;
14622     --list;
14623     --lptr;
14624 
14625     /* Function Body */
14626 
14627 /* Local parameters: */
14628 
14629 /* EPS =      Machine precision */
14630 /* IX,IY,IZ = int seeds for JRAND */
14631 /* LP =       LIST pointer */
14632 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14633 /*              cone (with vertex N0) containing P, or end- */
14634 /*              points of a boundary edge such that P Right */
14635 /*              N1->N2 */
14636 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14637 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14638 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14639 /* NF,NL =    First and last neighbors of N0, or first */
14640 /*              (rightmost) and last (leftmost) nodes */
14641 /*              visible from P when P is exterior to the */
14642 /*              triangulation */
14643 /* PTN1 =     Scalar product <P,N1> */
14644 /* PTN2 =     Scalar product <P,N2> */
14645 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14646 /*              the boundary traversal when P is exterior */
14647 /* S12 =      Scalar product <N1,N2> */
14648 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14649 /*              bound on the magnitude of a negative bary- */
14650 /*              centric coordinate (B1 or B2) for P in a */
14651 /*              triangle -- used to avoid an infinite number */
14652 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14653 /*              B2 < 0 but small in magnitude */
14654 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14655 /* X0,Y0,Z0 = Dummy arguments for DET */
14656 /* X1,Y1,Z1 = Dummy arguments for DET */
14657 /* X2,Y2,Z2 = Dummy arguments for DET */
14658 
14659 /* Statement function: */
14660 
14661 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14662 /*                       (closed) left hemisphere defined by */
14663 /*                       the plane containing (0,0,0), */
14664 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14665 /*                       left is defined relative to an ob- */
14666 /*                       server at (X1,Y1,Z1) facing */
14667 /*                       (X2,Y2,Z2). */
14668 
14669 
14670 /* Initialize variables. */
14671 
14672     xp = p[1];
14673     yp = p[2];
14674     zp = p[3];
14675     n0 = *nst;
14676     if (n0 < 1 || n0 > *n) {
14677         n0 = jrand_(n, &ix, &iy, &iz);
14678     }
14679 
14680 /* Compute the relative machine precision EPS and TOL. */
14681 
14682     eps = 1.;
14683 L1:
14684     eps /= 2.;
14685     d__1 = eps + 1.;
14686     if (store_(&d__1) > 1.) {
14687         goto L1;
14688     }
14689     eps *= 2.;
14690     tol = eps * 4.;
14691 
14692 /* Set NF and NL to the first and last neighbors of N0, and */
14693 /*   initialize N1 = NF. */
14694 
14695 L2:
14696     lp = lend[n0];
14697     nl = list[lp];
14698     lp = lptr[lp];
14699     nf = list[lp];
14700     n1 = nf;
14701 
14702 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14703 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14704 
14705     if (nl > 0) {
14706 
14707 /*   N0 is an interior node.  Find N1. */
14708 
14709 L3:
14710         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14711                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14712                 -1e-10) {
14713             lp = lptr[lp];
14714             n1 = list[lp];
14715             if (n1 == nl) {
14716                 goto L6;
14717             }
14718             goto L3;
14719         }
14720     } else {
14721 
14722 /*   N0 is a boundary node.  Test for P exterior. */
14723 
14724         nl = -nl;
14725         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14726                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14727                 -1e-10) {
14728 
14729 /*   P is to the right of the boundary edge N0->NF. */
14730 
14731             n1 = n0;
14732             n2 = nf;
14733             goto L9;
14734         }
14735         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14736                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14737                 -1e-10) {
14738 
14739 /*   P is to the right of the boundary edge NL->N0. */
14740 
14741             n1 = nl;
14742             n2 = n0;
14743             goto L9;
14744         }
14745     }
14746 
14747 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14748 /*   next neighbor of N0 (following N1). */
14749 
14750 L4:
14751     lp = lptr[lp];
14752     n2 = (i__1 = list[lp], abs(i__1));
14753     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14754             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14755         goto L7;
14756     }
14757     n1 = n2;
14758     if (n1 != nl) {
14759         goto L4;
14760     }
14761     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14762             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14763         goto L6;
14764     }
14765 
14766 /* P is left of or on arcs N0->NB for all neighbors NB */
14767 /*   of N0.  Test for P = +/-N0. */
14768 
14769     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14770     if (store_(&d__2) < 1. - eps * 4.) {
14771 
14772 /*   All points are collinear iff P Left NB->N0 for all */
14773 /*     neighbors NB of N0.  Search the neighbors of N0. */
14774 /*     Note:  N1 = NL and LP points to NL. */
14775 
14776 L5:
14777         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14778                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14779                 -1e-10) {
14780             lp = lptr[lp];
14781             n1 = (i__1 = list[lp], abs(i__1));
14782             if (n1 == nl) {
14783                 goto L14;
14784             }
14785             goto L5;
14786         }
14787     }
14788 
14789 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14790 /*   and start over. */
14791 
14792     n0 = n1;
14793     goto L2;
14794 
14795 /* P is between arcs N0->N1 and N0->NF. */
14796 
14797 L6:
14798     n2 = nf;
14799 
14800 /* P is contained in a wedge defined by geodesics N0-N1 and */
14801 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14802 /*   test for cycling. */
14803 
14804 L7:
14805     n3 = n0;
14806     n1s = n1;
14807     n2s = n2;
14808 
14809 /* Top of edge-hopping loop: */
14810 
14811 L8:
14812 
14813     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14814             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14815      if (*b3 < -1e-10) {
14816 
14817 /*   Set N4 to the first neighbor of N2 following N1 (the */
14818 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14819 
14820         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14821         if (list[lp] < 0) {
14822             goto L9;
14823         }
14824         lp = lptr[lp];
14825         n4 = (i__1 = list[lp], abs(i__1));
14826 
14827 /*   Define a new arc N1->N2 which intersects the geodesic */
14828 /*     N0-P. */
14829         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14830                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14831                 -1e-10) {
14832             n3 = n2;
14833             n2 = n4;
14834             n1s = n1;
14835             if (n2 != n2s && n2 != n0) {
14836                 goto L8;
14837             }
14838         } else {
14839             n3 = n1;
14840             n1 = n4;
14841             n2s = n2;
14842             if (n1 != n1s && n1 != n0) {
14843                 goto L8;
14844             }
14845         }
14846 
14847 /*   The starting node N0 or edge N1-N2 was encountered */
14848 /*     again, implying a cycle (infinite loop).  Restart */
14849 /*     with N0 randomly selected. */
14850 
14851         n0 = jrand_(n, &ix, &iy, &iz);
14852         goto L2;
14853     }
14854 
14855 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14856 /*   or P is close to -N0. */
14857 
14858     if (*b3 >= eps) {
14859 
14860 /*   B3 .NE. 0. */
14861 
14862         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14863                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14864         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14865                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14866         if (*b1 < -tol || *b2 < -tol) {
14867 
14868 /*   Restart with N0 randomly selected. */
14869 
14870             n0 = jrand_(n, &ix, &iy, &iz);
14871             goto L2;
14872         }
14873     } else {
14874 
14875 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14876 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14877 
14878         *b3 = 0.;
14879         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14880         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14881         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14882         *b1 = ptn1 - s12 * ptn2;
14883         *b2 = ptn2 - s12 * ptn1;
14884         if (*b1 < -tol || *b2 < -tol) {
14885 
14886 /*   Restart with N0 randomly selected. */
14887 
14888             n0 = jrand_(n, &ix, &iy, &iz);
14889             goto L2;
14890         }
14891     }
14892 
14893 /* P is in (N1,N2,N3). */
14894 
14895     *i1 = n1;
14896     *i2 = n2;
14897     *i3 = n3;
14898     if (*b1 < 0.f) {
14899         *b1 = 0.f;
14900     }
14901     if (*b2 < 0.f) {
14902         *b2 = 0.f;
14903     }
14904     return 0;
14905 
14906 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14907 /*   Save N1 and N2, and set NL = 0 to indicate that */
14908 /*   NL has not yet been found. */
14909 
14910 L9:
14911     n1s = n1;
14912     n2s = n2;
14913     nl = 0;
14914 
14915 /*           Counterclockwise Boundary Traversal: */
14916 
14917 L10:
14918 
14919     lp = lend[n2];
14920     lp = lptr[lp];
14921     next = list[lp];
14922      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14923              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14924             >= -1e-10) {
14925 
14926 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14927 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14928 
14929         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14930         q[0] = x[n1] - s12 * x[n2];
14931         q[1] = y[n1] - s12 * y[n2];
14932         q[2] = z__[n1] - s12 * z__[n2];
14933         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14934             goto L11;
14935         }
14936         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14937             goto L11;
14938         }
14939 
14940 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14941 /*     the leftmost visible node. */
14942 
14943         nl = n2;
14944     }
14945 
14946 /* Bottom of counterclockwise loop: */
14947 
14948     n1 = n2;
14949     n2 = next;
14950     if (n2 != n1s) {
14951         goto L10;
14952     }
14953 
14954 /* All boundary nodes are visible from P. */
14955 
14956     *i1 = n1s;
14957     *i2 = n1s;
14958     *i3 = 0;
14959     return 0;
14960 
14961 /* N2 is the rightmost visible node. */
14962 
14963 L11:
14964     nf = n2;
14965     if (nl == 0) {
14966 
14967 /* Restore initial values of N1 and N2, and begin the search */
14968 /*   for the leftmost visible node. */
14969 
14970         n2 = n2s;
14971         n1 = n1s;
14972 
14973 /*           Clockwise Boundary Traversal: */
14974 
14975 L12:
14976         lp = lend[n1];
14977         next = -list[lp];
14978         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14979                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14980                  y[next]) >= -1e-10) {
14981 
14982 /*   N1 is the leftmost visible node if P or NEXT is */
14983 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14984 
14985             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14986             q[0] = x[n2] - s12 * x[n1];
14987             q[1] = y[n2] - s12 * y[n1];
14988             q[2] = z__[n2] - s12 * z__[n1];
14989             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14990                 goto L13;
14991             }
14992             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14993                 goto L13;
14994             }
14995 
14996 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14997 /*     rightmost visible node. */
14998 
14999             nf = n1;
15000         }
15001 
15002 /* Bottom of clockwise loop: */
15003 
15004         n2 = n1;
15005         n1 = next;
15006         if (n1 != n1s) {
15007             goto L12;
15008         }
15009 
15010 /* All boundary nodes are visible from P. */
15011 
15012         *i1 = n1;
15013         *i2 = n1;
15014         *i3 = 0;
15015         return 0;
15016 
15017 /* N1 is the leftmost visible node. */
15018 
15019 L13:
15020         nl = n1;
15021     }
15022 
15023 /* NF and NL have been found. */
15024 
15025     *i1 = nf;
15026     *i2 = nl;
15027     *i3 = 0;
15028     return 0;
15029 
15030 /* All points are collinear (coplanar). */
15031 
15032 L14:
15033     *i1 = 0;
15034     *i2 = 0;
15035     *i3 = 0;
15036     return 0;
15037 } /* trfind_ */
15038 
15039 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
15040         int *lend, int *nrow, int *nt, int *ltri, int *
15041         ier)
15042 {
15043     /* System generated locals */
15044     int ltri_dim1, ltri_offset, i__1, i__2;
15045 
15046     /* Local variables */
15047     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
15048             lpl, isv;
15049     static long int arcs;
15050     static int lpln1;
15051 
15052 
15053 /* *********************************************************** */
15054 
15055 /*                                              From STRIPACK */
15056 /*                                            Robert J. Renka */
15057 /*                                  Dept. of Computer Science */
15058 /*                                       Univ. of North Texas */
15059 /*                                           renka@cs.unt.edu */
15060 /*                                                   07/20/96 */
15061 
15062 /*   This subroutine converts a triangulation data structure */
15063 /* from the linked list created by Subroutine TRMESH to a */
15064 /* triangle list. */
15065 
15066 /* On input: */
15067 
15068 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15069 
15070 /*       LIST,LPTR,LEND = Linked list data structure defin- */
15071 /*                        ing the triangulation.  Refer to */
15072 /*                        Subroutine TRMESH. */
15073 
15074 /*       NROW = Number of rows (entries per triangle) re- */
15075 /*              served for the triangle list LTRI.  The value */
15076 /*              must be 6 if only the vertex indexes and */
15077 /*              neighboring triangle indexes are to be */
15078 /*              stored, or 9 if arc indexes are also to be */
15079 /*              assigned and stored.  Refer to LTRI. */
15080 
15081 /* The above parameters are not altered by this routine. */
15082 
15083 /*       LTRI = int array of length at least NROW*NT, */
15084 /*              where NT is at most 2N-4.  (A sufficient */
15085 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
15086 
15087 /* On output: */
15088 
15089 /*       NT = Number of triangles in the triangulation unless */
15090 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
15091 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
15092 /*            number of boundary nodes. */
15093 
15094 /*       LTRI = NROW by NT array whose J-th column contains */
15095 /*              the vertex nodal indexes (first three rows), */
15096 /*              neighboring triangle indexes (second three */
15097 /*              rows), and, if NROW = 9, arc indexes (last */
15098 /*              three rows) associated with triangle J for */
15099 /*              J = 1,...,NT.  The vertices are ordered */
15100 /*              counterclockwise with the first vertex taken */
15101 /*              to be the one with smallest index.  Thus, */
15102 /*              LTRI(2,J) and LTRI(3,J) are larger than */
15103 /*              LTRI(1,J) and index adjacent neighbors of */
15104 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
15105 /*              and LTRI(I+6,J) index the triangle and arc, */
15106 /*              respectively, which are opposite (not shared */
15107 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
15108 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
15109 /*              indexes range from 1 to N, triangle indexes */
15110 /*              from 0 to NT, and, if included, arc indexes */
15111 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
15112 /*              or 3N-6 if NB = 0.  The triangles are or- */
15113 /*              dered on first (smallest) vertex indexes. */
15114 
15115 /*       IER = Error indicator. */
15116 /*             IER = 0 if no errors were encountered. */
15117 /*             IER = 1 if N or NROW is outside its valid */
15118 /*                     range on input. */
15119 /*             IER = 2 if the triangulation data structure */
15120 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
15121 /*                     however, that these arrays are not */
15122 /*                     completely tested for validity. */
15123 
15124 /* Modules required by TRLIST:  None */
15125 
15126 /* Intrinsic function called by TRLIST:  ABS */
15127 
15128 /* *********************************************************** */
15129 
15130 
15131 /* Local parameters: */
15132 
15133 /* ARCS =     long int variable with value TRUE iff are */
15134 /*              indexes are to be stored */
15135 /* I,J =      LTRI row indexes (1 to 3) associated with */
15136 /*              triangles KT and KN, respectively */
15137 /* I1,I2,I3 = Nodal indexes of triangle KN */
15138 /* ISV =      Variable used to permute indexes I1,I2,I3 */
15139 /* KA =       Arc index and number of currently stored arcs */
15140 /* KN =       Index of the triangle that shares arc I1-I2 */
15141 /*              with KT */
15142 /* KT =       Triangle index and number of currently stored */
15143 /*              triangles */
15144 /* LP =       LIST pointer */
15145 /* LP2 =      Pointer to N2 as a neighbor of N1 */
15146 /* LPL =      Pointer to the last neighbor of I1 */
15147 /* LPLN1 =    Pointer to the last neighbor of N1 */
15148 /* N1,N2,N3 = Nodal indexes of triangle KT */
15149 /* NM2 =      N-2 */
15150 
15151 
15152 /* Test for invalid input parameters. */
15153 
15154     /* Parameter adjustments */
15155     --lend;
15156     --list;
15157     --lptr;
15158     ltri_dim1 = *nrow;
15159     ltri_offset = 1 + ltri_dim1;
15160     ltri -= ltri_offset;
15161 
15162     /* Function Body */
15163     if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15164         goto L11;
15165     }
15166 
15167 /* Initialize parameters for loop on triangles KT = (N1,N2, */
15168 /*   N3), where N1 < N2 and N1 < N3. */
15169 
15170 /*   ARCS = TRUE iff arc indexes are to be stored. */
15171 /*   KA,KT = Numbers of currently stored arcs and triangles. */
15172 /*   NM2 = Upper bound on candidates for N1. */
15173 
15174     arcs = *nrow == 9;
15175     ka = 0;
15176     kt = 0;
15177     nm2 = *n - 2;
15178 
15179 /* Loop on nodes N1. */
15180 
15181     i__1 = nm2;
15182     for (n1 = 1; n1 <= i__1; ++n1) {
15183 
15184 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
15185 /*   to the last neighbor of N1, and LP2 points to N2. */
15186 
15187         lpln1 = lend[n1];
15188         lp2 = lpln1;
15189 L1:
15190         lp2 = lptr[lp2];
15191         n2 = list[lp2];
15192         lp = lptr[lp2];
15193         n3 = (i__2 = list[lp], abs(i__2));
15194         if (n2 < n1 || n3 < n1) {
15195             goto L8;
15196         }
15197 
15198 /* Add a new triangle KT = (N1,N2,N3). */
15199 
15200         ++kt;
15201         ltri[kt * ltri_dim1 + 1] = n1;
15202         ltri[kt * ltri_dim1 + 2] = n2;
15203         ltri[kt * ltri_dim1 + 3] = n3;
15204 
15205 /* Loop on triangle sides (I2,I1) with neighboring triangles */
15206 /*   KN = (I1,I2,I3). */
15207 
15208         for (i__ = 1; i__ <= 3; ++i__) {
15209             if (i__ == 1) {
15210                 i1 = n3;
15211                 i2 = n2;
15212             } else if (i__ == 2) {
15213                 i1 = n1;
15214                 i2 = n3;
15215             } else {
15216                 i1 = n2;
15217                 i2 = n1;
15218             }
15219 
15220 /* Set I3 to the neighbor of I1 that follows I2 unless */
15221 /*   I2->I1 is a boundary arc. */
15222 
15223             lpl = lend[i1];
15224             lp = lptr[lpl];
15225 L2:
15226             if (list[lp] == i2) {
15227                 goto L3;
15228             }
15229             lp = lptr[lp];
15230             if (lp != lpl) {
15231                 goto L2;
15232             }
15233 
15234 /*   I2 is the last neighbor of I1 unless the data structure */
15235 /*     is invalid.  Bypass the search for a neighboring */
15236 /*     triangle if I2->I1 is a boundary arc. */
15237 
15238             if ((i__2 = list[lp], abs(i__2)) != i2) {
15239                 goto L12;
15240             }
15241             kn = 0;
15242             if (list[lp] < 0) {
15243                 goto L6;
15244             }
15245 
15246 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15247 /*     a neighbor of I1. */
15248 
15249 L3:
15250             lp = lptr[lp];
15251             i3 = (i__2 = list[lp], abs(i__2));
15252 
15253 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15254 /*   and permute the vertex indexes of KN so that I1 is */
15255 /*   smallest. */
15256 
15257             if (i1 < i2 && i1 < i3) {
15258                 j = 3;
15259             } else if (i2 < i3) {
15260                 j = 2;
15261                 isv = i1;
15262                 i1 = i2;
15263                 i2 = i3;
15264                 i3 = isv;
15265             } else {
15266                 j = 1;
15267                 isv = i1;
15268                 i1 = i3;
15269                 i3 = i2;
15270                 i2 = isv;
15271             }
15272 
15273 /* Test for KN > KT (triangle index not yet assigned). */
15274 
15275             if (i1 > n1) {
15276                 goto L7;
15277             }
15278 
15279 /* Find KN, if it exists, by searching the triangle list in */
15280 /*   reverse order. */
15281 
15282             for (kn = kt - 1; kn >= 1; --kn) {
15283                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15284                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15285                     goto L5;
15286                 }
15287 /* L4: */
15288             }
15289             goto L7;
15290 
15291 /* Store KT as a neighbor of KN. */
15292 
15293 L5:
15294             ltri[j + 3 + kn * ltri_dim1] = kt;
15295 
15296 /* Store KN as a neighbor of KT, and add a new arc KA. */
15297 
15298 L6:
15299             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15300             if (arcs) {
15301                 ++ka;
15302                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15303                 if (kn != 0) {
15304                     ltri[j + 6 + kn * ltri_dim1] = ka;
15305                 }
15306             }
15307 L7:
15308             ;
15309         }
15310 
15311 /* Bottom of loop on triangles. */
15312 
15313 L8:
15314         if (lp2 != lpln1) {
15315             goto L1;
15316         }
15317 /* L9: */
15318     }
15319 
15320 /* No errors encountered. */
15321 
15322     *nt = kt;
15323     *ier = 0;
15324     return 0;
15325 
15326 /* Invalid input parameter. */
15327 
15328 L11:
15329     *nt = 0;
15330     *ier = 1;
15331     return 0;
15332 
15333 /* Invalid triangulation data structure:  I1 is a neighbor of */
15334 /*   I2, but I2 is not a neighbor of I1. */
15335 
15336 L12:
15337     *nt = 0;
15338     *ier = 2;
15339     return 0;
15340 } /* trlist_ */
15341 
15342 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15343         double *z__, int *iflag, int *nrow, int *nt, int *
15344         ltri, int *lout)
15345 {
15346     /* Initialized data */
15347 
15348     static int nmax = 9999;
15349     static int nlmax = 58;
15350 
15351     /* System generated locals */
15352     int ltri_dim1, ltri_offset, i__1;
15353 
15354     /* Local variables */
15355     static int i__, k, na, nb, nl, lun;
15356 
15357 
15358 /* *********************************************************** */
15359 
15360 /*                                              From STRIPACK */
15361 /*                                            Robert J. Renka */
15362 /*                                  Dept. of Computer Science */
15363 /*                                       Univ. of North Texas */
15364 /*                                           renka@cs.unt.edu */
15365 /*                                                   07/02/98 */
15366 
15367 /*   This subroutine prints the triangle list created by Sub- */
15368 /* routine TRLIST and, optionally, the nodal coordinates */
15369 /* (either latitude and longitude or Cartesian coordinates) */
15370 /* on long int unit LOUT.  The numbers of boundary nodes, */
15371 /* triangles, and arcs are also printed. */
15372 
15373 
15374 /* On input: */
15375 
15376 /*       N = Number of nodes in the triangulation. */
15377 /*           3 .LE. N .LE. 9999. */
15378 
15379 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15380 /*               coordinates of the nodes if IFLAG = 0, or */
15381 /*               (X and Y only) arrays of length N containing */
15382 /*               longitude and latitude, respectively, if */
15383 /*               IFLAG > 0, or unused dummy parameters if */
15384 /*               IFLAG < 0. */
15385 
15386 /*       IFLAG = Nodal coordinate option indicator: */
15387 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15388 /*                         Cartesian coordinates) are to be */
15389 /*                         printed (to 6 decimal places). */
15390 /*               IFLAG > 0 if only X and Y (assumed to con- */
15391 /*                         tain longitude and latitude) are */
15392 /*                         to be printed (to 6 decimal */
15393 /*                         places). */
15394 /*               IFLAG < 0 if only the adjacency lists are to */
15395 /*                         be printed. */
15396 
15397 /*       NROW = Number of rows (entries per triangle) re- */
15398 /*              served for the triangle list LTRI.  The value */
15399 /*              must be 6 if only the vertex indexes and */
15400 /*              neighboring triangle indexes are stored, or 9 */
15401 /*              if arc indexes are also stored. */
15402 
15403 /*       NT = Number of triangles in the triangulation. */
15404 /*            1 .LE. NT .LE. 9999. */
15405 
15406 /*       LTRI = NROW by NT array whose J-th column contains */
15407 /*              the vertex nodal indexes (first three rows), */
15408 /*              neighboring triangle indexes (second three */
15409 /*              rows), and, if NROW = 9, arc indexes (last */
15410 /*              three rows) associated with triangle J for */
15411 /*              J = 1,...,NT. */
15412 
15413 /*       LOUT = long int unit number for output.  If LOUT is */
15414 /*              not in the range 0 to 99, output is written */
15415 /*              to unit 6. */
15416 
15417 /* Input parameters are not altered by this routine. */
15418 
15419 /* On output: */
15420 
15421 /*   The triangle list and nodal coordinates (as specified by */
15422 /* IFLAG) are written to unit LOUT. */
15423 
15424 /* Modules required by TRLPRT:  None */
15425 
15426 /* *********************************************************** */
15427 
15428     /* Parameter adjustments */
15429     --z__;
15430     --y;
15431     --x;
15432     ltri_dim1 = *nrow;
15433     ltri_offset = 1 + ltri_dim1;
15434     ltri -= ltri_offset;
15435 
15436     /* Function Body */
15437 
15438 /* Local parameters: */
15439 
15440 /* I =     DO-loop, nodal index, and row index for LTRI */
15441 /* K =     DO-loop and triangle index */
15442 /* LUN =   long int unit number for output */
15443 /* NA =    Number of triangulation arcs */
15444 /* NB =    Number of boundary nodes */
15445 /* NL =    Number of lines printed on the current page */
15446 /* NLMAX = Maximum number of print lines per page (except */
15447 /*           for the last page which may have two addi- */
15448 /*           tional lines) */
15449 /* NMAX =  Maximum value of N and NT (4-digit format) */
15450 
15451     lun = *lout;
15452     if (lun < 0 || lun > 99) {
15453         lun = 6;
15454     }
15455 
15456 /* Print a heading and test for invalid input. */
15457 
15458 /*      WRITE (LUN,100) N */
15459     nl = 3;
15460     if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15461             nmax) {
15462 
15463 /* Print an error message and exit. */
15464 
15465 /*        WRITE (LUN,110) N, NROW, NT */
15466         return 0;
15467     }
15468     if (*iflag == 0) {
15469 
15470 /* Print X, Y, and Z. */
15471 
15472 /*        WRITE (LUN,101) */
15473         nl = 6;
15474         i__1 = *n;
15475         for (i__ = 1; i__ <= i__1; ++i__) {
15476             if (nl >= nlmax) {
15477 /*            WRITE (LUN,108) */
15478                 nl = 0;
15479             }
15480 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15481             ++nl;
15482 /* L1: */
15483         }
15484     } else if (*iflag > 0) {
15485 
15486 /* Print X (longitude) and Y (latitude). */
15487 
15488 /*        WRITE (LUN,102) */
15489         nl = 6;
15490         i__1 = *n;
15491         for (i__ = 1; i__ <= i__1; ++i__) {
15492             if (nl >= nlmax) {
15493 /*            WRITE (LUN,108) */
15494                 nl = 0;
15495             }
15496 /*          WRITE (LUN,104) I, X(I), Y(I) */
15497             ++nl;
15498 /* L2: */
15499         }
15500     }
15501 
15502 /* Print the triangulation LTRI. */
15503 
15504     if (nl > nlmax / 2) {
15505 /*        WRITE (LUN,108) */
15506         nl = 0;
15507     }
15508     if (*nrow == 6) {
15509 /*        WRITE (LUN,105) */
15510     } else {
15511 /*        WRITE (LUN,106) */
15512     }
15513     nl += 5;
15514     i__1 = *nt;
15515     for (k = 1; k <= i__1; ++k) {
15516         if (nl >= nlmax) {
15517 /*          WRITE (LUN,108) */
15518             nl = 0;
15519         }
15520 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15521         ++nl;
15522 /* L3: */
15523     }
15524 
15525 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15526 /*   triangles). */
15527 
15528     nb = (*n << 1) - *nt - 2;
15529     if (nb < 3) {
15530         nb = 0;
15531         na = *n * 3 - 6;
15532     } else {
15533         na = *nt + *n - 1;
15534     }
15535 /*      WRITE (LUN,109) NB, NA, NT */
15536     return 0;
15537 
15538 /* Print formats: */
15539 
15540 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15541 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15542 /*     .        'Z(Node)'//) */
15543 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15544 /*  103 FORMAT (8X,I4,3D17.6) */
15545 /*  104 FORMAT (16X,I4,2D17.6) */
15546 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15547 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15548 /*     .        'KT2',4X,'KT3'/) */
15549 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15550 /*     .        14X,'Arcs'/ */
15551 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15552 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15553 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15554 /*  108 FORMAT (///) */
15555 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15556 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15557 /*     .        ' Triangles') */
15558 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15559 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15560 } /* trlprt_ */
15561 
15562 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15563         double *z__, int *list, int *lptr, int *lend, int
15564         *lnew, int *near__, int *next, double *dist, int *ier)
15565 {
15566     /* System generated locals */
15567     int i__1, i__2;
15568 
15569     /* Local variables */
15570     static double d__;
15571     static int i__, j, k;
15572     static double d1, d2, d3;
15573     static int i0, lp, nn, lpl;
15574     long int left_(double *, double *, double *, double
15575             *, double *, double *, double *, double *,
15576             double *);
15577     static int nexti;
15578 
15579 
15580 /* *********************************************************** */
15581 
15582 /*                                              From STRIPACK */
15583 /*                                            Robert J. Renka */
15584 /*                                  Dept. of Computer Science */
15585 /*                                       Univ. of North Texas */
15586 /*                                           renka@cs.unt.edu */
15587 /*                                                   03/04/03 */
15588 
15589 /*   This subroutine creates a Delaunay triangulation of a */
15590 /* set of N arbitrarily distributed points, referred to as */
15591 /* nodes, on the surface of the unit sphere.  The Delaunay */
15592 /* triangulation is defined as a set of (spherical) triangles */
15593 /* with the following five properties: */
15594 
15595 /*  1)  The triangle vertices are nodes. */
15596 /*  2)  No triangle contains a node other than its vertices. */
15597 /*  3)  The interiors of the triangles are pairwise disjoint. */
15598 /*  4)  The union of triangles is the convex hull of the set */
15599 /*        of nodes (the smallest convex set that contains */
15600 /*        the nodes).  If the nodes are not contained in a */
15601 /*        single hemisphere, their convex hull is the en- */
15602 /*        tire sphere and there are no boundary nodes. */
15603 /*        Otherwise, there are at least three boundary nodes. */
15604 /*  5)  The interior of the circumcircle of each triangle */
15605 /*        contains no node. */
15606 
15607 /* The first four properties define a triangulation, and the */
15608 /* last property results in a triangulation which is as close */
15609 /* as possible to equiangular in a certain sense and which is */
15610 /* uniquely defined unless four or more nodes lie in a common */
15611 /* plane.  This property makes the triangulation well-suited */
15612 /* for solving closest-point problems and for triangle-based */
15613 /* interpolation. */
15614 
15615 /*   The algorithm has expected time complexity O(N*log(N)) */
15616 /* for most nodal distributions. */
15617 
15618 /*   Spherical coordinates (latitude and longitude) may be */
15619 /* converted to Cartesian coordinates by Subroutine TRANS. */
15620 
15621 /*   The following is a list of the software package modules */
15622 /* which a user may wish to call directly: */
15623 
15624 /*  ADDNOD - Updates the triangulation by appending a new */
15625 /*             node. */
15626 
15627 /*  AREAS  - Returns the area of a spherical triangle. */
15628 
15629 /*  AREAV  - Returns the area of a Voronoi region associated */
15630 /*           with an interior node without requiring that the */
15631 /*           entire Voronoi diagram be computed and stored. */
15632 
15633 /*  BNODES - Returns an array containing the indexes of the */
15634 /*             boundary nodes (if any) in counterclockwise */
15635 /*             order.  Counts of boundary nodes, triangles, */
15636 /*             and arcs are also returned. */
15637 
15638 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15639 /*           formly spaced points on the unit circle centered */
15640 /*           at (0,0). */
15641 
15642 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15643 /*             gle. */
15644 
15645 /*  CRLIST - Returns the set of triangle circumcenters */
15646 /*             (Voronoi vertices) and circumradii associated */
15647 /*             with a triangulation. */
15648 
15649 /*  DELARC - Deletes a boundary arc from a triangulation. */
15650 
15651 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15652 
15653 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15654 /*             ted by an arc in the triangulation. */
15655 
15656 /*  GETNP  - Determines the ordered sequence of L closest */
15657 /*             nodes to a given node, along with the associ- */
15658 /*             ated distances. */
15659 
15660 /*  INSIDE - Locates a point relative to a polygon on the */
15661 /*             surface of the sphere. */
15662 
15663 /*  INTRSC - Returns the point of intersection between a */
15664 /*             pair of great circle arcs. */
15665 
15666 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15667 /*             int. */
15668 
15669 /*  LEFT   - Locates a point relative to a great circle. */
15670 
15671 /*  NEARND - Returns the index of the nearest node to an */
15672 /*             arbitrary point, along with its squared */
15673 /*             distance. */
15674 
15675 /*  PROJCT - Applies a perspective-depth projection to a */
15676 /*             point in 3-space. */
15677 
15678 /*  SCOORD - Converts a point from Cartesian coordinates to */
15679 /*             spherical coordinates. */
15680 
15681 /*  STORE  - Forces a value to be stored in main memory so */
15682 /*             that the precision of floating point numbers */
15683 /*             in memory locations rather than registers is */
15684 /*             computed. */
15685 
15686 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15687 /*             coordinates on the unit sphere for input to */
15688 /*             Subroutine TRMESH. */
15689 
15690 /*  TRLIST - Converts the triangulation data structure to a */
15691 /*             triangle list more suitable for use in a fin- */
15692 /*             ite element code. */
15693 
15694 /*  TRLPRT - Prints the triangle list created by Subroutine */
15695 /*             TRLIST. */
15696 
15697 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15698 /*             nodes. */
15699 
15700 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15701 /*             file containing a triangulation plot. */
15702 
15703 /*  TRPRNT - Prints the triangulation data structure and, */
15704 /*             optionally, the nodal coordinates. */
15705 
15706 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15707 /*             file containing a Voronoi diagram plot. */
15708 
15709 
15710 /* On input: */
15711 
15712 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15713 
15714 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15715 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15716 /*               Z(K)) is referred to as node K, and K is re- */
15717 /*               ferred to as a nodal index.  It is required */
15718 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15719 /*               K.  The first three nodes must not be col- */
15720 /*               linear (lie on a common great circle). */
15721 
15722 /* The above parameters are not altered by this routine. */
15723 
15724 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15725 
15726 /*       LEND = Array of length at least N. */
15727 
15728 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15729 /*                        least N.  The space is used to */
15730 /*                        efficiently determine the nearest */
15731 /*                        triangulation node to each un- */
15732 /*                        processed node for use by ADDNOD. */
15733 
15734 /* On output: */
15735 
15736 /*       LIST = Set of nodal indexes which, along with LPTR, */
15737 /*              LEND, and LNEW, define the triangulation as a */
15738 /*              set of N adjacency lists -- counterclockwise- */
15739 /*              ordered sequences of neighboring nodes such */
15740 /*              that the first and last neighbors of a bound- */
15741 /*              ary node are boundary nodes (the first neigh- */
15742 /*              bor of an interior node is arbitrary).  In */
15743 /*              order to distinguish between interior and */
15744 /*              boundary nodes, the last neighbor of each */
15745 /*              boundary node is represented by the negative */
15746 /*              of its index. */
15747 
15748 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15749 /*              correspondence with the elements of LIST. */
15750 /*              LIST(LPTR(I)) indexes the node which follows */
15751 /*              LIST(I) in cyclical counterclockwise order */
15752 /*              (the first neighbor follows the last neigh- */
15753 /*              bor). */
15754 
15755 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15756 /*              points to the last neighbor of node K for */
15757 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15758 /*              only if K is a boundary node. */
15759 
15760 /*       LNEW = Pointer to the first empty location in LIST */
15761 /*              and LPTR (list length plus one).  LIST, LPTR, */
15762 /*              LEND, and LNEW are not altered if IER < 0, */
15763 /*              and are incomplete if IER > 0. */
15764 
15765 /*       NEAR,NEXT,DIST = Garbage. */
15766 
15767 /*       IER = Error indicator: */
15768 /*             IER =  0 if no errors were encountered. */
15769 /*             IER = -1 if N < 3 on input. */
15770 /*             IER = -2 if the first three nodes are */
15771 /*                      collinear. */
15772 /*             IER =  L if nodes L and M coincide for some */
15773 /*                      M > L.  The data structure represents */
15774 /*                      a triangulation of nodes 1 to M-1 in */
15775 /*                      this case. */
15776 
15777 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15778 /*                                INSERT, INTADD, JRAND, */
15779 /*                                LEFT, LSTPTR, STORE, SWAP, */
15780 /*                                SWPTST, TRFIND */
15781 
15782 /* Intrinsic function called by TRMESH:  ABS */
15783 
15784 /* *********************************************************** */
15785 
15786 
15787 /* Local parameters: */
15788 
15789 /* D =        (Negative cosine of) distance from node K to */
15790 /*              node I */
15791 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15792 /*              respectively */
15793 /* I,J =      Nodal indexes */
15794 /* I0 =       Index of the node preceding I in a sequence of */
15795 /*              unprocessed nodes:  I = NEXT(I0) */
15796 /* K =        Index of node to be added and DO-loop index: */
15797 /*              K > 3 */
15798 /* LP =       LIST index (pointer) of a neighbor of K */
15799 /* LPL =      Pointer to the last neighbor of K */
15800 /* NEXTI =    NEXT(I) */
15801 /* NN =       Local copy of N */
15802 
15803     /* Parameter adjustments */
15804     --dist;
15805     --next;
15806     --near__;
15807     --lend;
15808     --z__;
15809     --y;
15810     --x;
15811     --list;
15812     --lptr;
15813 
15814     /* Function Body */
15815     nn = *n;
15816     if (nn < 3) {
15817         *ier = -1;
15818         return 0;
15819     }
15820 
15821 /* Store the first triangle in the linked list. */
15822 
15823     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15824             z__[3])) {
15825 
15826 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15827 
15828         list[1] = 3;
15829         lptr[1] = 2;
15830         list[2] = -2;
15831         lptr[2] = 1;
15832         lend[1] = 2;
15833 
15834         list[3] = 1;
15835         lptr[3] = 4;
15836         list[4] = -3;
15837         lptr[4] = 3;
15838         lend[2] = 4;
15839 
15840         list[5] = 2;
15841         lptr[5] = 6;
15842         list[6] = -1;
15843         lptr[6] = 5;
15844         lend[3] = 6;
15845 
15846     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15847             y[3], &z__[3])) {
15848 
15849 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15850 /*     i.e., node 3 lies in the left hemisphere defined by */
15851 /*     arc 1->2. */
15852 
15853         list[1] = 2;
15854         lptr[1] = 2;
15855         list[2] = -3;
15856         lptr[2] = 1;
15857         lend[1] = 2;
15858 
15859         list[3] = 3;
15860         lptr[3] = 4;
15861         list[4] = -1;
15862         lptr[4] = 3;
15863         lend[2] = 4;
15864 
15865         list[5] = 1;
15866         lptr[5] = 6;
15867         list[6] = -2;
15868         lptr[6] = 5;
15869         lend[3] = 6;
15870 
15871     } else {
15872 
15873 /*   The first three nodes are collinear. */
15874 
15875         *ier = -2;
15876         return 0;
15877     }
15878 
15879 /* Initialize LNEW and test for N = 3. */
15880 
15881     *lnew = 7;
15882     if (nn == 3) {
15883         *ier = 0;
15884         return 0;
15885     }
15886 
15887 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15888 /*   used to obtain an expected-time (N*log(N)) incremental */
15889 /*   algorithm by enabling constant search time for locating */
15890 /*   each new node in the triangulation. */
15891 
15892 /* For each unprocessed node K, NEAR(K) is the index of the */
15893 /*   triangulation node closest to K (used as the starting */
15894 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15895 /*   is an increasing function of the arc length (angular */
15896 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15897 /*   length a. */
15898 
15899 /* Since it is necessary to efficiently find the subset of */
15900 /*   unprocessed nodes associated with each triangulation */
15901 /*   node J (those that have J as their NEAR entries), the */
15902 /*   subsets are stored in NEAR and NEXT as follows:  for */
15903 /*   each node J in the triangulation, I = NEAR(J) is the */
15904 /*   first unprocessed node in J's set (with I = 0 if the */
15905 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15906 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15907 /*   set are initially ordered by increasing indexes (which */
15908 /*   maximizes efficiency) but that ordering is not main- */
15909 /*   tained as the data structure is updated. */
15910 
15911 /* Initialize the data structure for the single triangle. */
15912 
15913     near__[1] = 0;
15914     near__[2] = 0;
15915     near__[3] = 0;
15916     for (k = nn; k >= 4; --k) {
15917         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15918         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15919         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15920         if (d1 <= d2 && d1 <= d3) {
15921             near__[k] = 1;
15922             dist[k] = d1;
15923             next[k] = near__[1];
15924             near__[1] = k;
15925         } else if (d2 <= d1 && d2 <= d3) {
15926             near__[k] = 2;
15927             dist[k] = d2;
15928             next[k] = near__[2];
15929             near__[2] = k;
15930         } else {
15931             near__[k] = 3;
15932             dist[k] = d3;
15933             next[k] = near__[3];
15934             near__[3] = k;
15935         }
15936 /* L1: */
15937     }
15938 
15939 /* Add the remaining nodes */
15940 
15941     i__1 = nn;
15942     for (k = 4; k <= i__1; ++k) {
15943         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15944                 lend[1], lnew, ier);
15945         if (*ier != 0) {
15946             return 0;
15947         }
15948 
15949 /* Remove K from the set of unprocessed nodes associated */
15950 /*   with NEAR(K). */
15951 
15952         i__ = near__[k];
15953         if (near__[i__] == k) {
15954             near__[i__] = next[k];
15955         } else {
15956             i__ = near__[i__];
15957 L2:
15958             i0 = i__;
15959             i__ = next[i0];
15960             if (i__ != k) {
15961                 goto L2;
15962             }
15963             next[i0] = next[k];
15964         }
15965         near__[k] = 0;
15966 
15967 /* Loop on neighbors J of node K. */
15968 
15969         lpl = lend[k];
15970         lp = lpl;
15971 L3:
15972         lp = lptr[lp];
15973         j = (i__2 = list[lp], abs(i__2));
15974 
15975 /* Loop on elements I in the sequence of unprocessed nodes */
15976 /*   associated with J:  K is a candidate for replacing J */
15977 /*   as the nearest triangulation node to I.  The next value */
15978 /*   of I in the sequence, NEXT(I), must be saved before I */
15979 /*   is moved because it is altered by adding I to K's set. */
15980 
15981         i__ = near__[j];
15982 L4:
15983         if (i__ == 0) {
15984             goto L5;
15985         }
15986         nexti = next[i__];
15987 
15988 /* Test for the distance from I to K less than the distance */
15989 /*   from I to J. */
15990 
15991         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15992         if (d__ < dist[i__]) {
15993 
15994 /* Replace J by K as the nearest triangulation node to I: */
15995 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15996 /*   of unprocessed nodes and add it to K's set. */
15997 
15998             near__[i__] = k;
15999             dist[i__] = d__;
16000             if (i__ == near__[j]) {
16001                 near__[j] = nexti;
16002             } else {
16003                 next[i0] = nexti;
16004             }
16005             next[i__] = near__[k];
16006             near__[k] = i__;
16007         } else {
16008             i0 = i__;
16009         }
16010 
16011 /* Bottom of loop on I. */
16012 
16013         i__ = nexti;
16014         goto L4;
16015 
16016 /* Bottom of loop on neighbors J. */
16017 
16018 L5:
16019         if (lp != lpl) {
16020             goto L3;
16021         }
16022 /* L6: */
16023     }
16024     return 0;
16025 } /* trmesh_ */
16026 
16027 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
16028         elat, double *elon, double *a, int *n, double *x,
16029         double *y, double *z__, int *list, int *lptr, int
16030         *lend, char *, long int *numbr, int *ier, short )
16031 {
16032     /* Initialized data */
16033 
16034     static long int annot = TRUE_;
16035     static double fsizn = 10.;
16036     static double fsizt = 16.;
16037     static double tol = .5;
16038 
16039     /* System generated locals */
16040     int i__1, i__2;
16041     double d__1;
16042 
16043     /* Builtin functions */
16044     //double atan(double), sin(double);
16045     //int i_dnnt(double *);
16046     //double cos(double), sqrt(double);
16047 
16048     /* Local variables */
16049     static double t;
16050     static int n0, n1;
16051     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
16052     static int ir, lp;
16053     static double ex, ey, ez, wr, tx, ty;
16054     static int lpl;
16055     static double wrs;
16056     static int ipx1, ipx2, ipy1, ipy2, nseg;
16057     /* Subroutine */ int drwarc_(int *, double *, double *,
16058              double *, int *);
16059 
16060 
16061 /* *********************************************************** */
16062 
16063 /*                                              From STRIPACK */
16064 /*                                            Robert J. Renka */
16065 /*                                  Dept. of Computer Science */
16066 /*                                       Univ. of North Texas */
16067 /*                                           renka@cs.unt.edu */
16068 /*                                                   03/04/03 */
16069 
16070 /*   This subroutine creates a level-2 Encapsulated Post- */
16071 /* script (EPS) file containing a graphical display of a */
16072 /* triangulation of a set of nodes on the surface of the unit */
16073 /* sphere.  The visible portion of the triangulation is */
16074 /* projected onto the plane that contains the origin and has */
16075 /* normal defined by a user-specified eye-position. */
16076 
16077 
16078 /* On input: */
16079 
16080 /*       LUN = long int unit number in the range 0 to 99. */
16081 /*             The unit should be opened with an appropriate */
16082 /*             file name before the call to this routine. */
16083 
16084 /*       PLTSIZ = Plot size in inches.  A circular window in */
16085 /*                the projection plane is mapped to a circu- */
16086 /*                lar viewport with diameter equal to .88* */
16087 /*                PLTSIZ (leaving room for labels outside the */
16088 /*                viewport).  The viewport is centered on the */
16089 /*                8.5 by 11 inch page, and its boundary is */
16090 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16091 
16092 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16093 /*                   the center of projection E (the center */
16094 /*                   of the plot).  The projection plane is */
16095 /*                   the plane that contains the origin and */
16096 /*                   has E as unit normal.  In a rotated */
16097 /*                   coordinate system for which E is the */
16098 /*                   north pole, the projection plane con- */
16099 /*                   tains the equator, and only northern */
16100 /*                   hemisphere nodes are visible (from the */
16101 /*                   point at infinity in the direction E). */
16102 /*                   These are projected orthogonally onto */
16103 /*                   the projection plane (by zeroing the z- */
16104 /*                   component in the rotated coordinate */
16105 /*                   system).  ELAT and ELON must be in the */
16106 /*                   range -90 to 90 and -180 to 180, respec- */
16107 /*                   tively. */
16108 
16109 /*       A = Angular distance in degrees from E to the boun- */
16110 /*           dary of a circular window against which the */
16111 /*           triangulation is clipped.  The projected window */
16112 /*           is a disk of radius r = Sin(A) centered at the */
16113 /*           origin, and only visible nodes whose projections */
16114 /*           are within distance r of the origin are included */
16115 /*           in the plot.  Thus, if A = 90, the plot includes */
16116 /*           the entire hemisphere centered at E.  0 .LT. A */
16117 /*           .LE. 90. */
16118 
16119 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
16120 
16121 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16122 /*               coordinates of the nodes (unit vectors). */
16123 
16124 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16125 /*                        gulation.  Refer to Subroutine */
16126 /*                        TRMESH. */
16127 
16128 /*       TITLE = Type CHARACTER variable or constant contain- */
16129 /*               ing a string to be centered above the plot. */
16130 /*               The string must be enclosed in parentheses; */
16131 /*               i.e., the first and last characters must be */
16132 /*               '(' and ')', respectively, but these are not */
16133 /*               displayed.  TITLE may have at most 80 char- */
16134 /*               acters including the parentheses. */
16135 
16136 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16137 /*               nodal indexes are plotted next to the nodes. */
16138 
16139 /* Input parameters are not altered by this routine. */
16140 
16141 /* On output: */
16142 
16143 /*       IER = Error indicator: */
16144 /*             IER = 0 if no errors were encountered. */
16145 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
16146 /*                     valid range. */
16147 /*             IER = 2 if ELAT, ELON, or A is outside its */
16148 /*                     valid range. */
16149 /*             IER = 3 if an error was encountered in writing */
16150 /*                     to unit LUN. */
16151 
16152 /*   The values in the data statement below may be altered */
16153 /* in order to modify various plotting options. */
16154 
16155 /* Module required by TRPLOT:  DRWARC */
16156 
16157 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
16158 /*                                          DBLE, NINT, SIN, */
16159 /*                                          SQRT */
16160 
16161 /* *********************************************************** */
16162 
16163 
16164     /* Parameter adjustments */
16165     --lend;
16166     --z__;
16167     --y;
16168     --x;
16169     --list;
16170     --lptr;
16171 
16172     /* Function Body */
16173 
16174 /* Local parameters: */
16175 
16176 /* ANNOT =     long int variable with value TRUE iff the plot */
16177 /*               is to be annotated with the values of ELAT, */
16178 /*               ELON, and A */
16179 /* CF =        Conversion factor for degrees to radians */
16180 /* CT =        Cos(ELAT) */
16181 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16182 /* FSIZN =     Font size in points for labeling nodes with */
16183 /*               their indexes if NUMBR = TRUE */
16184 /* FSIZT =     Font size in points for the title (and */
16185 /*               annotation if ANNOT = TRUE) */
16186 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16187 /*               left corner of the bounding box or viewport */
16188 /*               box */
16189 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16190 /*               right corner of the bounding box or viewport */
16191 /*               box */
16192 /* IR =        Half the width (height) of the bounding box or */
16193 /*               viewport box in points -- viewport radius */
16194 /* LP =        LIST index (pointer) */
16195 /* LPL =       Pointer to the last neighbor of N0 */
16196 /* N0 =        Index of a node whose incident arcs are to be */
16197 /*               drawn */
16198 /* N1 =        Neighbor of N0 */
16199 /* NSEG =      Number of line segments used by DRWARC in a */
16200 /*               polygonal approximation to a projected edge */
16201 /* P0 =        Coordinates of N0 in the rotated coordinate */
16202 /*               system or label location (first two */
16203 /*               components) */
16204 /* P1 =        Coordinates of N1 in the rotated coordinate */
16205 /*               system or intersection of edge N0-N1 with */
16206 /*               the equator (in the rotated coordinate */
16207 /*               system) */
16208 /* R11...R23 = Components of the first two rows of a rotation */
16209 /*               that maps E to the north pole (0,0,1) */
16210 /* SF =        Scale factor for mapping world coordinates */
16211 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16212 /*               to viewport coordinates in [IPX1,IPX2] X */
16213 /*               [IPY1,IPY2] */
16214 /* T =         Temporary variable */
16215 /* TOL =       Maximum distance in points between a projected */
16216 /*               triangulation edge and its approximation by */
16217 /*               a polygonal curve */
16218 /* TX,TY =     Translation vector for mapping world coordi- */
16219 /*               nates to viewport coordinates */
16220 /* WR =        Window radius r = Sin(A) */
16221 /* WRS =       WR**2 */
16222 
16223 
16224 /* Test for invalid parameters. */
16225 
16226     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16227         goto L11;
16228     }
16229     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16230         goto L12;
16231     }
16232 
16233 /* Compute a conversion factor CF for degrees to radians */
16234 /*   and compute the window radius WR. */
16235 
16236     cf = atan(1.) / 45.;
16237     wr = sin(cf * *a);
16238     wrs = wr * wr;
16239 
16240 /* Compute the lower left (IPX1,IPY1) and upper right */
16241 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16242 /*   The coordinates, specified in default user space units */
16243 /*   (points, at 72 points/inch with origin at the lower */
16244 /*   left corner of the page), are chosen to preserve the */
16245 /*   square aspect ratio, and to center the plot on the 8.5 */
16246 /*   by 11 inch page.  The center of the page is (306,396), */
16247 /*   and IR = PLTSIZ/2 in points. */
16248 
16249     d__1 = *pltsiz * 36.;
16250     ir = i_dnnt(&d__1);
16251     ipx1 = 306 - ir;
16252     ipx2 = ir + 306;
16253     ipy1 = 396 - ir;
16254     ipy2 = ir + 396;
16255 
16256 /* Output header comments. */
16257 
16258 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16259 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16260 /*     .        '%%BoundingBox:',4I4/ */
16261 /*     .        '%%Title:  Triangulation'/ */
16262 /*     .        '%%Creator:  STRIPACK'/ */
16263 /*     .        '%%EndComments') */
16264 
16265 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16266 /*   of a viewport box obtained by shrinking the bounding box */
16267 /*   by 12% in each dimension. */
16268 
16269     d__1 = (double) ir * .88;
16270     ir = i_dnnt(&d__1);
16271     ipx1 = 306 - ir;
16272     ipx2 = ir + 306;
16273     ipy1 = 396 - ir;
16274     ipy2 = ir + 396;
16275 
16276 /* Set the line thickness to 2 points, and draw the */
16277 /*   viewport boundary. */
16278 
16279     t = 2.;
16280 /*      WRITE (LUN,110,ERR=13) T */
16281 /*      WRITE (LUN,120,ERR=13) IR */
16282 /*      WRITE (LUN,130,ERR=13) */
16283 /*  110 FORMAT (F12.6,' setlinewidth') */
16284 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16285 /*  130 FORMAT ('stroke') */
16286 
16287 /* Set up an affine mapping from the window box [-WR,WR] X */
16288 /*   [-WR,WR] to the viewport box. */
16289 
16290     sf = (double) ir / wr;
16291     tx = ipx1 + sf * wr;
16292     ty = ipy1 + sf * wr;
16293 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16294 /*  140 FORMAT (2F12.6,' translate'/ */
16295 /*    .        2F12.6,' scale') */
16296 
16297 /* The line thickness must be changed to reflect the new */
16298 /*   scaling which is applied to all subsequent output. */
16299 /*   Set it to 1.0 point. */
16300 
16301     t = 1. / sf;
16302 /*      WRITE (LUN,110,ERR=13) T */
16303 
16304 /* Save the current graphics state, and set the clip path to */
16305 /*   the boundary of the window. */
16306 
16307 /*      WRITE (LUN,150,ERR=13) */
16308 /*      WRITE (LUN,160,ERR=13) WR */
16309 /*      WRITE (LUN,170,ERR=13) */
16310 /*  150 FORMAT ('gsave') */
16311 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16312 /*  170 FORMAT ('clip newpath') */
16313 
16314 /* Compute the Cartesian coordinates of E and the components */
16315 /*   of a rotation R which maps E to the north pole (0,0,1). */
16316 /*   R is taken to be a rotation about the z-axis (into the */
16317 /*   yz-plane) followed by a rotation about the x-axis chosen */
16318 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16319 /*   E is the north or south pole. */
16320 
16321 /*           ( R11  R12  0   ) */
16322 /*       R = ( R21  R22  R23 ) */
16323 /*           ( EX   EY   EZ  ) */
16324 
16325     t = cf * *elon;
16326     ct = cos(cf * *elat);
16327     ex = ct * cos(t);
16328     ey = ct * sin(t);
16329     ez = sin(cf * *elat);
16330     if (ct != 0.) {
16331         r11 = -ey / ct;
16332         r12 = ex / ct;
16333     } else {
16334         r11 = 0.;
16335         r12 = 1.;
16336     }
16337     r21 = -ez * r12;
16338     r22 = ez * r11;
16339     r23 = ct;
16340 
16341 /* Loop on visible nodes N0 that project to points */
16342 /*   (P0(1),P0(2)) in the window. */
16343 
16344     i__1 = *n;
16345     for (n0 = 1; n0 <= i__1; ++n0) {
16346         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16347         if (p0[2] < 0.) {
16348             goto L3;
16349         }
16350         p0[0] = r11 * x[n0] + r12 * y[n0];
16351         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16352         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16353             goto L3;
16354         }
16355         lpl = lend[n0];
16356         lp = lpl;
16357 
16358 /* Loop on neighbors N1 of N0.  LPL points to the last */
16359 /*   neighbor of N0.  Copy the components of N1 into P. */
16360 
16361 L1:
16362         lp = lptr[lp];
16363         n1 = (i__2 = list[lp], abs(i__2));
16364         p1[0] = r11 * x[n1] + r12 * y[n1];
16365         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16366         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16367         if (p1[2] < 0.) {
16368 
16369 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16370 /*     intersection of edge N0-N1 with the equator so that */
16371 /*     the edge is clipped properly.  P1(3) is set to 0. */
16372 
16373             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16374             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16375             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16376             p1[0] /= t;
16377             p1[1] /= t;
16378         }
16379 
16380 /*   If node N1 is in the window and N1 < N0, bypass edge */
16381 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16382 
16383         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16384             goto L2;
16385         }
16386 
16387 /*   Add the edge to the path.  (TOL is converted to world */
16388 /*     coordinates.) */
16389 
16390         if (p1[2] < 0.) {
16391             p1[2] = 0.;
16392         }
16393         d__1 = tol / sf;
16394         drwarc_(lun, p0, p1, &d__1, &nseg);
16395 
16396 /* Bottom of loops. */
16397 
16398 L2:
16399         if (lp != lpl) {
16400             goto L1;
16401         }
16402 L3:
16403         ;
16404     }
16405 
16406 /* Paint the path and restore the saved graphics state (with */
16407 /*   no clip path). */
16408 
16409 /*      WRITE (LUN,130,ERR=13) */
16410 /*      WRITE (LUN,190,ERR=13) */
16411 /*  190 FORMAT ('grestore') */
16412     if (*numbr) {
16413 
16414 /* Nodes in the window are to be labeled with their indexes. */
16415 /*   Convert FSIZN from points to world coordinates, and */
16416 /*   output the commands to select a font and scale it. */
16417 
16418         t = fsizn / sf;
16419 /*        WRITE (LUN,200,ERR=13) T */
16420 /*  200   FORMAT ('/Helvetica findfont'/ */
16421 /*     .          F12.6,' scalefont setfont') */
16422 
16423 /* Loop on visible nodes N0 that project to points */
16424 /*   P0 = (P0(1),P0(2)) in the window. */
16425 
16426         i__1 = *n;
16427         for (n0 = 1; n0 <= i__1; ++n0) {
16428             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16429                 goto L4;
16430             }
16431             p0[0] = r11 * x[n0] + r12 * y[n0];
16432             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16433             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16434                 goto L4;
16435             }
16436 
16437 /*   Move to P0 and draw the label N0.  The first character */
16438 /*     will will have its lower left corner about one */
16439 /*     character width to the right of the nodal position. */
16440 
16441 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16442 /*          WRITE (LUN,220,ERR=13) N0 */
16443 /*  210     FORMAT (2F12.6,' moveto') */
16444 /*  220     FORMAT ('(',I3,') show') */
16445 L4:
16446             ;
16447         }
16448     }
16449 
16450 /* Convert FSIZT from points to world coordinates, and output */
16451 /*   the commands to select a font and scale it. */
16452 
16453     t = fsizt / sf;
16454 /*      WRITE (LUN,200,ERR=13) T */
16455 
16456 /* Display TITLE centered above the plot: */
16457 
16458     p0[1] = wr + t * 3.;
16459 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16460 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16461 /*     .        ' moveto') */
16462 /*      WRITE (LUN,240,ERR=13) TITLE */
16463 /*  240 FORMAT (A80/'  show') */
16464     if (annot) {
16465 
16466 /* Display the window center and radius below the plot. */
16467 
16468         p0[0] = -wr;
16469         p0[1] = -wr - 50. / sf;
16470 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16471 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16472         p0[1] -= t * 2.;
16473 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16474 /*        WRITE (LUN,260,ERR=13) A */
16475 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16476 /*     .          ',  ELON = ',F8.2,') show') */
16477 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16478     }
16479 
16480 /* Paint the path and output the showpage command and */
16481 /*   end-of-file indicator. */
16482 
16483 /*      WRITE (LUN,270,ERR=13) */
16484 /*  270 FORMAT ('stroke'/ */
16485 /*     .        'showpage'/ */
16486 /*     .        '%%EOF') */
16487 
16488 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16489 /*   indicator (to eliminate a timeout error message): */
16490 /*   ASCII 4. */
16491 
16492 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16493 /*  280 FORMAT (A1) */
16494 
16495 /* No error encountered. */
16496 
16497     *ier = 0;
16498     return 0;
16499 
16500 /* Invalid input parameter LUN, PLTSIZ, or N. */
16501 
16502 L11:
16503     *ier = 1;
16504     return 0;
16505 
16506 /* Invalid input parameter ELAT, ELON, or A. */
16507 
16508 L12:
16509     *ier = 2;
16510     return 0;
16511 
16512 /* Error writing to unit LUN. */
16513 
16514 /* L13: */
16515     *ier = 3;
16516     return 0;
16517 } /* trplot_ */
16518 
16519 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16520         double *z__, int *iflag, int *list, int *lptr,
16521         int *lend, int *lout)
16522 {
16523     /* Initialized data */
16524 
16525     static int nmax = 9999;
16526     static int nlmax = 58;
16527 
16528     /* System generated locals */
16529     int i__1;
16530 
16531     /* Local variables */
16532     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16533             400];
16534 
16535 
16536 /* *********************************************************** */
16537 
16538 /*                                              From STRIPACK */
16539 /*                                            Robert J. Renka */
16540 /*                                  Dept. of Computer Science */
16541 /*                                       Univ. of North Texas */
16542 /*                                           renka@cs.unt.edu */
16543 /*                                                   07/25/98 */
16544 
16545 /*   This subroutine prints the triangulation adjacency lists */
16546 /* created by Subroutine TRMESH and, optionally, the nodal */
16547 /* coordinates (either latitude and longitude or Cartesian */
16548 /* coordinates) on long int unit LOUT.  The list of neighbors */
16549 /* of a boundary node is followed by index 0.  The numbers of */
16550 /* boundary nodes, triangles, and arcs are also printed. */
16551 
16552 
16553 /* On input: */
16554 
16555 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16556 /*           and N .LE. 9999. */
16557 
16558 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16559 /*               coordinates of the nodes if IFLAG = 0, or */
16560 /*               (X and Y only) arrays of length N containing */
16561 /*               longitude and latitude, respectively, if */
16562 /*               IFLAG > 0, or unused dummy parameters if */
16563 /*               IFLAG < 0. */
16564 
16565 /*       IFLAG = Nodal coordinate option indicator: */
16566 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16567 /*                         Cartesian coordinates) are to be */
16568 /*                         printed (to 6 decimal places). */
16569 /*               IFLAG > 0 if only X and Y (assumed to con- */
16570 /*                         tain longitude and latitude) are */
16571 /*                         to be printed (to 6 decimal */
16572 /*                         places). */
16573 /*               IFLAG < 0 if only the adjacency lists are to */
16574 /*                         be printed. */
16575 
16576 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16577 /*                        gulation.  Refer to Subroutine */
16578 /*                        TRMESH. */
16579 
16580 /*       LOUT = long int unit for output.  If LOUT is not in */
16581 /*              the range 0 to 99, output is written to */
16582 /*              long int unit 6. */
16583 
16584 /* Input parameters are not altered by this routine. */
16585 
16586 /* On output: */
16587 
16588 /*   The adjacency lists and nodal coordinates (as specified */
16589 /* by IFLAG) are written to unit LOUT. */
16590 
16591 /* Modules required by TRPRNT:  None */
16592 
16593 /* *********************************************************** */
16594 
16595     /* Parameter adjustments */
16596     --lend;
16597     --z__;
16598     --y;
16599     --x;
16600     --list;
16601     --lptr;
16602 
16603     /* Function Body */
16604 
16605 /* Local parameters: */
16606 
16607 /* I =     NABOR index (1 to K) */
16608 /* INC =   Increment for NL associated with an adjacency list */
16609 /* K =     Counter and number of neighbors of NODE */
16610 /* LP =    LIST pointer of a neighbor of NODE */
16611 /* LPL =   Pointer to the last neighbor of NODE */
16612 /* LUN =   long int unit for output (copy of LOUT) */
16613 /* NA =    Number of arcs in the triangulation */
16614 /* NABOR = Array containing the adjacency list associated */
16615 /*           with NODE, with zero appended if NODE is a */
16616 /*           boundary node */
16617 /* NB =    Number of boundary nodes encountered */
16618 /* ND =    Index of a neighbor of NODE (or negative index) */
16619 /* NL =    Number of lines that have been printed on the */
16620 /*           current page */
16621 /* NLMAX = Maximum number of print lines per page (except */
16622 /*           for the last page which may have two addi- */
16623 /*           tional lines) */
16624 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16625 /* NODE =  Index of a node and DO-loop index (1 to N) */
16626 /* NN =    Local copy of N */
16627 /* NT =    Number of triangles in the triangulation */
16628 
16629     nn = *n;
16630     lun = *lout;
16631     if (lun < 0 || lun > 99) {
16632         lun = 6;
16633     }
16634 
16635 /* Print a heading and test the range of N. */
16636 
16637 /*      WRITE (LUN,100) NN */
16638     if (nn < 3 || nn > nmax) {
16639 
16640 /* N is outside its valid range. */
16641 
16642 /*        WRITE (LUN,110) */
16643         return 0;
16644     }
16645 
16646 /* Initialize NL (the number of lines printed on the current */
16647 /*   page) and NB (the number of boundary nodes encountered). */
16648 
16649     nl = 6;
16650     nb = 0;
16651     if (*iflag < 0) {
16652 
16653 /* Print LIST only.  K is the number of neighbors of NODE */
16654 /*   that have been stored in NABOR. */
16655 
16656 /*        WRITE (LUN,101) */
16657         i__1 = nn;
16658         for (node = 1; node <= i__1; ++node) {
16659             lpl = lend[node];
16660             lp = lpl;
16661             k = 0;
16662 
16663 L1:
16664             ++k;
16665             lp = lptr[lp];
16666             nd = list[lp];
16667             nabor[k - 1] = nd;
16668             if (lp != lpl) {
16669                 goto L1;
16670             }
16671             if (nd <= 0) {
16672 
16673 /*   NODE is a boundary node.  Correct the sign of the last */
16674 /*     neighbor, add 0 to the end of the list, and increment */
16675 /*     NB. */
16676 
16677                 nabor[k - 1] = -nd;
16678                 ++k;
16679                 nabor[k - 1] = 0;
16680                 ++nb;
16681             }
16682 
16683 /*   Increment NL and print the list of neighbors. */
16684 
16685             inc = (k - 1) / 14 + 2;
16686             nl += inc;
16687             if (nl > nlmax) {
16688 /*            WRITE (LUN,108) */
16689                 nl = inc;
16690             }
16691 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16692 /*          IF (K .NE. 14) */
16693 /*           WRITE (LUN,107) */
16694 /* L2: */
16695         }
16696     } else if (*iflag > 0) {
16697 
16698 /* Print X (longitude), Y (latitude), and LIST. */
16699 
16700 /*        WRITE (LUN,102) */
16701         i__1 = nn;
16702         for (node = 1; node <= i__1; ++node) {
16703             lpl = lend[node];
16704             lp = lpl;
16705             k = 0;
16706 
16707 L3:
16708             ++k;
16709             lp = lptr[lp];
16710             nd = list[lp];
16711             nabor[k - 1] = nd;
16712             if (lp != lpl) {
16713                 goto L3;
16714             }
16715             if (nd <= 0) {
16716 
16717 /*   NODE is a boundary node. */
16718 
16719                 nabor[k - 1] = -nd;
16720                 ++k;
16721                 nabor[k - 1] = 0;
16722                 ++nb;
16723             }
16724 
16725 /*   Increment NL and print X, Y, and NABOR. */
16726 
16727             inc = (k - 1) / 8 + 2;
16728             nl += inc;
16729             if (nl > nlmax) {
16730 /*            WRITE (LUN,108) */
16731                 nl = inc;
16732             }
16733 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16734 /*          IF (K .NE. 8) */
16735 /*           PRINT *,K */
16736 /*           WRITE (LUN,107) */
16737 /* L4: */
16738         }
16739     } else {
16740 
16741 /* Print X, Y, Z, and LIST. */
16742 
16743 /*        WRITE (LUN,103) */
16744         i__1 = nn;
16745         for (node = 1; node <= i__1; ++node) {
16746             lpl = lend[node];
16747             lp = lpl;
16748             k = 0;
16749 
16750 L5:
16751             ++k;
16752             lp = lptr[lp];
16753             nd = list[lp];
16754             nabor[k - 1] = nd;
16755             if (lp != lpl) {
16756                 goto L5;
16757             }
16758             if (nd <= 0) {
16759 
16760 /*   NODE is a boundary node. */
16761 
16762                 nabor[k - 1] = -nd;
16763                 ++k;
16764                 nabor[k - 1] = 0;
16765                 ++nb;
16766             }
16767 
16768 /*   Increment NL and print X, Y, Z, and NABOR. */
16769 
16770             inc = (k - 1) / 5 + 2;
16771             nl += inc;
16772             if (nl > nlmax) {
16773 /*            WRITE (LUN,108) */
16774                 nl = inc;
16775             }
16776 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16777 /*          IF (K .NE. 5) */
16778 /*           print *,K */
16779 /*           WRITE (LUN,107) */
16780 /* L6: */
16781         }
16782     }
16783 
16784 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16785 /*   triangles). */
16786 
16787     if (nb != 0) {
16788         na = nn * 3 - nb - 3;
16789         nt = (nn << 1) - nb - 2;
16790     } else {
16791         na = nn * 3 - 6;
16792         nt = (nn << 1) - 4;
16793     }
16794 /*      WRITE (LUN,109) NB, NA, NT */
16795     return 0;
16796 
16797 /* Print formats: */
16798 
16799 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16800 /*     .        'Structure,  N = ',I5//) */
16801 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16802 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16803 /*     .        18X,'Neighbors of Node'//) */
16804 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16805 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16806 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16807 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16808 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16809 /*  107 FORMAT (1X) */
16810 /*  108 FORMAT (///) */
16811 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16812 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16813 /*     .        ' Triangles') */
16814 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16815 /*     .        ' range ***') */
16816 } /* trprnt_ */
16817 
16818 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16819         elat, double *elon, double *a, int *n, double *x,
16820         double *y, double *z__, int *nt, int *listc, int *
16821         lptr, int *lend, double *xc, double *yc, double *zc,
16822         char *, long int *numbr, int *ier, short)
16823 {
16824     /* Initialized data */
16825 
16826     static long int annot = TRUE_;
16827     static double fsizn = 10.;
16828     static double fsizt = 16.;
16829     static double tol = .5;
16830 
16831     /* System generated locals */
16832     int i__1;
16833     double d__1;
16834 
16835     /* Builtin functions */
16836     //double atan(double), sin(double);
16837     //int i_dnnt(double *);
16838     //double cos(double), sqrt(double);
16839 
16840     /* Local variables */
16841     static double t;
16842     static int n0;
16843     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16844             sf;
16845     static int ir, lp;
16846     static double ex, ey, ez, wr, tx, ty;
16847     static long int in1, in2;
16848     static int kv1, kv2, lpl;
16849     static double wrs;
16850     static int ipx1, ipx2, ipy1, ipy2, nseg;
16851     /* Subroutine */ int drwarc_(int *, double *, double *,
16852              double *, int *);
16853 
16854 
16855 /* *********************************************************** */
16856 
16857 /*                                              From STRIPACK */
16858 /*                                            Robert J. Renka */
16859 /*                                  Dept. of Computer Science */
16860 /*                                       Univ. of North Texas */
16861 /*                                           renka@cs.unt.edu */
16862 /*                                                   03/04/03 */
16863 
16864 /*   This subroutine creates a level-2 Encapsulated Post- */
16865 /* script (EPS) file containing a graphical depiction of a */
16866 /* Voronoi diagram of a set of nodes on the unit sphere. */
16867 /* The visible portion of the diagram is projected orthog- */
16868 /* onally onto the plane that contains the origin and has */
16869 /* normal defined by a user-specified eye-position. */
16870 
16871 /*   The parameters defining the Voronoi diagram may be com- */
16872 /* puted by Subroutine CRLIST. */
16873 
16874 
16875 /* On input: */
16876 
16877 /*       LUN = long int unit number in the range 0 to 99. */
16878 /*             The unit should be opened with an appropriate */
16879 /*             file name before the call to this routine. */
16880 
16881 /*       PLTSIZ = Plot size in inches.  A circular window in */
16882 /*                the projection plane is mapped to a circu- */
16883 /*                lar viewport with diameter equal to .88* */
16884 /*                PLTSIZ (leaving room for labels outside the */
16885 /*                viewport).  The viewport is centered on the */
16886 /*                8.5 by 11 inch page, and its boundary is */
16887 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16888 
16889 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16890 /*                   the center of projection E (the center */
16891 /*                   of the plot).  The projection plane is */
16892 /*                   the plane that contains the origin and */
16893 /*                   has E as unit normal.  In a rotated */
16894 /*                   coordinate system for which E is the */
16895 /*                   north pole, the projection plane con- */
16896 /*                   tains the equator, and only northern */
16897 /*                   hemisphere points are visible (from the */
16898 /*                   point at infinity in the direction E). */
16899 /*                   These are projected orthogonally onto */
16900 /*                   the projection plane (by zeroing the z- */
16901 /*                   component in the rotated coordinate */
16902 /*                   system).  ELAT and ELON must be in the */
16903 /*                   range -90 to 90 and -180 to 180, respec- */
16904 /*                   tively. */
16905 
16906 /*       A = Angular distance in degrees from E to the boun- */
16907 /*           dary of a circular window against which the */
16908 /*           Voronoi diagram is clipped.  The projected win- */
16909 /*           dow is a disk of radius r = Sin(A) centered at */
16910 /*           the origin, and only visible vertices whose */
16911 /*           projections are within distance r of the origin */
16912 /*           are included in the plot.  Thus, if A = 90, the */
16913 /*           plot includes the entire hemisphere centered at */
16914 /*           E.  0 .LT. A .LE. 90. */
16915 
16916 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16917 /*           regions.  N .GE. 3. */
16918 
16919 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16920 /*               coordinates of the nodes (unit vectors). */
16921 
16922 /*       NT = Number of Voronoi region vertices (triangles, */
16923 /*            including those in the extended triangulation */
16924 /*            if the number of boundary nodes NB is nonzero): */
16925 /*            NT = 2*N-4. */
16926 
16927 /*       LISTC = Array of length 3*NT containing triangle */
16928 /*               indexes (indexes to XC, YC, and ZC) stored */
16929 /*               in 1-1 correspondence with LIST/LPTR entries */
16930 /*               (or entries that would be stored in LIST for */
16931 /*               the extended triangulation):  the index of */
16932 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16933 /*               LISTC(L), and LISTC(M), where LIST(K), */
16934 /*               LIST(L), and LIST(M) are the indexes of N2 */
16935 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16936 /*               and N1 as a neighbor of N3.  The Voronoi */
16937 /*               region associated with a node is defined by */
16938 /*               the CCW-ordered sequence of circumcenters in */
16939 /*               one-to-one correspondence with its adjacency */
16940 /*               list (in the extended triangulation). */
16941 
16942 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16943 /*              set of pointers (LISTC indexes) in one-to-one */
16944 /*              correspondence with the elements of LISTC. */
16945 /*              LISTC(LPTR(I)) indexes the triangle which */
16946 /*              follows LISTC(I) in cyclical counterclockwise */
16947 /*              order (the first neighbor follows the last */
16948 /*              neighbor). */
16949 
16950 /*       LEND = Array of length N containing a set of */
16951 /*              pointers to triangle lists.  LP = LEND(K) */
16952 /*              points to a triangle (indexed by LISTC(LP)) */
16953 /*              containing node K for K = 1 to N. */
16954 
16955 /*       XC,YC,ZC = Arrays of length NT containing the */
16956 /*                  Cartesian coordinates of the triangle */
16957 /*                  circumcenters (Voronoi vertices). */
16958 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16959 
16960 /*       TITLE = Type CHARACTER variable or constant contain- */
16961 /*               ing a string to be centered above the plot. */
16962 /*               The string must be enclosed in parentheses; */
16963 /*               i.e., the first and last characters must be */
16964 /*               '(' and ')', respectively, but these are not */
16965 /*               displayed.  TITLE may have at most 80 char- */
16966 /*               acters including the parentheses. */
16967 
16968 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16969 /*               nodal indexes are plotted at the Voronoi */
16970 /*               region centers. */
16971 
16972 /* Input parameters are not altered by this routine. */
16973 
16974 /* On output: */
16975 
16976 /*       IER = Error indicator: */
16977 /*             IER = 0 if no errors were encountered. */
16978 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16979 /*                     its valid range. */
16980 /*             IER = 2 if ELAT, ELON, or A is outside its */
16981 /*                     valid range. */
16982 /*             IER = 3 if an error was encountered in writing */
16983 /*                     to unit LUN. */
16984 
16985 /* Module required by VRPLOT:  DRWARC */
16986 
16987 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16988 /*                                          DBLE, NINT, SIN, */
16989 /*                                          SQRT */
16990 
16991 /* *********************************************************** */
16992 
16993 
16994     /* Parameter adjustments */
16995     --lend;
16996     --z__;
16997     --y;
16998     --x;
16999     --zc;
17000     --yc;
17001     --xc;
17002     --listc;
17003     --lptr;
17004 
17005     /* Function Body */
17006 
17007 /* Local parameters: */
17008 
17009 /* ANNOT =     long int variable with value TRUE iff the plot */
17010 /*               is to be annotated with the values of ELAT, */
17011 /*               ELON, and A */
17012 /* CF =        Conversion factor for degrees to radians */
17013 /* CT =        Cos(ELAT) */
17014 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
17015 /* FSIZN =     Font size in points for labeling nodes with */
17016 /*               their indexes if NUMBR = TRUE */
17017 /* FSIZT =     Font size in points for the title (and */
17018 /*               annotation if ANNOT = TRUE) */
17019 /* IN1,IN2 =   long int variables with value TRUE iff the */
17020 /*               projections of vertices KV1 and KV2, respec- */
17021 /*               tively, are inside the window */
17022 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
17023 /*               left corner of the bounding box or viewport */
17024 /*               box */
17025 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
17026 /*               right corner of the bounding box or viewport */
17027 /*               box */
17028 /* IR =        Half the width (height) of the bounding box or */
17029 /*               viewport box in points -- viewport radius */
17030 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
17031 /* LP =        LIST index (pointer) */
17032 /* LPL =       Pointer to the last neighbor of N0 */
17033 /* N0 =        Index of a node */
17034 /* NSEG =      Number of line segments used by DRWARC in a */
17035 /*               polygonal approximation to a projected edge */
17036 /* P1 =        Coordinates of vertex KV1 in the rotated */
17037 /*               coordinate system */
17038 /* P2 =        Coordinates of vertex KV2 in the rotated */
17039 /*               coordinate system or intersection of edge */
17040 /*               KV1-KV2 with the equator (in the rotated */
17041 /*               coordinate system) */
17042 /* R11...R23 = Components of the first two rows of a rotation */
17043 /*               that maps E to the north pole (0,0,1) */
17044 /* SF =        Scale factor for mapping world coordinates */
17045 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
17046 /*               to viewport coordinates in [IPX1,IPX2] X */
17047 /*               [IPY1,IPY2] */
17048 /* T =         Temporary variable */
17049 /* TOL =       Maximum distance in points between a projected */
17050 /*               Voronoi edge and its approximation by a */
17051 /*               polygonal curve */
17052 /* TX,TY =     Translation vector for mapping world coordi- */
17053 /*               nates to viewport coordinates */
17054 /* WR =        Window radius r = Sin(A) */
17055 /* WRS =       WR**2 */
17056 /* X0,Y0 =     Projection plane coordinates of node N0 or */
17057 /*               label location */
17058 
17059 
17060 /* Test for invalid parameters. */
17061 
17062     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
17063             nt != 2 * *n - 4) {
17064         goto L11;
17065     }
17066     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
17067         goto L12;
17068     }
17069 
17070 /* Compute a conversion factor CF for degrees to radians */
17071 /*   and compute the window radius WR. */
17072 
17073     cf = atan(1.) / 45.;
17074     wr = sin(cf * *a);
17075     wrs = wr * wr;
17076 
17077 /* Compute the lower left (IPX1,IPY1) and upper right */
17078 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
17079 /*   The coordinates, specified in default user space units */
17080 /*   (points, at 72 points/inch with origin at the lower */
17081 /*   left corner of the page), are chosen to preserve the */
17082 /*   square aspect ratio, and to center the plot on the 8.5 */
17083 /*   by 11 inch page.  The center of the page is (306,396), */
17084 /*   and IR = PLTSIZ/2 in points. */
17085 
17086     d__1 = *pltsiz * 36.;
17087     ir = i_dnnt(&d__1);
17088     ipx1 = 306 - ir;
17089     ipx2 = ir + 306;
17090     ipy1 = 396 - ir;
17091     ipy2 = ir + 396;
17092 
17093 /* Output header comments. */
17094 
17095 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
17096 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
17097 /*     .        '%%BoundingBox:',4I4/ */
17098 /*     .        '%%Title:  Voronoi diagram'/ */
17099 /*     .        '%%Creator:  STRIPACK'/ */
17100 /*     .        '%%EndComments') */
17101 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
17102 /*   of a viewport box obtained by shrinking the bounding box */
17103 /*   by 12% in each dimension. */
17104 
17105     d__1 = (double) ir * .88;
17106     ir = i_dnnt(&d__1);
17107     ipx1 = 306 - ir;
17108     ipx2 = ir + 306;
17109     ipy1 = 396 - ir;
17110     ipy2 = ir + 396;
17111 
17112 /* Set the line thickness to 2 points, and draw the */
17113 /*   viewport boundary. */
17114 
17115     t = 2.;
17116 /*      WRITE (LUN,110,ERR=13) T */
17117 /*      WRITE (LUN,120,ERR=13) IR */
17118 /*      WRITE (LUN,130,ERR=13) */
17119 /*  110 FORMAT (F12.6,' setlinewidth') */
17120 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
17121 /*  130 FORMAT ('stroke') */
17122 
17123 /* Set up an affine mapping from the window box [-WR,WR] X */
17124 /*   [-WR,WR] to the viewport box. */
17125 
17126     sf = (double) ir / wr;
17127     tx = ipx1 + sf * wr;
17128     ty = ipy1 + sf * wr;
17129 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
17130 /*  140 FORMAT (2F12.6,' translate'/ */
17131 /*     .        2F12.6,' scale') */
17132 
17133 /* The line thickness must be changed to reflect the new */
17134 /*   scaling which is applied to all subsequent output. */
17135 /*   Set it to 1.0 point. */
17136 
17137     t = 1. / sf;
17138 /*      WRITE (LUN,110,ERR=13) T */
17139 
17140 /* Save the current graphics state, and set the clip path to */
17141 /*   the boundary of the window. */
17142 
17143 /*      WRITE (LUN,150,ERR=13) */
17144 /*      WRITE (LUN,160,ERR=13) WR */
17145 /*      WRITE (LUN,170,ERR=13) */
17146 /*  150 FORMAT ('gsave') */
17147 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
17148 /*  170 FORMAT ('clip newpath') */
17149 
17150 /* Compute the Cartesian coordinates of E and the components */
17151 /*   of a rotation R which maps E to the north pole (0,0,1). */
17152 /*   R is taken to be a rotation about the z-axis (into the */
17153 /*   yz-plane) followed by a rotation about the x-axis chosen */
17154 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
17155 /*   E is the north or south pole. */
17156 
17157 /*           ( R11  R12  0   ) */
17158 /*       R = ( R21  R22  R23 ) */
17159 /*           ( EX   EY   EZ  ) */
17160 
17161     t = cf * *elon;
17162     ct = cos(cf * *elat);
17163     ex = ct * cos(t);
17164     ey = ct * sin(t);
17165     ez = sin(cf * *elat);
17166     if (ct != 0.) {
17167         r11 = -ey / ct;
17168         r12 = ex / ct;
17169     } else {
17170         r11 = 0.;
17171         r12 = 1.;
17172     }
17173     r21 = -ez * r12;
17174     r22 = ez * r11;
17175     r23 = ct;
17176 
17177 /* Loop on nodes (Voronoi centers) N0. */
17178 /*   LPL indexes the last neighbor of N0. */
17179 
17180     i__1 = *n;
17181     for (n0 = 1; n0 <= i__1; ++n0) {
17182         lpl = lend[n0];
17183 
17184 /* Set KV2 to the first (and last) vertex index and compute */
17185 /*   its coordinates P2 in the rotated coordinate system. */
17186 
17187         kv2 = listc[lpl];
17188         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17189         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17190         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17191 
17192 /*   IN2 = TRUE iff KV2 is in the window. */
17193 
17194         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17195 
17196 /* Loop on neighbors N1 of N0.  For each triangulation edge */
17197 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
17198 
17199         lp = lpl;
17200 L1:
17201         lp = lptr[lp];
17202         kv1 = kv2;
17203         p1[0] = p2[0];
17204         p1[1] = p2[1];
17205         p1[2] = p2[2];
17206         in1 = in2;
17207         kv2 = listc[lp];
17208 
17209 /*   Compute the new values of P2 and IN2. */
17210 
17211         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17212         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17213         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17214         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17215 
17216 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
17217 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
17218 /*   outside (so that the edge is drawn only once). */
17219 
17220         if (! in1 || (in2 && kv2 <= kv1)) {
17221             goto L2;
17222         }
17223         if (p2[2] < 0.) {
17224 
17225 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17226 /*     intersection of edge KV1-KV2 with the equator so that */
17227 /*     the edge is clipped properly.  P2(3) is set to 0. */
17228 
17229             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17230             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17231             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17232             p2[0] /= t;
17233             p2[1] /= t;
17234         }
17235 
17236 /*   Add the edge to the path.  (TOL is converted to world */
17237 /*     coordinates.) */
17238 
17239         if (p2[2] < 0.) {
17240             p2[2] = 0.f;
17241         }
17242         d__1 = tol / sf;
17243         drwarc_(lun, p1, p2, &d__1, &nseg);
17244 
17245 /* Bottom of loops. */
17246 
17247 L2:
17248         if (lp != lpl) {
17249             goto L1;
17250         }
17251 /* L3: */
17252     }
17253 
17254 /* Paint the path and restore the saved graphics state (with */
17255 /*   no clip path). */
17256 
17257 /*      WRITE (LUN,130,ERR=13) */
17258 /*      WRITE (LUN,190,ERR=13) */
17259 /*  190 FORMAT ('grestore') */
17260     if (*numbr) {
17261 
17262 /* Nodes in the window are to be labeled with their indexes. */
17263 /*   Convert FSIZN from points to world coordinates, and */
17264 /*   output the commands to select a font and scale it. */
17265 
17266         t = fsizn / sf;
17267 /*        WRITE (LUN,200,ERR=13) T */
17268 /*  200   FORMAT ('/Helvetica findfont'/ */
17269 /*     .          F12.6,' scalefont setfont') */
17270 
17271 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17272 /*   the window. */
17273 
17274         i__1 = *n;
17275         for (n0 = 1; n0 <= i__1; ++n0) {
17276             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17277                 goto L4;
17278             }
17279             x0 = r11 * x[n0] + r12 * y[n0];
17280             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17281             if (x0 * x0 + y0 * y0 > wrs) {
17282                 goto L4;
17283             }
17284 
17285 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17286 /*     of the first character at (X0,Y0). */
17287 
17288 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17289 /*          WRITE (LUN,220,ERR=13) N0 */
17290 /*  210     FORMAT (2F12.6,' moveto') */
17291 /*  220     FORMAT ('(',I3,') show') */
17292 L4:
17293             ;
17294         }
17295     }
17296 
17297 /* Convert FSIZT from points to world coordinates, and output */
17298 /*   the commands to select a font and scale it. */
17299 
17300     t = fsizt / sf;
17301 /*      WRITE (LUN,200,ERR=13) T */
17302 
17303 /* Display TITLE centered above the plot: */
17304 
17305     y0 = wr + t * 3.;
17306 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17307 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17308 /*     .        ' moveto') */
17309 /*      WRITE (LUN,240,ERR=13) TITLE */
17310 /*  240 FORMAT (A80/'  show') */
17311     if (annot) {
17312 
17313 /* Display the window center and radius below the plot. */
17314 
17315         x0 = -wr;
17316         y0 = -wr - 50. / sf;
17317 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17318 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17319         y0 -= t * 2.;
17320 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17321 /*        WRITE (LUN,260,ERR=13) A */
17322 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17323 /*     .          ',  ELON = ',F8.2,') show') */
17324 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17325     }
17326 
17327 /* Paint the path and output the showpage command and */
17328 /*   end-of-file indicator. */
17329 
17330 /*      WRITE (LUN,270,ERR=13) */
17331 /*  270 FORMAT ('stroke'/ */
17332 /*     .        'showpage'/ */
17333 /*     .        '%%EOF') */
17334 
17335 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17336 /*   indicator (to eliminate a timeout error message): */
17337 /*   ASCII 4. */
17338 
17339 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17340 /*  280 FORMAT (A1) */
17341 
17342 /* No error encountered. */
17343 
17344     *ier = 0;
17345     return 0;
17346 
17347 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17348 
17349 L11:
17350     *ier = 1;
17351     return 0;
17352 
17353 /* Invalid input parameter ELAT, ELON, or A. */
17354 
17355 L12:
17356     *ier = 2;
17357     return 0;
17358 
17359 /* Error writing to unit LUN. */
17360 
17361 /* L13: */
17362     *ier = 3;
17363     return 0;
17364 } /* vrplot_ */
17365 
17366 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17367         double *rannum)
17368 {
17369     static double x;
17370 
17371 
17372 /*   This routine returns pseudo-random numbers uniformly */
17373 /* distributed in the interval (0,1).  int seeds IX, IY, */
17374 /* and IZ should be initialized to values in the range 1 to */
17375 /* 30,000 before the first call to RANDOM, and should not */
17376 /* be altered between subsequent calls (unless a sequence */
17377 /* of random numbers is to be repeated by reinitializing the */
17378 /* seeds). */
17379 
17380 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17381 /*             and Portable Pseudo-random Number Generator, */
17382 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17383 /*             pp. 188-190. */
17384 
17385     *ix = *ix * 171 % 30269;
17386     *iy = *iy * 172 % 30307;
17387     *iz = *iz * 170 % 30323;
17388     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17389             double) (*iz) / 30323.;
17390     *rannum = x - (int) x;
17391     return 0;
17392 } /* random_ */
17393 
17394 #undef TRUE_
17395 #undef FALSE_
17396 #undef abs
17397 
17398 /*################################################################################################
17399 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17400 ######   You must link the resulting object file with the libraries: #############################
17401 ####################    -lf2c -lm   (in that order)   ############################################
17402 ################################################################################################*/
17403 
17404 
17405 
17406 EMData* Util::mult_scalar(EMData* img, float scalar)
17407 {
17408         ENTERFUNC;
17409         /* Exception Handle */
17410         if (!img) {
17411                 throw NullPointerException("NULL input image");
17412         }
17413         /* ============  output = scalar*input  ================== */
17414 
17415         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17416         size_t size = (size_t)nx*ny*nz;
17417         EMData * img2 = img->copy_head();
17418         float *img_ptr  =img->get_data();
17419         float *img2_ptr = img2->get_data();
17420         for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17421         img2->update();
17422 
17423         if(img->is_complex()) {
17424                 img2->set_complex(true);
17425                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17426         }
17427         EXITFUNC;
17428         return img2;
17429 }
17430 
17431 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17432 {
17433         ENTERFUNC;
17434         /* Exception Handle */
17435         if (!img) {
17436                 throw NullPointerException("NULL input image");
17437         }
17438         /* ==============   output = img + scalar*img1   ================ */
17439 
17440         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17441         size_t size = (size_t)nx*ny*nz;
17442         EMData * img2 = img->copy_head();
17443         float *img_ptr  =img->get_data();
17444         float *img2_ptr = img2->get_data();
17445         float *img1_ptr = img1->get_data();
17446         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17447         img2->update();
17448         if(img->is_complex()) {
17449                 img2->set_complex(true);
17450                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17451         }
17452 
17453         EXITFUNC;
17454         return img2;
17455 }
17456 
17457 EMData* Util::addn_img(EMData* img, EMData* img1)
17458 {
17459         ENTERFUNC;
17460         /* Exception Handle */
17461         if (!img) {
17462                 throw NullPointerException("NULL input image");
17463         }
17464         /* ==============   output = img + img1   ================ */
17465 
17466         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17467         size_t size = (size_t)nx*ny*nz;
17468         EMData * img2 = img->copy_head();
17469         float *img_ptr  =img->get_data();
17470         float *img2_ptr = img2->get_data();
17471         float *img1_ptr = img1->get_data();
17472         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17473         img2->update();
17474         if(img->is_complex()) {
17475                 img2->set_complex(true);
17476                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17477         }
17478 
17479         EXITFUNC;
17480         return img2;
17481 }
17482 
17483 EMData* Util::subn_img(EMData* img, EMData* img1)
17484 {
17485         ENTERFUNC;
17486         /* Exception Handle */
17487         if (!img) {
17488                 throw NullPointerException("NULL input image");
17489         }
17490         /* ==============   output = img - img1   ================ */
17491 
17492         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17493         size_t size = (size_t)nx*ny*nz;
17494         EMData * img2 = img->copy_head();
17495         float *img_ptr  =img->get_data();
17496         float *img2_ptr = img2->get_data();
17497         float *img1_ptr = img1->get_data();
17498         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17499         img2->update();
17500         if(img->is_complex()) {
17501                 img2->set_complex(true);
17502                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17503         }
17504 
17505         EXITFUNC;
17506         return img2;
17507 }
17508 
17509 EMData* Util::muln_img(EMData* img, EMData* img1)
17510 {
17511         ENTERFUNC;
17512         /* Exception Handle */
17513         if (!img) {
17514                 throw NullPointerException("NULL input image");
17515         }
17516         /* ==============   output = img * img1   ================ */
17517 
17518         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17519         size_t size = (size_t)nx*ny*nz;
17520         EMData * img2 = img->copy_head();
17521         float *img_ptr  =img->get_data();
17522         float *img2_ptr = img2->get_data();
17523         float *img1_ptr = img1->get_data();
17524         if(img->is_complex()) {
17525                 for (size_t i=0; i<size; i+=2) {
17526                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17527                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17528                 }
17529                 img2->set_complex(true);
17530                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17531         } else {
17532                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17533                 img2->update();
17534         }
17535 
17536         EXITFUNC;
17537         return img2;
17538 }
17539 
17540 EMData* Util::divn_img(EMData* img, EMData* img1)
17541 {
17542         ENTERFUNC;
17543         /* Exception Handle */
17544         if (!img) {
17545                 throw NullPointerException("NULL input image");
17546         }
17547         /* ==============   output = img / img1   ================ */
17548 
17549         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17550         size_t size = (size_t)nx*ny*nz;
17551         EMData * img2 = img->copy_head();
17552         float *img_ptr  =img->get_data();
17553         float *img2_ptr = img2->get_data();
17554         float *img1_ptr = img1->get_data();
17555         if(img->is_complex()) {
17556                 float  sq2;
17557                 for (size_t i=0; i<size; i+=2) {
17558                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17559                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17560                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17561                 }
17562                 img2->set_complex(true);
17563                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17564         } else {
17565                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17566                 img2->update();
17567         }
17568 
17569         EXITFUNC;
17570         return img2;
17571 }
17572 
17573 EMData* Util::divn_filter(EMData* img, EMData* img1)
17574 {
17575         ENTERFUNC;
17576         /* Exception Handle */
17577         if (!img) {
17578                 throw NullPointerException("NULL input image");
17579         }
17580         /* ========= img /= img1 ===================== */
17581 
17582         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17583         size_t size = (size_t)nx*ny*nz;
17584         EMData * img2 = img->copy_head();
17585         float *img_ptr  =img->get_data();
17586         float *img1_ptr = img1->get_data();
17587         float *img2_ptr = img2->get_data();
17588         if(img->is_complex()) {
17589                 for (size_t i=0; i<size; i+=2) {
17590                         if(img1_ptr[i] > 1.e-10f) {
17591                                 img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17592                                 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17593                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17594                 }
17595         } else  throw ImageFormatException("Only Fourier image allowed");
17596 
17597         img->update();
17598 
17599         EXITFUNC;
17600         return img2;
17601 }
17602 
17603 void Util::mul_scalar(EMData* img, float scalar)
17604 {
17605         ENTERFUNC;
17606         /* Exception Handle */
17607         if (!img) {
17608                 throw NullPointerException("NULL input image");
17609         }
17610         /* ============  output = scalar*input  ================== */
17611 
17612         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17613         size_t size = (size_t)nx*ny*nz;
17614         float *img_ptr  =img->get_data();
17615         for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17616         img->update();
17617 
17618         EXITFUNC;
17619 }
17620 
17621 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17622 {
17623         ENTERFUNC;
17624         /* Exception Handle */
17625         if (!img) {
17626                 throw NullPointerException("NULL input image");
17627         }
17628         /* ==============   img += scalar*img1   ================ */
17629 
17630         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17631         size_t size = (size_t)nx*ny*nz;
17632         float *img_ptr  =img->get_data();
17633         float *img1_ptr = img1->get_data();
17634         for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17635         img1->update();
17636 
17637         EXITFUNC;
17638 }
17639 
17640 void Util::add_img(EMData* img, EMData* img1)
17641 {
17642         ENTERFUNC;
17643         /* Exception Handle */
17644         if (!img || !img1) {
17645                 throw NullPointerException("NULL input image");
17646         }
17647         /* ========= img += img1 ===================== */
17648 
17649         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17650         size_t size = (size_t)nx*ny*nz;
17651         float *img_ptr  = img->get_data();
17652         float *img1_ptr = img1->get_data();
17653         for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17654         img->update();
17655 
17656         EXITFUNC;
17657 }
17658 
17659 void Util::add_img_abs(EMData* img, EMData* img1)
17660 {
17661         ENTERFUNC;
17662         /* Exception Handle */
17663         if (!img) {
17664                 throw NullPointerException("NULL input image");
17665         }
17666         /* ========= img += img1 ===================== */
17667 
17668         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17669         size_t size = (size_t)nx*ny*nz;
17670         float *img_ptr  = img->get_data();
17671         float *img1_ptr = img1->get_data();
17672         for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17673         img->update();
17674 
17675         EXITFUNC;
17676 }
17677 
17678 void Util::add_img2(EMData* img, EMData* img1)
17679 {
17680         ENTERFUNC;
17681         /* Exception Handle */
17682         if (!img) {
17683                 throw NullPointerException("NULL input image");
17684         }
17685         /* ========= img += img1**2 ===================== */
17686 
17687         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17688         size_t size = (size_t)nx*ny*nz;
17689         float *img_ptr  = img->get_data();
17690         float *img1_ptr = img1->get_data();
17691         if(img->is_complex()) {
17692                 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] ;
17693         } else {
17694                 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17695         }
17696         img->update();
17697 
17698         EXITFUNC;
17699 }
17700 
17701 void Util::sub_img(EMData* img, EMData* img1)
17702 {
17703         ENTERFUNC;
17704         /* Exception Handle */
17705         if (!img) {
17706                 throw NullPointerException("NULL input image");
17707         }
17708         /* ========= img -= img1 ===================== */
17709 
17710         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17711         size_t size = (size_t)nx*ny*nz;
17712         float *img_ptr  = img->get_data();
17713         float *img1_ptr = img1->get_data();
17714         for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17715         img->update();
17716 
17717         EXITFUNC;
17718 }
17719 
17720 void Util::mul_img(EMData* img, EMData* img1)
17721 {
17722         ENTERFUNC;
17723         /* Exception Handle */
17724         if (!img) {
17725                 throw NullPointerException("NULL input image");
17726         }
17727         /* ========= img *= img1 ===================== */
17728 
17729         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17730         size_t size = (size_t)nx*ny*nz;
17731         float *img_ptr  = img->get_data();
17732         float *img1_ptr = img1->get_data();
17733         if(img->is_complex()) {
17734                 for (size_t i=0; i<size; i+=2) {
17735                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17736                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17737                         img_ptr[i]   = tmp;
17738 
17739                 }
17740         } else {
17741                 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17742         }
17743         img->update();
17744 
17745         EXITFUNC;
17746 }
17747 
17748 void Util::div_img(EMData* img, EMData* img1)
17749 {
17750         ENTERFUNC;
17751         /* Exception Handle */
17752         if (!img) {
17753                 throw NullPointerException("NULL input image");
17754         }
17755         /* ========= img /= img1 ===================== */
17756 
17757         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17758         size_t size = (size_t)nx*ny*nz;
17759         float *img_ptr  = img->get_data();
17760         float *img1_ptr = img1->get_data();
17761         if(img->is_complex()) {
17762                 float  sq2;
17763                 for (size_t i=0; i<size; i+=2) {
17764                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17765                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17766                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17767                         img_ptr[i]   = tmp;
17768                 }
17769         } else {
17770                 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17771         }
17772         img->update();
17773 
17774         EXITFUNC;
17775 }
17776 
17777 void Util::div_filter(EMData* img, EMData* img1)
17778 {
17779         ENTERFUNC;
17780         /* Exception Handle */
17781         if (!img) {
17782                 throw NullPointerException("NULL input image");
17783         }
17784         /* ========= img /= img1 ===================== */
17785 
17786         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17787         size_t size = (size_t)nx*ny*nz;
17788         float *img_ptr  = img->get_data();
17789         float *img1_ptr = img1->get_data();
17790         if(img->is_complex()) {
17791                 for (size_t i=0; i<size; i+=2) {
17792                         if(img1_ptr[i] > 1.e-10f) {
17793                                 img_ptr[i]   /= img1_ptr[i];
17794                                 img_ptr[i+1] /= img1_ptr[i];
17795                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17796                 }
17797         } else throw ImageFormatException("Only Fourier image allowed");
17798 
17799         img->update();
17800 
17801         EXITFUNC;
17802 }
17803 
17804 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17805 
17806 EMData* Util::pack_complex_to_real(EMData* img)
17807 {
17808         ENTERFUNC;
17809         /* Exception Handle */
17810         if (!img) {
17811                 throw NullPointerException("NULL input image");
17812         }
17813         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17814                               output is img packed into real image with Friedel part added,   ================ */
17815 
17816         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17817         int nx = nxo - 2 + img->is_fftodd();
17818         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17819         int nyt, nzt;
17820         int nx2 = nx/2;
17821         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17822         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17823         int nx2p = nx2+nx%2;
17824         int ny2p = ny2+ny%2;
17825         int nz2p = nz2+nz%2;
17826         EMData& power = *(new EMData()); // output image
17827         power.set_size(nx, ny, nz);
17828         power.set_array_offsets(-nx2,-ny2,-nz2);
17829         //img->set_array_offsets(1,1,1);
17830         float *img_ptr  = img->get_data();
17831         for (int iz = 1; iz <= nz; iz++) {
17832                 int jz=iz-1;
17833                 if(jz>=nz2p) jz=jz-nzt;
17834                 for (int iy = 1; iy <= ny; iy++) {
17835                         int jy=iy-1;
17836                         if(jy>=ny2p) jy=jy-nyt;
17837                         for (int ix = 1; ix <= lsd2; ix++) {
17838                                 int jx=ix-1;
17839                                 if(jx>=nx2p) jx=jx-nx;
17840                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17841                         }
17842                 }
17843         }
17844 //  Create the Friedel related half
17845         int  nzb, nze, nyb, nye, nxb, nxe;
17846         nxb =-nx2+(nx+1)%2;
17847         nxe = nx2-(nx+1)%2;
17848         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17849         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17850         for (int iz = nzb; iz <= nze; iz++) {
17851                 for (int iy = nyb; iy <= nye; iy++) {
17852                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17853                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17854                         }
17855                 }
17856         }
17857         if(ny2 != 0)  {
17858                 if(nz2 != 0)  {
17859                         if(nz%2 == 0) {  //if nz even, fix the first slice
17860                                 for (int iy = nyb; iy <= nye; iy++) {
17861                                         for (int ix = nxb; ix <= -1; ix++) {
17862                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17863                                         }
17864                                 }
17865                                 if(ny%2 == 0) {  //if ny even, fix the first line
17866                                         for (int ix = nxb; ix <= -1; ix++) {
17867                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17868                                         }
17869                                 }
17870                         }
17871                 }
17872                 if(ny%2 == 0) {  //if ny even, fix the first column
17873                         for (int iz = nzb; iz <= nze; iz++) {
17874                                 for (int ix = nxb; ix <= -1; ix++) {
17875                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17876                                 }
17877                         }
17878                 }
17879 
17880         }
17881         power.update();
17882         power.set_array_offsets(0,0,0);
17883         return &power;
17884 }
17885 #undef  img_ptr
17886 
17887 float Util::ang_n(float peakp, string mode, int maxrin)
17888 {
17889     if (mode == "f" || mode == "F")
17890         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17891     else
17892         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17893 }
17894 
17895 
17896 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17897 {
17898     float* data = ring->get_data();
17899     float av=0.0;
17900     float sq=0.0;
17901     float nn=0.0;
17902     int nring = numr.size()/3;
17903     for( int i=0; i < nring; ++i )
17904     {
17905         int numr3i = numr[3*i+2];
17906         int numr2i = numr[3*i+1]-1;
17907         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17908         for( int j=0; j < numr3i; ++j )
17909         {
17910             int jc = numr2i+j;
17911             av += data[jc] * w;
17912             sq += data[jc] * data[jc] * w;
17913             nn += w;
17914         }
17915     }
17916 
17917     float avg = av/nn;
17918     float sgm = sqrt( (sq-av*av/nn)/nn );
17919     size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17920     for( size_t i=0; i < n; ++i )
17921     {
17922         data[i] -= avg;
17923         data[i] /= sgm;
17924     }
17925 
17926     ring->update();
17927 }
17928 
17929 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17930                 float xrng, float yrng, float step, string mode,
17931                 vector<int>numr, float cnx, float cny) {
17932 
17933     // Manually extract.
17934 /*    vector< EMAN::EMData* > crefim;
17935     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17936     crefim.reserve(crefim_len);
17937 
17938     for(std::size_t i=0;i<crefim_len;i++) {
17939         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17940         crefim.push_back(proxy());
17941     }
17942 */
17943 
17944         size_t crefim_len = crefim.size();
17945 
17946         int   ky = int(2*yrng/step+0.5)/2;
17947         int   kx = int(2*xrng/step+0.5)/2;
17948         int   iref, nref=0, mirror=0;
17949         float iy, ix, sx=0, sy=0;
17950         float peak = -1.0E23f;
17951         float ang=0.0f;
17952         for (int i = -ky; i <= ky; i++) {
17953                 iy = i * step ;
17954                 for (int j = -kx; j <= kx; j++) {
17955                         ix = j*step ;
17956                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17957 
17958                         Normalize_ring( cimage, numr );
17959 
17960                         Frngs(cimage, numr);
17961                         //  compare with all reference images
17962                         // for iref in xrange(len(crefim)):
17963                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17964                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17965                                 double qn = retvals["qn"];
17966                                 double qm = retvals["qm"];
17967                                 if(qn >= peak || qm >= peak) {
17968                                         sx = -ix;
17969                                         sy = -iy;
17970                                         nref = iref;
17971                                         if (qn >= qm) {
17972                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17973                                                 peak = static_cast<float>(qn);
17974                                                 mirror = 0;
17975                                         } else {
17976                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17977                                                 peak = static_cast<float>(qm);
17978                                                 mirror = 1;
17979                                         }
17980                                 }
17981                         }  delete cimage; cimage = 0;
17982                 }
17983         }
17984         float co, so, sxs, sys;
17985         co = static_cast<float>( cos(ang*pi/180.0) );
17986         so = static_cast<float>( -sin(ang*pi/180.0) );
17987         sxs = sx*co - sy*so;
17988         sys = sx*so + sy*co;
17989         vector<float> res;
17990         res.push_back(ang);
17991         res.push_back(sxs);
17992         res.push_back(sys);
17993         res.push_back(static_cast<float>(mirror));
17994         res.push_back(static_cast<float>(nref));
17995         res.push_back(peak);
17996         return res;
17997 }
17998 
17999 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
18000                 float xrng, float yrng, float step, string mode,
18001                 vector<int>numr, float cnx, float cny) {
18002 
18003         size_t crefim_len = crefim.size();
18004 
18005         int   ky = int(2*yrng/step+0.5)/2;
18006         int   kx = int(2*xrng/step+0.5)/2;
18007         float iy, ix;
18008         vector<float> peak(crefim_len*5, -1.0e23f);
18009         for (int i = -ky; i <= ky; i++) {
18010                 iy = i * step ;
18011                 for (int j = -kx; j <= kx; j++) {
18012                         ix = j*step ;
18013                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18014                         Normalize_ring( cimage, numr );
18015                         Frngs(cimage, numr);
18016                         for (int iref = 0; iref < (int)crefim_len; iref++) {
18017                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18018                                 double qn = retvals["qn"];
18019                                 double qm = retvals["qm"];
18020                                 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
18021                                         if (qn >= qm) {
18022                                                 peak[iref*5] = static_cast<float>(qn);
18023                                                 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18024                                                 peak[iref*5+2] = -ix;
18025                                                 peak[iref*5+3] = -iy;
18026                                                 peak[iref*5+4] = 0;
18027                                         } else {
18028                                                 peak[iref*5] = static_cast<float>(qm);
18029                                                 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18030                                                 peak[iref*5+2] = -ix;
18031                                                 peak[iref*5+3] = -iy;
18032                                                 peak[iref*5+4] = 1;
18033                                         }
18034                                 }
18035                         }  delete cimage; cimage = 0;
18036                 }
18037         }
18038         for (int iref = 0; iref < (int)crefim_len; iref++) {
18039                 float ang = peak[iref*5+1];
18040                 float sx = peak[iref*5+2];
18041                 float sy = peak[iref*5+3];
18042                 float co =  cos(ang*pi/180.0);
18043                 float so = -sin(ang*pi/180.0);
18044                 float sxs = sx*co - sy*so;
18045                 float sys = sx*so + sy*co;
18046                 peak[iref*5+2] = sxs;
18047                 peak[iref*5+3] = sys;
18048         }
18049         return peak;
18050 }
18051 
18052 struct peak_table {
18053         float value;
18054         int index;
18055         bool operator<(const peak_table& b) const { return value > b.value; }
18056 };
18057 
18058 vector<int> Util::assign_groups(const vector<float>& d, int nref, int nima) {
18059 
18060         int kt = nref;
18061         unsigned int maxasi = nima/nref;
18062         vector< vector<int> > id_list;
18063         id_list.resize(nref);
18064         int group, ima;
18065 
18066         peak_table* dd = new peak_table[nref*nima];
18067         for (int i=0; i<nref*nima; i++)  {
18068                 dd[i].value = d[i];
18069                 dd[i].index = i;
18070         }
18071         sort(dd, dd+nref*nima);
18072         int begin = 0;
18073 
18074         bool* del_row = new bool[nref];
18075         for (int i=0; i<nref; i++) del_row[i] = false;
18076         bool* del_column = new bool[nima];
18077         for (int i=0; i<nima; i++) del_column[i] = false;
18078         while (kt > 0) {
18079                 bool flag = true;
18080                 while (flag) {
18081                         int l = dd[begin].index;
18082                         group = l/nima;
18083                         ima = l%nima;
18084                         if (del_column[ima] || del_row[group]) begin++;
18085                         else flag = false;
18086                 }
18087 
18088                 id_list[group].push_back(ima);
18089                 if (kt > 1) {
18090                         if (id_list[group].size() < maxasi) group = -1;
18091                         else kt -= 1;
18092                 } else {
18093                         if (id_list[group].size() < maxasi+nima%nref) group = -1;
18094                         else kt -= 1;
18095                 }
18096                 del_column[ima] = true;
18097                 if (group != -1) {
18098                         del_row[group] = true;
18099                 }
18100         }
18101 
18102         vector<int> id_list_1; 
18103         for (int iref=0; iref<nref; iref++)
18104                 for (unsigned int im=0; im<maxasi; im++)
18105                         id_list_1.push_back(id_list[iref][im]);
18106         for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
18107                         id_list_1.push_back(id_list[group][im]);
18108         id_list_1.push_back(group);
18109 
18110         delete[] del_row;
18111         delete[] del_column;
18112         delete[] dd;
18113         return id_list_1;
18114 }
18115 
18116 int Util::nearest_ang(const vector<float>& vecref, float x, float y, float z) {
18117         float best_v = -1.0f;
18118         int best_i = -1;
18119         
18120         for (unsigned int i=0; i<vecref.size()/3; i++) {
18121                 float v = abs(vecref[i*3]*x+vecref[i*3+1]*y+vecref[i*3+2]*z);
18122                 if (v > best_v) {
18123                         best_v = v;
18124                         best_i = i;
18125                 }
18126         }
18127         return best_i;
18128 }
18129 
18130 struct d_ang {
18131         float d;
18132         int i;
18133         int mirror;
18134         d_ang(float _d, int _i, int _m):d(_d), i(_i), mirror(_m) {}
18135         bool operator<(const d_ang& a) const { return d < a.d || (d == a.d && i < a.i); }
18136 };
18137 
18138 vector<int> Util::assign_projangles(const vector<float>& projangles, const vector<float>& refangles) {
18139         int nref = refangles.size()/2;
18140         int nproj = projangles.size()/2;
18141         vector<int> asg(nproj);
18142         vector<float> vecref(nref*3);
18143         for (int i=0; i<nref; i++)
18144                 getvec(refangles[i*2], refangles[i*2+1], vecref[i*3], vecref[i*3+1], vecref[i*3+2]);
18145         for (int i=0; i<nproj; i++) {
18146                 float x, y, z;
18147                 getvec(projangles[i*2], projangles[i*2+1], x, y, z);
18148                 asg[i] = nearest_ang(vecref, x, y, z);
18149         }
18150         return asg;
18151 }
18152 
18153 
18154 vector<int> Util::nearestk_to_refdir(const vector<float>& projangles, const vector<float>& refangles, const int howmany) {
18155         int nref = refangles.size()/2;
18156         int nproj = projangles.size()/2;
18157         vector<int> asg(howmany*nref);
18158         vector<float> vecproj(nproj*3);
18159         for (int i=0; i<nproj; i++)
18160                 getvec(projangles[i*2], projangles[i*2+1], vecproj[i*3], vecproj[i*3+1], vecproj[i*3+2]);
18161 
18162 
18163         vector<bool> taken(nproj);
18164         for (int k=0; k<nref; k++) {
18165                 for (int i=0; i<nproj; i++)  taken[i] = true;
18166                 float x, y, z;
18167                 getvec(refangles[k*2], refangles[k*2+1], x, y, z);
18168                 for (int h=0; h<howmany; h++) {
18169                         float best_v = -1.0f;
18170                         int best_i = -1;
18171                         for (int i=0; i<nproj; i++) {
18172                                 if( taken[i] ) {
18173                                         float v = abs(vecproj[i*3]*x+vecproj[i*3+1]*y+vecproj[i*3+2]*z);
18174                                         if (v > best_v) {
18175                                                 best_v = v;
18176                                                 best_i = i;
18177                                         }
18178                                 }
18179                         }
18180                         asg[k*howmany + h] = best_i;
18181                         taken[best_i] = false;
18182                 }
18183         }
18184         return asg;
18185 }
18186 
18187 
18188 vector<int> Util::group_proj_by_phitheta(const vector<float>& projangles, const vector<float>& ref_ang, const int img_per_grp) {
18189         float c = 100.0;
18190         int L = max(100, img_per_grp);
18191         int N = projangles.size()/2;
18192 
18193         int sz = ref_ang.size();
18194         int nref1 = ref_ang[sz-4];
18195         int nref2 = ref_ang[sz-3];
18196         int nref3 = ref_ang[sz-2];
18197         int nref4 = ref_ang[sz-1];
18198         int nref;
18199 
18200         set<int> pt;
18201         for (int i=0; i<N; i++) pt.insert(i);
18202         vector<float> v(N*3, 0.0f);
18203         for (int i=0; i<N; i++) 
18204                 getvec(projangles[i*2], projangles[i*2+1], v[i*3], v[i*3+1], v[i*3+2], 1);
18205 
18206         int previous_group = -1;
18207         int previous_zone = 5;
18208         int max_group = 0;
18209         vector<float> ref_ang_list;
18210         vector<float> diff_table;
18211         map<int, int> diff_table_index;
18212         vector<int> proj_list;
18213         vector<int> sg;
18214         vector<int> remain_index;
18215         vector<int> asg;
18216         int mirror;
18217         for (int grp=0; grp<N/img_per_grp; grp++) {
18218                 int N_remain = N-grp*img_per_grp;
18219                 assert(N_remain == static_cast<int>(pt.size()));
18220                 if (N_remain >= nref4*L) {
18221                         if (previous_zone > 4) {
18222                                 ref_ang_list.resize(nref4*2);
18223                                 for (int i=0; i<nref4*2; i++)  ref_ang_list[i] = ref_ang[(nref1+nref2+nref3)*2+i];
18224                                 nref = nref4;
18225                                 previous_group = -1;
18226                                 previous_zone = 4;
18227                         }
18228                 } else if (N_remain >= nref3*L) {
18229                         if (previous_zone > 3) {
18230                                 ref_ang_list.resize(nref3*2);
18231                                 for (int i=0; i<nref3*2; i++)  ref_ang_list[i] = ref_ang[(nref1+nref2)*2+i];
18232                                 nref = nref3;
18233                                 previous_group = -1;
18234                                 previous_zone = 3;
18235                         }
18236                 } else if (N_remain >= nref2*L) {
18237                         if (previous_zone > 2) {
18238                                 ref_ang_list.resize(nref2*2);
18239                                 for (int i=0; i<nref2*2; i++)  ref_ang_list[i] = ref_ang[nref1*2+i];
18240                                 nref = nref2;
18241                                 previous_group = -1;
18242                                 previous_zone = 2;
18243                         }
18244                 } else if (N_remain >= nref1*L) {
18245                         if (previous_zone > 1) {
18246                                 ref_ang_list.resize(nref1*2);
18247                                 for (int i=0; i<nref1*2; i++)  ref_ang_list[i] = ref_ang[i];
18248                                 nref = nref1;
18249                                 previous_group = -1;
18250                                 previous_zone = 1;
18251                         }
18252                 } else if (previous_zone > 0) {
18253                         previous_group = -1;
18254                         previous_zone = 0;
18255                 }
18256 
18257                 vector<int> index;
18258                 if (N_remain >=  nref1*L) {
18259                         if (previous_group == -1) { // which means it just changed zone
18260                                 vector<float> proj_ang_list(N_remain*2, 0.0f);
18261                                 remain_index.resize(N_remain);
18262                                 int l = 0;
18263                                 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18264                                         remain_index[l] = (*si);
18265                                         proj_ang_list[l*2] = projangles[(*si)*2];
18266                                         proj_ang_list[l*2+1] = projangles[(*si)*2+1];
18267                                         l++; 
18268                                 }
18269                                 assert(N_remain == l);
18270                                 asg = assign_projangles(proj_ang_list, ref_ang_list);
18271                                 sg.resize(nref);
18272                                 for (int i=0; i<nref; i++) sg[i] = 0;
18273                                 for (int i=0; i<N_remain; i++)  sg[asg[i]]++;
18274                         }
18275                         int max_group_size = 0;
18276                         for (int i=0; i<nref; i++)
18277                                 if (sg[i] > max_group_size)     { max_group_size = sg[i]; max_group = i; }
18278                         for (unsigned int i=0; i<remain_index.size(); i++)
18279                                 if (asg[i] == max_group)  index.push_back(remain_index[i]);
18280                 } else {
18281                         for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) 
18282                                 index.push_back(*si);
18283                         max_group = 0;
18284                 }
18285                         
18286                 int Nn = index.size();
18287                 if (max_group != previous_group) {
18288                         diff_table.resize(Nn*Nn);
18289                         diff_table_index.clear();
18290                         for (int i=0; i<Nn-1; i++)
18291                                 for (int j=i+1; j<Nn; j++) {
18292                                         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);
18293                                         float q = exp(-c*pow(diff/180.0f*static_cast<float>(M_PI), 2.0f));
18294                                         diff_table[i*Nn+j] = q;
18295                                         diff_table[j*Nn+i] = q;
18296                                 }
18297                         for (int i=0; i<Nn; i++)  {
18298                                 diff_table[i*Nn+i] = 0.0f;      // diagonal values
18299                                 diff_table_index[index[i]] = i; 
18300                         }
18301                         previous_group = max_group;
18302                 } 
18303 
18304                 int diff_table_size = static_cast<int>(sqrt((float)diff_table.size())+0.5f);
18305                 float max_density = -1;
18306                 int max_density_i = -1;
18307                 for (int i=0; i<Nn; i++) {
18308                         float s = 0.0f;
18309                         int z = diff_table_index[index[i]];
18310                         for (int j=0; j<diff_table_size; j++)  s += diff_table[z*diff_table_size+j];
18311                         if (s > max_density) {
18312                                 max_density = s;
18313                                 max_density_i = i;
18314                         }
18315                 }
18316 
18317                 vector<d_ang> dang(Nn, d_ang(0.0, 0, 0));
18318                 for (int i=0; i<Nn; i++) {
18319                         dang[i].d = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2], 
18320                           v[index[max_density_i]*3], v[index[max_density_i]*3+1], v[index[max_density_i]*3+2], mirror);
18321                         dang[i].mirror = mirror;
18322                         dang[i].i = i;
18323                 }
18324                 dang[max_density_i].d = -1;
18325                 sort(dang.begin(), dang.end());         
18326 
18327                 for (int i=0; i<img_per_grp; i++) {
18328                         int idd = index[dang[i].i];
18329                         mirror = dang[i].mirror;
18330                         for (unsigned int j=0; j<remain_index.size(); j++)
18331                                 if (idd == remain_index[j]) asg[j] = -1;
18332                         for (int j=0; j<diff_table_size; j++) {
18333                                 diff_table[diff_table_index[idd]*diff_table_size+j] = 0.0f;
18334                                 diff_table[diff_table_index[idd]+diff_table_size*j] = 0.0f;
18335                         }
18336                         proj_list.push_back(mirror*idd);
18337                         pt.erase(idd);
18338                 }
18339                 sg[max_group] -= img_per_grp;
18340         }
18341         for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18342                 proj_list.push_back(*si);
18343         }
18344         return proj_list;
18345 }
18346 
18347 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18348                 float xrng, float yrng, float step, string mode,
18349                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18350 
18351     // Manually extract.
18352 /*    vector< EMAN::EMData* > crefim;
18353     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18354     crefim.reserve(crefim_len);
18355 
18356     for(std::size_t i=0;i<crefim_len;i++) {
18357         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18358         crefim.push_back(proxy());
18359     }
18360 */
18361 
18362         size_t crefim_len = crefim.size();
18363 
18364         int   ky = int(2*yrng/step+0.5)/2;
18365         int   kx = int(2*xrng/step+0.5)/2;
18366         int   iref, nref=0, mirror=0;
18367         float iy, ix, sx=0, sy=0;
18368         float peak = -1.0E23f;
18369         float ang=0.0f;
18370         for (int i = -ky; i <= ky; i++) {
18371                 iy = i * step ;
18372                 for (int j = -kx; j <= kx; j++) {
18373                         ix = j*step ;
18374                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18375 
18376                         Normalize_ring( cimage, numr );
18377 
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                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18383                                 double qn = retvals["qn"];
18384                                 double qm = retvals["qm"];
18385                                 if(qn >= peak || qm >= peak) {
18386                                         sx = -ix;
18387                                         sy = -iy;
18388                                         nref = iref;
18389                                         if (qn >= qm) {
18390                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18391                                                 peak = static_cast<float>(qn);
18392                                                 mirror = 0;
18393                                         } else {
18394                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18395                                                 peak = static_cast<float>(qm);
18396                                                 mirror = 1;
18397                                         }
18398                                 }
18399                         }  delete cimage; cimage = 0;
18400                 }
18401         }
18402         float co, so, sxs, sys;
18403         co = static_cast<float>( cos(ang*pi/180.0) );
18404         so = static_cast<float>( -sin(ang*pi/180.0) );
18405         sxs = sx*co - sy*so;
18406         sys = sx*so + sy*co;
18407         vector<float> res;
18408         res.push_back(ang);
18409         res.push_back(sxs);
18410         res.push_back(sys);
18411         res.push_back(static_cast<float>(mirror));
18412         res.push_back(static_cast<float>(nref));
18413         res.push_back(peak);
18414         return res;
18415 }
18416 
18417 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18418                 float xrng, float yrng, float step, string mode,
18419                 vector< int >numr, float cnx, float cny) {
18420 
18421     // Manually extract.
18422 /*    vector< EMAN::EMData* > crefim;
18423     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18424     crefim.reserve(crefim_len);
18425 
18426     for(std::size_t i=0;i<crefim_len;i++) {
18427         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18428         crefim.push_back(proxy());
18429     }
18430 */
18431         size_t crefim_len = crefim.size();
18432 
18433         int   ky = int(2*yrng/step+0.5)/2;
18434         int   kx = int(2*xrng/step+0.5)/2;
18435         int   iref, nref=0;
18436         float iy, ix, sx=0, sy=0;
18437         float peak = -1.0E23f;
18438         float ang=0.0f;
18439         for (int i = -ky; i <= ky; i++) {
18440                 iy = i * step ;
18441                 for (int j = -kx; j <= kx; j++) {
18442                         ix = j*step ;
18443                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18444                         Frngs(cimage, numr);
18445                         //  compare with all reference images
18446                         // for iref in xrange(len(crefim)):
18447                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18448                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18449                                 double qn = retvals["qn"];
18450                                 if(qn >= peak) {
18451                                         sx = -ix;
18452                                         sy = -iy;
18453                                         nref = iref;
18454                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18455                                         peak = static_cast<float>(qn);
18456                                 }
18457                         }  delete cimage; cimage = 0;
18458                 }
18459         }
18460         float co, so, sxs, sys;
18461         co = static_cast<float>( cos(ang*pi/180.0) );
18462         so = static_cast<float>( -sin(ang*pi/180.0) );
18463         sxs = sx*co - sy*so;
18464         sys = sx*so + sy*co;
18465         vector<float> res;
18466         res.push_back(ang);
18467         res.push_back(sxs);
18468         res.push_back(sys);
18469         res.push_back(static_cast<float>(nref));
18470         res.push_back(peak);
18471         return res;
18472 }
18473 
18474 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18475                 float xrng, float yrng, float step, float ant, string mode,
18476                 vector<int>numr, float cnx, float cny) {
18477 
18478     // Manually extract.
18479 /*    vector< EMAN::EMData* > crefim;
18480     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18481     crefim.reserve(crefim_len);
18482 
18483     for(std::size_t i=0;i<crefim_len;i++) {
18484         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18485         crefim.push_back(proxy());
18486     }
18487 */
18488         size_t crefim_len = crefim.size();
18489         const float qv = static_cast<float>( pi/180.0 );
18490 
18491         Transform * t = image->get_attr("xform.projection");
18492         Dict d = t->get_params("spider");
18493         if(t) {delete t; t=0;}
18494         float phi = d["phi"];
18495         float theta = d["theta"];
18496         int   ky = int(2*yrng/step+0.5)/2;
18497         int   kx = int(2*xrng/step+0.5)/2;
18498         int   iref, nref=0, mirror=0;
18499         float iy, ix, sx=0, sy=0;
18500         float peak = -1.0E23f;
18501         float ang=0.0f;
18502         float imn1 = sin(theta*qv)*cos(phi*qv);
18503         float imn2 = sin(theta*qv)*sin(phi*qv);
18504         float imn3 = cos(theta*qv);
18505         vector<float> n1(crefim_len);
18506         vector<float> n2(crefim_len);
18507         vector<float> n3(crefim_len);
18508         for ( iref = 0; iref < (int)crefim_len; iref++) {
18509                         n1[iref] = crefim[iref]->get_attr("n1");
18510                         n2[iref] = crefim[iref]->get_attr("n2");
18511                         n3[iref] = crefim[iref]->get_attr("n3");
18512         }
18513         for (int i = -ky; i <= ky; i++) {
18514             iy = i * step ;
18515             for (int j = -kx; j <= kx; j++) {
18516                 ix = j*step;
18517                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18518 
18519                 Normalize_ring( cimage, numr );
18520 
18521                 Frngs(cimage, numr);
18522                 //  compare with all reference images
18523                 // for iref in xrange(len(crefim)):
18524                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18525                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18526                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18527                                 double qn = retvals["qn"];
18528                                 double qm = retvals["qm"];
18529                                 if(qn >= peak || qm >= peak) {
18530                                         sx = -ix;
18531                                         sy = -iy;
18532                                         nref = iref;
18533                                         if (qn >= qm) {
18534                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18535                                                 peak = static_cast<float>( qn );
18536                                                 mirror = 0;
18537                                         } else {
18538                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18539                                                 peak = static_cast<float>( qm );
18540                                                 mirror = 1;
18541                                         }
18542                                 }
18543                         }
18544                 }  delete cimage; cimage = 0;
18545             }
18546         }
18547         float co, so, sxs, sys;
18548         if(peak == -1.0E23) {
18549                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18550                 nref = -1;
18551         } else {
18552                 co =  cos(ang*qv);
18553                 so = -sin(ang*qv);
18554                 sxs = sx*co - sy*so;
18555                 sys = sx*so + sy*co;
18556         }
18557         vector<float> res;
18558         res.push_back(ang);
18559         res.push_back(sxs);
18560         res.push_back(sys);
18561         res.push_back(static_cast<float>(mirror));
18562         res.push_back(static_cast<float>(nref));
18563         res.push_back(peak);
18564         return res;
18565 }
18566 
18567 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18568                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18569                 vector<int>numr, float cnx, float cny) {
18570 
18571     // Manually extract.
18572 /*    vector< EMAN::EMData* > crefim;
18573     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18574     crefim.reserve(crefim_len);
18575 
18576     for(std::size_t i=0;i<crefim_len;i++) {
18577         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18578         crefim.push_back(proxy());
18579     }
18580 */
18581         size_t crefim_len = crefim.size();
18582         const float qv = static_cast<float>(pi/180.0);
18583 
18584         Transform* t = image->get_attr("xform.projection");
18585         Dict d = t->get_params("spider");
18586         if(t) {delete t; t=0;}
18587         float phi = d["phi"];
18588         float theta = d["theta"];
18589         float psi = d["psi"];
18590         int ky = int(2*yrng/step+0.5)/2;
18591         int kx = int(2*xrng/step+0.5)/2;
18592         int iref, nref = 0, mirror = 0;
18593         float iy, ix, sx = 0, sy = 0;
18594         float peak = -1.0E23f;
18595         float ang = 0.0f;
18596         float imn1 = sin(theta*qv)*cos(phi*qv);
18597         float imn2 = sin(theta*qv)*sin(phi*qv);
18598         float imn3 = cos(theta*qv);
18599         vector<float> n1(crefim_len);
18600         vector<float> n2(crefim_len);
18601         vector<float> n3(crefim_len);
18602         for (iref = 0; iref < (int)crefim_len; iref++) {
18603                         n1[iref] = crefim[iref]->get_attr("n1");
18604                         n2[iref] = crefim[iref]->get_attr("n2");
18605                         n3[iref] = crefim[iref]->get_attr("n3");
18606         }
18607         bool nomirror = (theta<90.0) || (theta==90.0);
18608         if (!nomirror) {
18609                 phi = fmod(phi+540.0f, 360.0f);
18610                 theta = 180-theta;
18611                 psi = fmod(540.0f-psi, 360.0f);
18612         } else { psi = fmod(360.0f-psi, 360.0f); }
18613         for (int i = -ky; i <= ky; i++) {
18614             iy = i * step ;
18615             for (int j = -kx; j <= kx; j++) {
18616                 ix = j*step;
18617                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18618 
18619                 Normalize_ring(cimage, numr);
18620 
18621                 Frngs(cimage, numr);
18622                 //  compare with all reference images
18623                 // for iref in xrange(len(crefim)):
18624                 for (iref = 0; iref < (int)crefim_len; iref++) {
18625                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18626                                 float refpsi = crefim[iref]->get_attr("psi");
18627                                 if (nomirror) {
18628                                 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi+refpsi, 360.0), 0, psi_max);
18629                                 double qn = retvals["qn"];
18630                                 if (qn >= peak) {
18631                                                 sx = -ix;
18632                                                 sy = -iy;
18633                                                 nref = iref;
18634                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18635                                                 peak = static_cast<float>(qn);
18636                                                 mirror = 0;
18637                                         }
18638                                 } else {
18639                                 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi-refpsi, 360.0), 1, psi_max);
18640                                 double qn = retvals["qn"];
18641                                 if (qn >= peak) {
18642                                                 sx = -ix;
18643                                                 sy = -iy;
18644                                                 nref = iref;
18645                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18646                                                 peak = static_cast<float>(qn);
18647                                                 mirror = 1;
18648                                         }
18649                                 }
18650                     }
18651                 }  delete cimage; cimage = 0;
18652             }
18653         }
18654         float co, so, sxs, sys;
18655         if(peak == -1.0E23) {
18656                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18657                 nref = -1;
18658         } else {
18659                 co =  cos(ang*qv);
18660                 so = -sin(ang*qv);
18661                 sxs = sx*co - sy*so;
18662                 sys = sx*so + sy*co;
18663         }
18664         vector<float> res;
18665         res.push_back(ang);
18666         res.push_back(sxs);
18667         res.push_back(sys);
18668         res.push_back(static_cast<float>(mirror));
18669         res.push_back(static_cast<float>(nref));
18670         res.push_back(peak);
18671         return res;
18672 }
18673 
18674 
18675 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18676                 float xrng, float yrng, float step, float psi_max, string mode,
18677                 vector<int>numr, float cnx, float cny, int ynumber) {
18678         
18679         size_t crefim_len = crefim.size();
18680 
18681         int   iref, nref=0, mirror=0;
18682         float iy, ix, sx=0, sy=0;
18683         float peak = -1.0E23f;
18684         float ang=0.0f;
18685         int   kx = int(2*xrng/step+0.5)/2;
18686         //if ynumber==-1, use the old code which process x and y direction equally.
18687         //if ynumber is given, it should be even. We need to check whether it is zero
18688 
18689         int ky;
18690         float stepy;
18691         int kystart;
18692         
18693         if (ynumber == -1){
18694             ky = int(2*yrng/step+0.5)/2;
18695             stepy = step;
18696             kystart = -ky;
18697         }
18698         else if(ynumber == 0){
18699              ky = 0;
18700                  stepy = 0.0f;
18701                  kystart = ky;
18702         }
18703         else {
18704             ky = int(ynumber/2);                
18705                 stepy=2*yrng/ynumber;
18706                 kystart = -ky + 1;    
18707         }
18708         //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18709         for (int i = kystart; i <= ky; i++) {
18710                 iy = i * stepy ;
18711                 for (int j = -kx; j <= kx; j++) {
18712                         ix = j*step ;
18713                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18714 
18715                         Normalize_ring( cimage, numr );
18716 
18717                         Frngs(cimage, numr);
18718                         //  compare with all reference images
18719                         // for iref in xrange(len(crefim)):
18720                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18721                                 Dict retvals_0 = Crosrng_psi(crefim[iref], cimage, numr, 0, psi_max);
18722                                 Dict retvals_180 = Crosrng_psi(crefim[iref], cimage, numr, 180, psi_max);
18723                                 double qn_0 = retvals_0["qn"];
18724                                 double qn_180 = retvals_180["qn"];
18725                                 double qm_0 = retvals_0["qm"];
18726                                 double qm_180 = retvals_180["qm"];
18727                                 double qn;
18728                                 double qm;
18729                                 bool qn_is_zero = false;
18730                                 bool qm_is_zero = false;
18731                                 
18732                                 if (qn_0 >= qn_180){
18733                                         qn = qn_0;
18734                                         qn_is_zero = true;
18735                                 }
18736                                 else{
18737                                         qn = qn_180;
18738                                         qn_is_zero = false; 
18739                                 }
18740                                         
18741                                 if (qm_0 >= qm_180){
18742                                         qm = qm_0;
18743                                         qm_is_zero = true;
18744                                 }
18745                                 else{
18746                                         qm = qm_180;
18747                                         qm_is_zero = false; 
18748                                 }
18749                                         
18750                                 if(qn >= peak || qm >= peak) {
18751                                         sx = -ix;
18752                                         sy = -iy;
18753                                         nref = iref;
18754                                         if (qn >= qm) {
18755                                                 if (qn_is_zero){
18756                                                         ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18757                                                 }
18758                                                 else{
18759                                                         ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18760                                                 }
18761                                                 peak = static_cast<float>(qn);
18762                                                 mirror = 0;
18763                                         } else {
18764                                                 if (qm_is_zero){
18765                                                         ang = ang_n(retvals_0["tmt"], mode, numr[numr.size()-1]);
18766                                                 }
18767                                                 else{
18768                                                         ang = ang_n(retvals_180["tmt"], mode, numr[numr.size()-1]);
18769                                                 }
18770                                                 peak = static_cast<float>(qm);
18771                                                 mirror = 1;
18772                                         }
18773                                 }
18774                         }
18775                         delete cimage; cimage = 0;
18776                 }
18777         }
18778         float co, so, sxs, sys;
18779         co = static_cast<float>( cos(ang*pi/180.0) );
18780         so = static_cast<float>( -sin(ang*pi/180.0) );
18781         sxs = sx*co - sy*so;
18782         sys = sx*so + sy*co;
18783         vector<float> res;
18784         res.push_back(ang);
18785         res.push_back(sxs);
18786         res.push_back(sys);
18787         res.push_back(static_cast<float>(mirror));
18788         res.push_back(static_cast<float>(nref));
18789         res.push_back(peak);
18790         return res;
18791 }
18792 
18793 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18794                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18795                 vector<int>numr, float cnx, float cny, int ynumber, bool mirror_only, float yrnglocal, bool CONS) {
18796         
18797         size_t crefim_len = crefim.size();
18798         
18799         float phi_lhs=1000.0;
18800         float phi_rhs=1000.0;
18801         float y_lhs=1000.0;
18802         float y_rhs=1000.0;
18803         
18804         if (CONS){
18805                 phi_lhs = image->get_attr("phi_lhs");
18806                 phi_rhs = image->get_attr("phi_rhs");
18807                 y_lhs = image->get_attr("y_lhs");
18808                 y_rhs = image->get_attr("y_rhs");
18809         }
18810         int   iref, nref=-1, mirror=0;
18811         float iy, ix, sx=0, sy=0;
18812         float peak = -1.0E23f;
18813         float ang=0.0f;
18814         const float qv = static_cast<float>( pi/180.0 );
18815         Transform * t = image->get_attr("xform.projection");
18816         Dict d = t->get_params("spider");
18817         if(t) {delete t; t=0;}
18818         float phi = d["phi"];
18819         float phi_upper = phi+phi_rhs;
18820         float phi_lower = phi+phi_lhs;
18821         float theta = d["theta"];
18822         float psi = d["psi"];
18823         float imn1 = sin(theta*qv)*cos(phi*qv);
18824         float imn2 = sin(theta*qv)*sin(phi*qv);
18825         float imn3 = cos(theta*qv);
18826         vector<float> n1(crefim_len);
18827         vector<float> n2(crefim_len);
18828         vector<float> n3(crefim_len);
18829         vector<float> ref_phi(crefim_len);
18830         for ( iref = 0; iref < (int)crefim_len; iref++) {
18831                         n1[iref] = crefim[iref]->get_attr("n1");
18832                         n2[iref] = crefim[iref]->get_attr("n2");
18833                         n3[iref] = crefim[iref]->get_attr("n3");
18834                         
18835                         ref_phi[iref] = crefim[iref]->get_attr("phi");
18836         }
18837         float nbrinp;
18838         bool use_ref;
18839         int   kx = int(2*xrng/step+0.5)/2;
18840         int ychoice = 0;
18841         int phichoice = 0;
18842         //if ynumber==-1, use the old code which process x and y direction equally.
18843         if(ynumber==-1) {
18844                 int   ky = int(2*yrng/step+0.5)/2;
18845                 for (int i = -ky; i <= ky; i++) {
18846                         iy = i * step ;
18847                         for (int j = -kx; j <= kx; j++)  {
18848                                 ix = j*step ;
18849                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18850 
18851                                 Normalize_ring( cimage, numr );
18852 
18853                                 Frngs(cimage, numr);
18854                                 //  compare with all reference images
18855                                 // for iref in xrange(len(crefim)):
18856                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18857                                         
18858                                         use_ref = false;
18859                                         if (!mirror_only){
18860                                                 // inner product of iref's Eulerian angles with that of the data
18861                                                 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18862                                                 if (nbrinp >= ant){
18863                                                         use_ref = true;
18864                                                 }
18865                                         }
18866                                         else if (mirror_only) {
18867                                                 // inner product of the mirror of iref's Eulerian angles with that of the data
18868                                                 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
18869                                                 if (nbrinp >= ant){
18870                                                         use_ref = true;
18871                                                 }
18872                                         }
18873                                         
18874                                         
18875                                         if(use_ref) {
18876                                                 Dict retvals;
18877                                                 if (mirror_only == true){
18878                                                     if ((psi-90) < 90)  
18879                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18880                                                     else
18881                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
18882                                                 }       
18883                                                 else{ 
18884                                                     if ((psi-90) < 90)  
18885                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18886                                                     else
18887                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18888                                                 }   
18889                                                 double qn = retvals["qn"];
18890                                                 
18891                                                 if(qn >= peak) {
18892                                                         sx = -ix;
18893                                                         sy = -iy;
18894                                                         nref = iref;
18895                                                         if (!mirror_only) {
18896                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18897                                                                 peak = static_cast<float>(qn);
18898                                                                 mirror = 0;
18899                                                         } else {
18900                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18901                                                                 peak = static_cast<float>(qn);
18902                                                                 mirror = 1;
18903                                                         }
18904                                                 }
18905                                         }
18906                                 }  
18907                                 delete cimage; cimage = 0;
18908                         }
18909                    }
18910         }
18911         //if ynumber is given, it should be even. We need to check whether it is zero
18912         else if(ynumber==0) {
18913                 sy = 0.0f;
18914                 for (int j = -kx; j <= kx; j++) {
18915                         ix = j*step ;
18916                         EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18917 
18918                         Normalize_ring( cimage, numr );
18919 
18920                         Frngs(cimage, numr);
18921                         //  compare with all reference images
18922                         // for iref in xrange(len(crefim)):
18923                         for ( iref = 0; iref < (int)crefim_len; iref++)  {
18924                                 
18925                                 use_ref = false;
18926                                 if (!mirror_only){
18927                                         // inner product of iref's Eulerian angles with that of the data
18928                                         nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18929                                         if (nbrinp >= ant){
18930                                                 use_ref = true;
18931                                         }
18932                                 }
18933                                 else if (mirror_only) {
18934                                         // inner product of the mirror of iref's Eulerian angles with that of the data
18935                                         nbrinp = (-1.0f*n1[iref]*imn1) + (-1.0f*n2[iref]*imn2) + n3[iref]*imn3;
18936                                         if (nbrinp >= ant){
18937                                                 use_ref = true;
18938                                         }
18939                                 }
18940                                 
18941                                 if(use_ref) {
18942                                                 Dict retvals;
18943                                                 if (mirror_only == true){
18944                                                     if ((psi-90) < 90)  
18945                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18946                                                     else
18947                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
18948                                                 }       
18949                                                 else{ 
18950                                                     if ((psi-90) < 90)  
18951                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18952                                                     else
18953                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18954                                                 }   
18955                                                 double qn = retvals["qn"];
18956                                                 
18957                                                 if(qn >= peak) {
18958                                                         sx = -ix;
18959                                                         sy = -iy;
18960                                                         nref = iref;
18961                                                         if (!mirror_only) {
18962                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18963                                                                 peak = static_cast<float>(qn);
18964                                                                 mirror = 0;
18965                                                         } else {
18966                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18967                                                                 peak = static_cast<float>(qn);
18968                                                                 mirror = 1;
18969                                                         }
18970                                                 }
18971                                 }
18972                         } 
18973                         delete cimage; cimage = 0;
18974                 }                       
18975         } else {
18976                 int   ky = int(ynumber/2);              
18977                 float stepy=2*yrng/ynumber;
18978                 int ky_rhs = ky;
18979                 int ky_lhs = -ky + 1;
18980                 
18981                 // when yrnglocal is not equal to -1.0, the search range is limited to +/- yrnglocal
18982                 // leave step size the same
18983                 
18984                 if (CONS){
18985                         
18986                         ky_rhs = floor((abs(y_lhs))/stepy);
18987                         ky_lhs = -1.0*floor((abs(y_rhs))/stepy);
18988                         
18989                 }
18990                 else{
18991                         if (yrnglocal >= 0.0){
18992                                 ky_rhs = int(yrnglocal/stepy);
18993                                 ky_lhs = -ky_rhs + 1;
18994                         }
18995                 }
18996                 
18997                 //std::cout<<"yrnglocal="<<yrnglocal<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18998                 //cout<<"ky stepy: "<<ky<<" "<<stepy<<endl;
18999                 for (int i = ky_lhs; i <= ky_rhs; i++) {
19000                         iy = i * stepy ;
19001                         ychoice = ychoice+1;
19002                         for (int j = -kx; j <= kx; j++) {
19003                                 ix = j*step ;
19004                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19005 
19006                                 Normalize_ring( cimage, numr );
19007 
19008                                 Frngs(cimage, numr);
19009                                 //  compare with all reference images
19010                                 // for iref in xrange(len(crefim)):
19011                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
19012                                         
19013                                         use_ref = false;
19014                                         if (!mirror_only){
19015                                                 // inner product of iref's Eulerian angles with that of the data
19016                                                 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
19017                                                 if (CONS){
19018                                                         
19019                                                         if ((ref_phi[iref] <= phi_upper) && (ref_phi[iref] >= phi_lower)){
19020                                                                 use_ref = true;
19021                                                         }
19022                                                 }
19023                                                 else{
19024                                                         if (nbrinp >= ant){
19025                                                                 use_ref = true;
19026                                                         }
19027                                                 }
19028                                         }
19029                                         else if (mirror_only) {
19030                                                 // inner product of the mirror of iref's Eulerian angles with that of the data
19031                                                 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
19032                                                 if (CONS){
19033                                                         if ((ref_phi[iref] + 180. <= phi_upper) && (ref_phi[iref]+180. >= phi_lower)){
19034                                                                 use_ref = true;
19035                                                         }
19036                                                 }
19037                                                 else{
19038                                                         if (nbrinp >= ant){
19039                                                                 use_ref = true;
19040                                                         }
19041                                                 }
19042                                                 
19043                                         }
19044                                         if(use_ref) {
19045                                                 phichoice = phichoice + 1;
19046                                                 Dict retvals;
19047                                                 if (mirror_only == true){
19048                                                     if ((psi-90) < 90)  
19049                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
19050                                                     else
19051                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
19052                                                 }       
19053                                                 else{ 
19054                                                     if ((psi-90) < 90)  
19055                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
19056                                                     else
19057                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
19058                                                 }   
19059                                                 double qn = retvals["qn"];
19060                                                 
19061                                                 if(qn >= peak) {
19062                                                         sx = -ix;
19063                                                         sy = -iy;
19064                                                         nref = iref;
19065                                                         if (!mirror_only) {
19066                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19067                                                                 peak = static_cast<float>(qn);
19068                                                                 mirror = 0;
19069                                                         } else {
19070                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19071                                                                 peak = static_cast<float>(qn);
19072                                                                 mirror = 1;
19073                                                         }
19074                                                 }
19075                                         }
19076                                 }
19077                                 delete cimage; cimage = 0;
19078                         }
19079                 }
19080         }
19081         if ((phichoice < 1) || (ychoice  < 1))
19082                 cout<<ychoice<<", "<<phichoice<<", ..."<<mirror_only<<"...,"<<phi<<","<<phi_lhs<<","<<phi_rhs<<endl;
19083         
19084         float co, so, sxs, sys;
19085         co = static_cast<float>( cos(ang*pi/180.0) );
19086         so = static_cast<float>( -sin(ang*pi/180.0) );
19087         sxs = sx*co - sy*so;
19088         sys = sx*so + sy*co;
19089         vector<float> res;
19090         res.push_back(ang);
19091         res.push_back(sxs);
19092         res.push_back(sys);
19093         res.push_back(static_cast<float>(mirror));
19094         res.push_back(static_cast<float>(nref));
19095         res.push_back(peak);
19096         return res;
19097 }
19098 
19099 
19100 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
19101                 float xrng, float yrng, float step, float psi_max, string mode,
19102                 vector<int>numr, float cnx, float cny, int ynumber) {
19103 
19104         size_t crefim_len = crefim.size();
19105 
19106         int   iref, nref=0, mirror=0;
19107         float iy, ix, sx=0, sy=0;
19108         float peak = -1.0E23f;
19109         float ang=0.0f;
19110         int   kx = int(2*xrng/step+0.5)/2;
19111         //if ynumber==-1, use the old code which process x and y direction equally.
19112         
19113         int ky;
19114         float stepy;
19115         int kystart;
19116         
19117         if (ynumber == -1){
19118             ky = int(2*yrng/step+0.5)/2;
19119             stepy = step;
19120             kystart = -ky;
19121         }
19122         else if(ynumber == 0){
19123              ky = 0;
19124                  stepy = 0.0f;
19125                  kystart = ky;
19126         }
19127         else {
19128             ky = int(ynumber/2);                
19129                 stepy=2*yrng/ynumber;
19130                 kystart = -ky + 1;    
19131         }
19132         
19133                 //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
19134         for (int i = kystart; i <= ky; i++) {
19135                 iy = i * stepy ;
19136                 for (int j = -kx; j <= kx; j++) {
19137                         ix = j*step ;
19138                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19139 
19140                         Normalize_ring( cimage, numr );
19141 
19142                         Frngs(cimage, numr);
19143                         //  compare with all reference images
19144                         // for iref in xrange(len(crefim)):
19145                         for ( iref = 0; iref < (int)crefim_len; iref++) {
19146                                 Dict retvals_0 = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
19147                                 Dict retvals_180 = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
19148                                 double qn_0 = retvals_0["qn"];
19149                                 double qn_180 = retvals_180["qn"];
19150                                 double qn;
19151                                 bool qn_is_zero = false;
19152                                 
19153                                 if (qn_0 >= qn_180){
19154                                         qn = qn_0;
19155                                         qn_is_zero = true;
19156                                 }
19157                                 else{
19158                                         qn = qn_180;
19159                                         qn_is_zero = false; 
19160                                 }
19161                                         
19162                                 if(qn >= peak) {
19163                                         sx = -ix;
19164                                         sy = -iy;
19165                                         nref = iref;
19166                                         
19167                                         if (qn_is_zero){
19168                                                 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
19169                                         }
19170                                         else{
19171                                                 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
19172                                         }
19173                                         peak = static_cast<float>(qn);
19174                                         mirror = 0;
19175                                          
19176                                 }
19177                         }
19178                         delete cimage; cimage = 0;
19179                 }
19180         }       
19181         float co, so, sxs, sys;
19182         co = static_cast<float>( cos(ang*pi/180.0) );
19183         so = static_cast<float>( -sin(ang*pi/180.0) );
19184         sxs = sx*co - sy*so;
19185         sys = sx*so + sy*co;
19186         vector<float> res;
19187         res.push_back(ang);
19188         res.push_back(sxs);
19189         res.push_back(sys);
19190         res.push_back(static_cast<float>(mirror));
19191         res.push_back(static_cast<float>(nref));
19192         res.push_back(peak);
19193         return res;
19194 }
19195 
19196 
19197 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
19198                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
19199                 vector<int>numr, float cnx, float cny, int ynumber) {
19200 
19201         size_t crefim_len = crefim.size();
19202         const float qv = static_cast<float>( pi/180.0 );
19203         Transform * t = image->get_attr("xform.projection");
19204         Dict d = t->get_params("spider");
19205         if(t) {delete t; t=0;}
19206         float phi = d["phi"];
19207         float theta = d["theta"];
19208         float imn1 = sin(theta*qv)*cos(phi*qv);
19209         float imn2 = sin(theta*qv)*sin(phi*qv);
19210         float imn3 = cos(theta*qv);
19211         vector<float> n1(crefim_len);
19212         vector<float> n2(crefim_len);
19213         vector<float> n3(crefim_len);
19214         int   iref, nref=-1, mirror=0;
19215         float iy, ix, sx=0, sy=0;
19216         float peak = -1.0E23f;
19217         float ang=0.0f;
19218         int   kx = int(2*xrng/step+0.5)/2;
19219         
19220         for ( iref = 0; iref < (int)crefim_len; iref++) {
19221                 n1[iref] = crefim[iref]->get_attr("n1");
19222                 n2[iref] = crefim[iref]->get_attr("n2");
19223                 n3[iref] = crefim[iref]->get_attr("n3");
19224         }
19225         
19226         //if ynumber==-1, use the old code which process x and y direction equally.
19227         if(ynumber==-1) {
19228                 int   ky = int(2*yrng/step+0.5)/2;
19229                 for (int i = -ky; i <= ky; i++) {
19230                         iy = i * step ;
19231                         for (int j = -kx; j <= kx; j++)  {
19232                                 ix = j*step ;
19233                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19234 
19235                                 Normalize_ring( cimage, numr );
19236 
19237                                 Frngs(cimage, numr);
19238                                 //  compare with all reference images
19239                                 // for iref in xrange(len(crefim)):
19240                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
19241                                         if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19242                                                 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
19243                                                 double qn = retvals["qn"];
19244                                                 if( qn >= peak) {
19245                                                         sx = -ix;
19246                                                         sy = -iy;
19247                                                         nref = iref;
19248                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19249                                                         peak = static_cast<float>(qn);
19250                                                         mirror = 0;
19251                                                 }
19252                                         }
19253                                 }  
19254                                 delete cimage; cimage = 0;
19255                         }
19256                    }
19257         }
19258         //if ynumber is given, it should be even. We need to check whether it is zero
19259         else if(ynumber==0) {
19260                 sy = 0.0f;
19261                 for (int j = -kx; j <= kx; j++) {
19262                         ix = j*step ;
19263                         iy = 0.0f ;
19264                         EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
19265 
19266                         Normalize_ring( cimage, numr );
19267 
19268                         Frngs(cimage, numr);
19269                         //  compare with all reference images
19270                         // for iref in xrange(len(crefim)):
19271                         for ( iref = 0; iref < (int)crefim_len; iref++)  {
19272                                 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19273                                         Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
19274                                         double qn = retvals["qn"];
19275                                         if( qn >= peak ) {
19276                                                 sx = -ix;
19277                                                 nref = iref;
19278                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19279                                                 peak = static_cast<float>(qn);
19280                                                 mirror = 0;
19281                                         }
19282                                 }
19283                         } 
19284                         delete cimage; cimage = 0;
19285                 }                       
19286         } else {
19287                 int   ky = int(ynumber/2);              
19288                 float stepy=2*yrng/ynumber;
19289                 //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
19290                 for (int i = -ky+1; i <= ky; i++) {
19291                         iy = i * stepy ;
19292                         for (int j = -kx; j <= kx; j++) {
19293                                 ix = j*step ;
19294                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19295 
19296                                 Normalize_ring( cimage, numr );
19297 
19298                                 Frngs(cimage, numr);
19299                                 //  compare with all reference images
19300                                 // for iref in xrange(len(crefim)):
19301                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
19302                                         if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19303                                                 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
19304                                                 double qn = retvals["qn"];
19305                                                 if( qn >= peak) {
19306                                                         sx = -ix;
19307                                                         sy = -iy;
19308                                                         nref = iref;
19309                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19310                                                         peak = static_cast<float>(qn);
19311                                                         mirror = 0;
19312                                                 }
19313                                         }
19314                                 }
19315                                 delete cimage; cimage = 0;
19316                         }
19317                 }
19318         }
19319         float co, so, sxs, sys;
19320         co = static_cast<float>( cos(ang*pi/180.0) );
19321         so = static_cast<float>( -sin(ang*pi/180.0) );
19322         sxs = sx*co - sy*so;
19323         sys = sx*so + sy*co;
19324         vector<float> res;
19325         res.push_back(ang);
19326         res.push_back(sxs);
19327         res.push_back(sys);
19328         res.push_back(static_cast<float>(mirror));
19329         res.push_back(static_cast<float>(nref));
19330         res.push_back(peak);
19331         return res;
19332 }
19333 
19334 
19335 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
19336                         float xrng, float yrng, float step, string mode,
19337                         vector< int >numr, float cnx, float cny,
19338                         EMData *peaks, EMData *peakm) {
19339 
19340         int   maxrin = numr[numr.size()-1];
19341 
19342         int   ky = int(2*yrng/step+0.5)/2;
19343         int   kx = int(2*xrng/step+0.5)/2;
19344 
19345         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19346         float *p_ccf1ds = peaks->get_data();
19347 
19348         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19349         float *p_ccf1dm = peakm->get_data();
19350 
19351         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19352                 p_ccf1ds[i] = -1.e20f;
19353                 p_ccf1dm[i] = -1.e20f;
19354         }
19355 
19356         for (int i = -ky; i <= ky; i++) {
19357                 float iy = i * step;
19358                 for (int j = -kx; j <= kx; j++) {
19359                         float ix = j*step;
19360                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19361                         Frngs(cimage, numr);
19362                         Crosrng_msg_vec(crefim, cimage, numr,
19363                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19364                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19365                         delete cimage; cimage = 0;
19366                 }
19367         }
19368         return;
19369 }
19370 
19371 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19372      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19373      EMData *peaks_compress, EMData *peakm_compress) {
19374 
19375         int   maxrin = numr[numr.size()-1];
19376 
19377         int   ky = int(2*yrng/step+0.5)/2;
19378         int   kx = int(2*xrng/step+0.5)/2;
19379 
19380         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19381         float *p_ccf1ds = peaks->get_data();
19382 
19383         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19384         float *p_ccf1dm = peakm->get_data();
19385 
19386         peaks_compress->set_size(maxrin, 1, 1);
19387         float *p_ccf1ds_compress = peaks_compress->get_data();
19388 
19389         peakm_compress->set_size(maxrin, 1, 1);
19390         float *p_ccf1dm_compress = peakm_compress->get_data();
19391 
19392         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19393                 p_ccf1ds[i] = -1.e20f;
19394                 p_ccf1dm[i] = -1.e20f;
19395         }
19396 
19397         for (int i = -ky; i <= ky; i++) {
19398                 float iy = i * step;
19399                 for (int j = -kx; j <= kx; j++) {
19400                         float ix = j*step;
19401                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19402                         Frngs(cimage, numr);
19403                         Crosrng_msg_vec(crefim, cimage, numr,
19404                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19405                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19406                         delete cimage; cimage = 0;
19407                 }
19408         }
19409         for (int x=0; x<maxrin; x++) {
19410                 float maxs = -1.0e22f;
19411                 float maxm = -1.0e22f;
19412                 for (int i=1; i<=2*ky+1; i++) {
19413                         for (int j=1; j<=2*kx+1; j++) {
19414                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19415                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19416                         }
19417                 }
19418                 p_ccf1ds_compress[x] = maxs;
19419                 p_ccf1dm_compress[x] = maxm;
19420         }
19421         return;
19422 }
19423 
19424 struct ccf_point
19425 {
19426     float value;
19427     int i;
19428     int j;
19429     int k;
19430     int mirror;
19431 };
19432 
19433 
19434 struct ccf_value
19435 {
19436     bool operator()( const ccf_point& a, const ccf_point& b )
19437     {
19438         return a.value > b.value;
19439     }
19440 };
19441 
19442 
19443 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19444                         float xrng, float yrng, float step, string mode,
19445                         vector< int >numr, float cnx, float cny, double T) {
19446 
19447         int   maxrin = numr[numr.size()-1];
19448 
19449         int   ky = int(2*yrng/step+0.5)/2;
19450         int   kx = int(2*xrng/step+0.5)/2;
19451 
19452         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19453         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19454         int vol = maxrin*(2*kx+1)*(2*ky+1);
19455         vector<ccf_point> ccf(2*vol);
19456         ccf_point temp;
19457 
19458         int index = 0;
19459         for (int i = -ky; i <= ky; i++) {
19460                 float iy = i * step;
19461                 for (int j = -kx; j <= kx; j++) {
19462                         float ix = j*step;
19463                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19464                         Frngs(cimage, numr);
19465                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19466                         for (int k=0; k<maxrin; k++) {
19467                                 temp.value = p_ccf1ds[k];
19468                                 temp.i = k;
19469                                 temp.j = j;
19470                                 temp.k = i;
19471                                 temp.mirror = 0;
19472                                 ccf[index] = temp;
19473                                 index++;
19474                                 temp.value = p_ccf1dm[k];
19475                                 temp.mirror = 1;
19476                                 ccf[index] = temp;
19477                                 index++;
19478                         }
19479                         delete cimage; cimage = 0;
19480                 }
19481         }
19482 
19483         delete p_ccf1ds;
19484         delete p_ccf1dm;
19485         std::sort(ccf.begin(), ccf.end(), ccf_value());
19486 
19487         double qt = (double)ccf[0].value;
19488         vector <double> p(2*vol), cp(2*vol);
19489 
19490         double sump = 0.0;
19491         for (int i=0; i<2*vol; i++) {
19492                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19493                 sump += p[i];
19494         }
19495         for (int i=0; i<2*vol; i++) {
19496                 p[i] /= sump;
19497         }
19498         for (int i=1; i<2*vol; i++) {
19499                 p[i] += p[i-1];
19500         }
19501         p[2*vol-1] = 2.0;
19502 
19503         float t = get_frand(0.0f, 1.0f);
19504         int select = 0;
19505         while (p[select] < t)   select += 1;
19506 
19507         vector<float> a(6);
19508         a[0] = ccf[select].value;
19509         a[1] = (float)ccf[select].i;
19510         a[2] = (float)ccf[select].j;
19511         a[3] = (float)ccf[select].k;
19512         a[4] = (float)ccf[select].mirror;
19513         a[5] = (float)select;
19514         return a;
19515 }
19516 
19517 
19518 /*
19519 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
19520                         float xrng, float yrng, float step, string mode,
19521                         vector< int >numr, float cnx, float cny,
19522                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
19523 
19524 // formerly known as apmq
19525     // Determine shift and rotation between image and many reference
19526     // images (crefim, weights have to be applied) quadratic
19527     // interpolation
19528 
19529 
19530     // Manually extract.
19531 *//*    vector< EMAN::EMData* > crefim;
19532     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
19533     crefim.reserve(crefim_len);
19534 
19535     for(std::size_t i=0;i<crefim_len;i++) {
19536         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
19537         crefim.push_back(proxy());
19538     }
19539 */
19540 /*
19541         int   maxrin = numr[numr.size()-1];
19542 
19543         size_t crefim_len = crefim.size();
19544 
19545         int   iref;
19546         int   ky = int(2*yrng/step+0.5)/2;
19547         int   kx = int(2*xrng/step+0.5)/2;
19548         int   tkx = 2*kx+3;
19549         int   tky = 2*ky+3;
19550 
19551         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
19552         float *p_ccf1ds = peaks->get_data();
19553 
19554 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
19555 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
19556         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
19557         float *p_ccf1dm = peakm->get_data();
19558 
19559         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
19560                 p_ccf1ds[i] = -1.e20f;
19561                 p_ccf1dm[i] = -1.e20f;
19562         }
19563 
19564         float  iy, ix;
19565         for (int i = -ky; i <= ky; i++) {
19566                 iy = i * step ;
19567                 for (int j = -kx; j <= kx; j++) {
19568                         ix = j*step ;
19569                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19570                         Frngs(cimage, numr);
19571                         //  compare with all reference images
19572                         // for iref in xrange(len(crefim)):
19573                         for ( iref = 0; iref < (int)crefim_len; iref++) {
19574                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
19575                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
19576                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
19577                         }
19578                         delete cimage; cimage = 0;
19579                 }
19580         }
19581         return;
19582 }
19583 */
19584 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19585 
19586         EMData *rot;
19587 
19588         const int nmax=3, mmax=3;
19589         char task[60], csave[60];
19590         long int lsave[4];
19591         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19592         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];
19593         long int SIXTY=60;
19594 
19595         //     We wish to have no output.
19596         iprint = -1;
19597 
19598         //c     We specify the tolerances in the stopping criteria.
19599         factr=1.0e1;
19600         pgtol=1.0e-5;
19601 
19602         //     We specify the dimension n of the sample problem and the number
19603         //        m of limited memory corrections stored.  (n and m should not
19604         //        exceed the limits nmax and mmax respectively.)
19605         n=3;
19606         m=3;
19607 
19608         //     We now provide nbd which defines the bounds on the variables:
19609         //                    l   specifies the lower bounds,
19610         //                    u   specifies the upper bounds.
19611         //                    x   specifies the initial guess
19612         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19613         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19614         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19615 
19616 
19617         //     We start the iteration by initializing task.
19618         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19619         strcpy(task,"START");
19620         for (int i=5;i<60;i++)  task[i]=' ';
19621 
19622         //     This is the call to the L-BFGS-B code.
19623         // (* call the L-BFGS-B routine with task='START' once before loop *)
19624         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19625         //int step = 1;
19626 
19627         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19628         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19629 
19630                 if (strncmp(task,"FG",2)==0) {
19631                 //   the minimization routine has returned to request the
19632                 //   function f and gradient g values at the current x
19633 
19634                 //        Compute function value f for the sample problem.
19635                 rot = new EMData();
19636                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19637                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19638                 //f = -f;
19639                 delete rot;
19640 
19641                 //        Compute gradient g for the sample problem.
19642                 float dt = 1.0e-3f;
19643                 rot = new EMData();
19644                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19645                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19646                 //f1 = -f1;
19647                 g[0] = (f1-f)/dt;
19648                 delete rot;
19649 
19650                 dt = 1.0e-2f;
19651                 rot = new EMData();
19652                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19653                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19654                 //f2 = -f2;
19655                 g[1] = (f2-f)/dt;
19656                 delete rot;
19657 
19658                 rot = new EMData();
19659                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19660                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19661                 //f3 = -f3;
19662                 g[2] = (f3-f)/dt;
19663                 delete rot;
19664                 }
19665 
19666                 //c          go back to the minimization routine.
19667                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19668                 //step++;
19669         }
19670 
19671         //printf("Total step is %d\n", step);
19672         vector<float> res;
19673         res.push_back(static_cast<float>(x[0]));
19674         res.push_back(static_cast<float>(x[1]));
19675         res.push_back(static_cast<float>(x[2]));
19676         //res.push_back(step);
19677         return res;
19678 }
19679 
19680 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19681 
19682         EMData *rot;
19683 
19684         const int nmax=3, mmax=3;
19685         char task[60], csave[60];
19686         long int lsave[4];
19687         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19688         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];
19689         long int SIXTY=60;
19690 
19691         //     We wish to have no output.
19692         iprint = -1;
19693 
19694         //c     We specify the tolerances in the stopping criteria.
19695         factr=1.0e1;
19696         pgtol=1.0e-5;
19697 
19698         //     We specify the dimension n of the sample problem and the number
19699         //        m of limited memory corrections stored.  (n and m should not
19700         //        exceed the limits nmax and mmax respectively.)
19701         n=3;
19702         m=3;
19703 
19704         //     We now provide nbd which defines the bounds on the variables:
19705         //                    l   specifies the lower bounds,
19706         //                    u   specifies the upper bounds.
19707         //                    x   specifies the initial guess
19708         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19709         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19710         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19711 
19712 
19713         //     We start the iteration by initializing task.
19714         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19715         strcpy(task,"START");
19716         for (int i=5;i<60;i++)  task[i]=' ';
19717 
19718         //     This is the call to the L-BFGS-B code.
19719         // (* call the L-BFGS-B routine with task='START' once before loop *)
19720         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19721         //int step = 1;
19722 
19723         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19724         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19725 
19726                 if (strncmp(task,"FG",2)==0) {
19727                 //   the minimization routine has returned to request the
19728                 //   function f and gradient g values at the current x
19729 
19730                 //        Compute function value f for the sample problem.
19731                 rot = new EMData();
19732                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19733                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19734                 //f = -f;
19735                 delete rot;
19736 
19737                 //        Compute gradient g for the sample problem.
19738                 float dt = 1.0e-3f;
19739                 rot = new EMData();
19740                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19741                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19742                 //f1 = -f1;
19743                 g[0] = (f1-f)/dt;
19744                 delete rot;
19745 
19746                 rot = new EMData();
19747                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19748                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19749                 //f2 = -f2;
19750                 g[1] = (f2-f)/dt;
19751                 delete rot;
19752 
19753                 rot = new EMData();
19754                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19755                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19756                 //f3 = -f3;
19757                 g[2] = (f3-f)/dt;
19758                 delete rot;
19759                 }
19760 
19761                 //c          go back to the minimization routine.
19762                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19763                 //step++;
19764         }
19765 
19766         //printf("Total step is %d\n", step);
19767         vector<float> res;
19768         res.push_back(static_cast<float>(x[0]));
19769         res.push_back(static_cast<float>(x[1]));
19770         res.push_back(static_cast<float>(x[2]));
19771         //res.push_back(step);
19772         return res;
19773 }
19774 
19775 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) {
19776 
19777         EMData *proj, *proj2;
19778 
19779         const int nmax=5, mmax=5;
19780         char task[60], csave[60];
19781         long int lsave[4];
19782         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19783         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];
19784         long int SIXTY=60;
19785 
19786         //     We wish to have no output.
19787         iprint = -1;
19788 
19789         //c     We specify the tolerances in the stopping criteria.
19790         factr=1.0e1;
19791         pgtol=1.0e-5;
19792 
19793         //     We specify the dimension n of the sample problem and the number
19794         //        m of limited memory corrections stored.  (n and m should not
19795         //        exceed the limits nmax and mmax respectively.)
19796         n=5;
19797         m=5;
19798 
19799         //     We now provide nbd which defines the bounds on the variables:
19800         //                    l   specifies the lower bounds,
19801         //                    u   specifies the upper bounds.
19802         //                    x   specifies the initial guess
19803         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
19804         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
19805         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
19806         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
19807         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
19808 
19809 
19810         //     We start the iteration by initializing task.
19811         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19812         strcpy(task,"START");
19813         for (int i=5;i<60;i++)  task[i]=' ';
19814 
19815         //     This is the call to the L-BFGS-B code.
19816         // (* call the L-BFGS-B routine with task='START' once before loop *)
19817         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19818         int step = 1;
19819 
19820         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19821         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19822 
19823                 if (strncmp(task,"FG",2)==0) {
19824                 //   the minimization routine has returned to request the
19825                 //   function f and gradient g values at the current x
19826 
19827                 //        Compute function value f for the sample problem.
19828                 proj = new EMData();
19829                 proj2 = new EMData();
19830                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19831                 proj->fft_shuffle();
19832                 proj->center_origin_fft();
19833                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19834                 proj->do_ift_inplace();
19835                 int M = proj->get_ysize()/2;
19836                 proj2 = proj->window_center(M);
19837                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19838                 //f = -f;
19839                 delete proj;
19840                 delete proj2;
19841 
19842                 //        Compute gradient g for the sample problem.
19843                 float dt = 1.0e-3f;
19844                 proj = new EMData();
19845                 proj2 = new EMData();
19846                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19847                 proj->fft_shuffle();
19848                 proj->center_origin_fft();
19849                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19850                 proj->do_ift_inplace();
19851                 proj2 = proj->window_center(M);
19852                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19853                 //ft = -ft;
19854                 delete proj;
19855                 delete proj2;
19856                 g[0] = (ft-f)/dt;
19857 
19858                 proj = new EMData();
19859                 proj2 = new EMData();
19860                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19861                 proj->fft_shuffle();
19862                 proj->center_origin_fft();
19863                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19864                 proj->do_ift_inplace();
19865                 proj2 = proj->window_center(M);
19866                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19867                 //ft = -ft;
19868                 delete proj;
19869                 delete proj2;
19870                 g[1] = (ft-f)/dt;
19871 
19872                 proj = new EMData();
19873                 proj2 = new EMData();
19874                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19875                 proj->fft_shuffle();
19876                 proj->center_origin_fft();
19877                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19878                 proj->do_ift_inplace();
19879                 proj2 = proj->window_center(M);
19880                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19881                 //ft = -ft;
19882                 delete proj;
19883                 delete proj2;
19884                 g[2] = (ft-f)/dt;
19885 
19886                 proj = new EMData();
19887                 proj2 = new EMData();
19888                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19889                 proj->fft_shuffle();
19890                 proj->center_origin_fft();
19891                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19892                 proj->do_ift_inplace();
19893                 proj2 = proj->window_center(M);
19894                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19895                 //ft = -ft;
19896                 delete proj;
19897                 delete proj2;
19898                 g[3] = (ft-f)/dt;
19899 
19900                 proj = new EMData();
19901                 proj2 = new EMData();
19902                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19903                 proj->fft_shuffle();
19904                 proj->center_origin_fft();
19905                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19906                 proj->do_ift_inplace();
19907                 proj2 = proj->window_center(M);
19908                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19909                 //ft = -ft;
19910                 delete proj;
19911                 delete proj2;
19912                 g[4] = (ft-f)/dt;
19913                 }
19914 
19915                 //c          go back to the minimization routine.
19916                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19917                 step++;
19918         }
19919 
19920         //printf("Total step is %d\n", step);
19921         vector<float> res;
19922         res.push_back(static_cast<float>(x[0]));
19923         res.push_back(static_cast<float>(x[1]));
19924         res.push_back(static_cast<float>(x[2]));
19925         res.push_back(static_cast<float>(x[3]));
19926         res.push_back(static_cast<float>(x[4]));
19927         //res.push_back(step);
19928         return res;
19929 }
19930 
19931 
19932 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19933 
19934         double  x[4];
19935         int n;
19936         int l = 3;
19937         int m = 200;
19938         double e = 1e-9;
19939         double step = 0.01;
19940         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19941 
19942         x[1] = ang;
19943         x[2] = sxs;
19944         x[3] = sys;
19945 
19946         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
19947         //printf("Took %d steps\n", n);
19948 
19949         vector<float> res;
19950         res.push_back(static_cast<float>(x[1]));
19951         res.push_back(static_cast<float>(x[2]));
19952         res.push_back(static_cast<float>(x[3]));
19953         res.push_back(static_cast<float>(n));
19954         return res;
19955 }
19956 
19957 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19958         
19959         const int nmax=args.size(), mmax=nmax;
19960         char task[60], csave[60];
19961         long int lsave[4];
19962         long int n, m, iprint, isave[44];
19963         long int* nbd = new long int[nmax];
19964         long int* iwa = new long int[3*nmax];
19965         double f, factr, pgtol;
19966         double* x = new double[nmax];
19967         double* l = new double[nmax];
19968         double* u = new double[nmax];
19969         double* g = new double[nmax];
19970         double dsave[29];
19971         double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19972         long int SIXTY=60;
19973 
19974         int num_ali = nmax/3+1;
19975         int nima = all_ali_params.size()/(num_ali*4);
19976         
19977         //     We wish to have no output.
19978         iprint = -1;
19979 
19980         //c     We specify the tolerances in the stopping criteria.
19981         factr=1.0e1;
19982         pgtol=1.0e-9;
19983 
19984         //     We specify the dimension n of the sample problem and the number
19985         //        m of limited memory corrections stored.  (n and m should not
19986         //        exceed the limits nmax and mmax respectively.)
19987         n=nmax;
19988         m=mmax;
19989 
19990         //     We now provide nbd which defines the bounds on the variables:
19991         //                    l   specifies the lower bounds,
19992         //                    u   specifies the upper bounds.
19993         //                    x   specifies the initial guess
19994         for (int i=0; i<nmax; i++) {
19995                 x[i] = args[i]; 
19996                 nbd[i] = 0;
19997         }
19998 
19999         //     We start the iteration by initializing task.
20000         // (**MUST clear remaining chars in task with spaces (else crash)!**)
20001         strcpy(task,"START");
20002         for (int i=5;i<60;i++)  task[i]=' ';
20003 
20004         //     This is the call to the L-BFGS-B code.
20005         // (* call the L-BFGS-B routine with task='START' once before loop *)
20006         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
20007         int step = 1;
20008 
20009         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
20010         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
20011 
20012                 if (strncmp(task,"FG",2)==0) {
20013                 //   the minimization routine has returned to request the
20014                 //   function f and gradient g values at the current x
20015 
20016                 //        Compute function value f for the sample problem.
20017                 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
20018 
20019                 //        Compute gradient g for the sample problem.
20020                 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
20021 
20022                 }
20023                 //c          go back to the minimization routine.
20024                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
20025                 step++;
20026         }
20027 
20028         //printf("Total step is %d\n", step);
20029         vector<float> res;
20030         for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
20031         res.push_back(static_cast<float>(f));
20032 
20033         delete[] nbd;
20034         delete[] iwa;
20035         delete[] x;
20036         delete[] l;
20037         delete[] u;
20038         delete[] g;
20039         delete[] wa;
20040 
20041         return res;
20042 
20043 }
20044 
20045 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d) {
20046 
20047         vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
20048         double sum_sqr_pixel_error = 0.0;
20049         for (int i=0; i<nima; i++)  sum_sqr_pixel_error += sqr_pixel_error[i];
20050         return sum_sqr_pixel_error/static_cast<float>(nima);
20051 }
20052 
20053 
20054 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d) {
20055 
20056         double* args = new double[num_ali*3];
20057         for (int i=0; i<3*num_ali-3; i++)   args[i] = x[i];
20058         args[3*num_ali-3] = 0.0;
20059         args[3*num_ali-2] = 0.0;
20060         args[3*num_ali-1] = 0.0;
20061         double* cosa = new double[num_ali];
20062         double* sina = new double[num_ali];
20063         for (int i=0; i<num_ali; i++) {
20064                 cosa[i] = cos(args[i*3]*M_PI/180.0);
20065                 sina[i] = sin(args[i*3]*M_PI/180.0);
20066         }
20067         double* sx = new double[num_ali];
20068         double* sy = new double[num_ali];
20069         
20070         vector<double> sqr_pixel_error(nima);
20071 
20072         for (int i=0; i<nima; i++) {
20073                 double sum_cosa = 0.0;
20074                 double sum_sina = 0.0;
20075                 for (int j=0; j<num_ali; j++) {
20076                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
20077                                 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20078                                 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20079                                 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];
20080                                 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];
20081                         } else {
20082                                 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20083                                 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20084                                 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];
20085                                 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];
20086                         }
20087                 }
20088                 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
20089                 sum_cosa /= P;
20090                 sum_sina /= P;
20091                 sqr_pixel_error[i] = d*d/4.0*(1.0-P/num_ali)+var(sx, num_ali)+var(sy, num_ali);
20092         }
20093         
20094         delete[] args;
20095         delete[] cosa;
20096         delete[] sina;
20097         delete[] sx;
20098         delete[] sy;
20099         
20100         return sqr_pixel_error;
20101 }
20102 
20103 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
20104 
20105         for (int i=0; i<num_ali*3-3; i++)    g[i] = 0.0;
20106 
20107         double* args = new double[num_ali*3];
20108         for (int i=0; i<3*num_ali-3; i++)   args[i] = x[i];
20109         args[3*num_ali-3] = 0.0;
20110         args[3*num_ali-2] = 0.0;
20111         args[3*num_ali-1] = 0.0;
20112         double* cosa = new double[num_ali];
20113         double* sina = new double[num_ali];
20114         for (int i=0; i<num_ali; i++) {
20115                 cosa[i] = cos(args[i*3]*M_PI/180.0);
20116                 sina[i] = sin(args[i*3]*M_PI/180.0);
20117         }
20118         double* sx = new double[num_ali];
20119         double* sy = new double[num_ali];
20120         
20121         vector<float> sqr_pixel_error(nima);
20122 
20123         for (int i=0; i<nima; i++) {
20124                 double sum_cosa = 0.0;
20125                 double sum_sina = 0.0;
20126                 for (int j=0; j<num_ali; j++) {
20127                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
20128                                 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20129                                 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20130                                 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];
20131                                 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];
20132                         } else {
20133                                 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20134                                 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20135                                 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];
20136                                 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];
20137                         }
20138                 }
20139                 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
20140                 sum_cosa /= P;
20141                 sum_sina /= P;
20142                 for (int j=0; j<num_ali-1; j++) {
20143                         double dx = 2.0*(sx[j]-mean(sx, num_ali));
20144                         double dy = 2.0*(sy[j]-mean(sy, num_ali));
20145                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
20146                                 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
20147                                                     sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
20148                                                     dx*(-ali_params[j*nima*4+i*4+1]*sina[j]-ali_params[j*nima*4+i*4+2]*cosa[j])+
20149                                                     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;
20150                                 g[j*3+1] += dx;
20151                                 g[j*3+2] += dy;
20152                         } else {
20153                                 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
20154                                                      sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
20155                                                     dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
20156                                                     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;
20157                                 g[j*3+1] += -dx;
20158                                 g[j*3+2] += dy;
20159                         }
20160                 }
20161         }
20162         
20163         for (int i=0; i<3*num_ali-3; i++)  g[i] /= (num_ali*nima);
20164         
20165         delete[] args;
20166         delete[] cosa;
20167         delete[] sina;
20168         delete[] sx;
20169         delete[] sy;
20170 }
20171 
20172 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
20173 
20174         EMData *rot= new EMData();
20175         float ccc;
20176 
20177         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
20178         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
20179         delete rot;
20180         return ccc;
20181 }
20182 
20183 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
20184 
20185         double  x[4];
20186         int n;
20187         int l = 3;
20188         int m = 200;
20189         double e = 1e-9;
20190         double step = 0.001;
20191         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
20192 
20193         x[1] = ang;
20194         x[2] = sxs;
20195         x[3] = sys;
20196 
20197         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
20198         //printf("Took %d steps\n", n);
20199 
20200         vector<float> res;
20201         res.push_back(static_cast<float>(x[1]));
20202         res.push_back(static_cast<float>(x[2]));
20203         res.push_back(static_cast<float>(x[3]));
20204         res.push_back(static_cast<float>(n));
20205         return res;
20206 }
20207 
20208 
20209 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
20210 
20211         EMData *rot= new EMData();
20212         float ccc;
20213 
20214         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
20215         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
20216         delete rot;
20217         return ccc;
20218 }
20219 
20220 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*(size_t)nx]
20221 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
20222 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
20223 {
20224         ENTERFUNC;
20225         /* Exception Handle */
20226         if (!img) {
20227                 throw NullPointerException("NULL input image");
20228         }
20229 
20230         int newx, newy, newz;
20231         bool  keep_going;
20232         cout << " entered   " <<endl;
20233         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
20234         //int size = nx*ny*nz;
20235         EMData * img2 = new EMData();
20236         img2->set_size(nx,ny,nz);
20237         img2->to_zero();
20238         float *img_ptr  =img->get_data();
20239         float *img2_ptr = img2->get_data();
20240         int r2 = ro*ro;
20241         int r3 = r2*ro;
20242         int ri2 = ri*ri;
20243         int ri3 = ri2*ri;
20244 
20245         int n2 = nx/2;
20246 
20247         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
20248                 float z2 = static_cast<float>(k*k);
20249                 for (int j=-n2; j<=n2; j++) {
20250                         float y2 = z2 + j*j;
20251                         if(y2 <= r2) {
20252                                                                                         //cout << "  j  "<<j <<endl;
20253 
20254                                 for (int i=-n2; i<=n2; i++) {
20255                                         float x2 = y2 + i*i;
20256                                         if(x2 <= r3) {
20257                                                                                         //cout << "  i  "<<i <<endl;
20258                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
20259                                                 if(x2 >= ri3) {
20260                                                         //  this is the outer shell, here points can only vanish
20261                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
20262                                                                 //cout << "  1  "<<ib <<endl;
20263                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
20264                                                                         img2_ptr(ib,jb,kb) = 0.0f;
20265                                                                         keep_going = true;
20266                                                                 //cout << "  try  "<<ib <<endl;
20267                                                                         while(keep_going) {
20268                                                                                 newx = Util::get_irand(-ro,ro);
20269                                                                                 newy = Util::get_irand(-ro,ro);
20270                                                                                 newz = Util::get_irand(-ro,ro);
20271                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
20272                                                                                         newx += n2; newy += n2; newz += n2;
20273                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
20274                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
20275                                                                                                 keep_going = false; }
20276                                                                                 }
20277                                                                         }
20278                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
20279                                                         }
20280                                                 }  else  {
20281                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
20282                                                         if(img_ptr(ib,jb,kb) == 1.0) {
20283                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
20284                                                                         //  find out the number of neighbors
20285                                                                         float  numn = -1.0f;  // we already know the central one is 1
20286                                                                         for (newz = -1; newz <= 1; newz++)
20287                                                                                 for (newy = -1; newy <= 1; newy++)
20288                                                                                         for (newx = -1; newx <= 1; newx++)
20289                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
20290                                                                         img2_ptr(ib,jb,kb) = 0.0;
20291                                                                         if(numn == 26.0f) {
20292                                                                                 //  all neighbors exist, it has to vanish
20293                                                                                 keep_going = true;
20294                                                                                 while(keep_going) {
20295                                                                                         newx = Util::get_irand(-ro,ro);
20296                                                                                         newy = Util::get_irand(-ro,ro);
20297                                                                                         newz = Util::get_irand(-ro,ro);
20298                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
20299                                                                                                 newx += n2; newy += n2; newz += n2;
20300                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
20301                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
20302                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
20303                                                                                                                         newx += n2; newy += n2; newz += n2;
20304                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
20305                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
20306                                                                                                                                 keep_going = false; }
20307                                                                                                                 }
20308                                                                                                         }
20309                                                                                                 }
20310                                                                                         }
20311                                                                                 }
20312                                                                         }  else if(numn == 25.0f) {
20313                                                                                 // there is only one empty neighbor, move there
20314                                                                                 for (newz = -1; newz <= 1; newz++) {
20315                                                                                         for (newy = -1; newy <= 1; newy++) {
20316                                                                                                 for (newx = -1; newx <= 1; newx++) {
20317                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
20318                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
20319                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
20320                                                                                                                         }
20321                                                                                                         }
20322                                                                                                 }
20323                                                                                         }
20324                                                                                 }
20325                                                                         }  else {
20326                                                                                 //  more than one neighbor is zero, select randomly one and move there
20327                                                                                 keep_going = true;
20328                                                                                 while(keep_going) {
20329                                                                                         newx = Util::get_irand(-1,1);
20330                                                                                         newy = Util::get_irand(-1,1);
20331                                                                                         newz = Util::get_irand(-1,1);
20332                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
20333                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
20334                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
20335                                                                                                         keep_going = false;
20336                                                                                                 }
20337                                                                                         }
20338                                                                                 }
20339                                                                         }
20340                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
20341                                                         }
20342                                                 }
20343                                         }
20344                                 }
20345                         }
20346                 }
20347         }
20348         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
20349         img2->update();
20350 
20351         EXITFUNC;
20352         return img2;
20353 }
20354 #undef img_ptr
20355 #undef img2_ptr
20356 
20357 struct point3d_t
20358 {
20359         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20360 
20361         int x;
20362         int y;
20363         int z;
20364 };
20365 
20366 
20367 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20368 {
20369         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20370         int noff = 6;
20371 
20372         int nx = visited->get_xsize();
20373         int ny = visited->get_ysize();
20374         int nz = visited->get_zsize();
20375 
20376         vector< point3d_t > pts;
20377         pts.push_back( point3d_t(ix, iy, iz) );
20378         visited->set_value_at( ix, iy, iz, (float)grpid );
20379 
20380         int start = 0;
20381         int end = pts.size();
20382 
20383         while( end > start ) {
20384                 for(int i=start; i < end; ++i ) {
20385                         int ix = pts[i].x;
20386                         int iy = pts[i].y;
20387                         int iz = pts[i].z;
20388 
20389                         for( int j=0; j < noff; ++j ) {
20390                                 int jx = ix + offs[j][0];
20391                                 int jy = iy + offs[j][1];
20392                                 int jz = iz + offs[j][2];
20393 
20394                                 if( jx < 0 || jx >= nx ) continue;
20395                                 if( jy < 0 || jy >= ny ) continue;
20396                                 if( jz < 0 || jz >= nz ) continue;
20397 
20398 
20399                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20400                                     pts.push_back( point3d_t(jx, jy, jz) );
20401                                     visited->set_value_at( jx, jy, jz, (float)grpid );
20402                                 }
20403 
20404                         }
20405                 }
20406 
20407                 start = end;
20408                 end = pts.size();
20409         }
20410         return pts.size();
20411 }
20412 
20413 
20414 EMData* Util::get_biggest_cluster( EMData* mg )
20415 {
20416         int nx = mg->get_xsize();
20417         int ny = mg->get_ysize();
20418         int nz = mg->get_zsize();
20419 
20420         EMData* visited = new EMData();
20421         visited->set_size( nx, ny, nz );
20422         visited->to_zero();
20423         int grpid = 0;
20424         int maxgrp = 0;
20425         int maxsize = 0;
20426         for( int iz=0; iz < nz; ++iz ) {
20427                 for( int iy=0; iy < ny; ++iy ) {
20428                         for( int ix=0; ix < nx; ++ix ) {
20429                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20430 
20431                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
20432                                         // visited before, must be in other group.
20433                                         continue;
20434                                 }
20435 
20436                                 grpid++;
20437                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20438                                 if( grpsize > maxsize ) {
20439                                         maxsize = grpsize;
20440                                         maxgrp = grpid;
20441                                 }
20442                         }
20443                 }
20444         }
20445 
20446         Assert( maxgrp > 0 );
20447 
20448         int npoint = 0;
20449         EMData* result = new EMData();
20450         result->set_size( nx, ny, nz );
20451         result->to_zero();
20452 
20453         for( int iz=0; iz < nz; ++iz ) {
20454                 for( int iy=0; iy < ny; ++iy ) {
20455                         for( int ix=0; ix < nx; ++ix ) {
20456                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
20457                                         (*result)(ix,iy,iz) = 1.0;
20458                                         npoint++;
20459                                 }
20460                         }
20461                 }
20462         }
20463 
20464         Assert( npoint==maxsize );
20465         delete visited;
20466         return result;
20467 
20468 }
20469 
20470 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)
20471 {
20472         int    ix, iy, iz;
20473         int    i,  j, k;
20474         int    nr2, nl2;
20475         float  az, ak;
20476         float  scx, scy, scz;
20477         int    offset = 2 - nx%2;
20478         int    lsm = nx + offset;
20479         EMData* ctf_img1 = new EMData();
20480         ctf_img1->set_size(lsm, ny, nz);
20481         float freq = 1.0f/(2.0f*ps);
20482         scx = 2.0f/float(nx);
20483         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20484         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20485         nr2 = ny/2 ;
20486         nl2 = nz/2 ;
20487         float pihalf = M_PI/2.0f;
20488         for ( k=0; k<nz;k++) {
20489                 iz = k;  if(k>nl2) iz=k-nz;
20490                 float oz2 = iz*scz*iz*scz;
20491                 for ( j=0; j<ny;j++) {
20492                         iy = j;  if(j>nr2) iy=j - ny;
20493                         float oy = iy*scy;
20494                         float oy2 = oy*oy;
20495                         for ( i=0; i<lsm/2; i++) {
20496                                 ix=i;
20497                                 if( dza == 0.0f) {
20498                                         ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20499                                         (*ctf_img1) (i*2,j,k)   = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20500                                 } else {
20501                                         float ox = ix*scx;
20502                                         ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20503                                         az = atan2(oy, ox);
20504                                         float dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f-pihalf));
20505                                         (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20506                                 }
20507                                 //(*ctf_img1) (i*2+1,j,k) = 0.0f;  PAP  I assumed new EMData sets to zero
20508                         }
20509                 }
20510         }
20511         ctf_img1->update();
20512         ctf_img1->set_complex(true);
20513         ctf_img1->set_ri(true);
20514         //ctf_img1->attr_dict["is_complex"] = 1;
20515         //ctf_img1->attr_dict["is_ri"] = 1;
20516         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20517         return ctf_img1;
20518 }
20519 /*
20520 #define  cent(i)     out[i+N]
20521 #define  assign(i)   out[i]
20522 vector<float> Util::cluster_pairwise(EMData* d, int K) {
20523 
20524         int nx = d->get_xsize();
20525         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20526         vector<float> out(N+K+2);
20527         if(N*(N-1)/2 != nx) {
20528                 //print  "  incorrect dimension"
20529                 return out;}
20530         //  assign random objects as centers
20531         for(int i=0; i<N; i++) assign(i) = float(i);
20532         // shuffle
20533         for(int i=0; i<N; i++) {
20534                 int j = Util::get_irand(0,N-1);
20535                 float temp = assign(i);
20536                 assign(i) = assign(j);
20537                 assign(j) = temp;
20538         }
20539         for(int k=0; k<K; k++) cent(k) = float(assign(k));
20540         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
20541         //
20542         for(int i=0; i<N; i++) assign(i) = 0.0f;
20543         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20544         bool change = true;
20545         int it = -1;
20546         while(change && disp < dispold) {
20547                 change = false;
20548                 dispold = disp;
20549                 it++;
20550                 //cout<<"Iteration:  "<<it<<endl;
20551                 // dispersion is a sum of distance from objects to object center
20552                 disp = 0.0f;
20553                 for(int i=0; i<N; i++) {
20554                         qm = 1.0e23f;
20555                         for(int k=0; k<K; k++) {
20556                                 if(float(i) == cent(k)) {
20557                                         qm = 0.0f;
20558                                         na = (float)k;
20559                                 } else {
20560                                         float dt = (*d)(mono(i,int(cent(k))));
20561                                         if(dt < qm) {
20562                                                 qm = dt;
20563                                                 na = (float)k;
20564                                         }
20565                                 }
20566                         }
20567                         disp += qm;
20568                         if(na != assign(i)) {
20569                                 assign(i) = na;
20570                                 change = true;
20571                         }
20572                 }
20573         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
20574                 //print disp
20575                 //print  assign
20576                 // find centers
20577                 for(int k=0; k<K; k++) {
20578                         qm = 1.0e23f;
20579                         for(int i=0; i<N; i++) {
20580                                 if(assign(i) == float(k)) {
20581                                         float q = 0.0;
20582                                         for(int j=0; j<N; j++) {
20583                                                 if(assign(j) == float(k)) {
20584                                                                 //it cannot be the same object
20585                                                         if(i != j)  q += (*d)(mono(i,j));
20586                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
20587                                                 }
20588                                         }
20589                                         if(q < qm) {
20590                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
20591                                                 qm = q;
20592                                                 cent(k) = float(i);
20593                                         }
20594                                 }
20595                         }
20596                 }
20597         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
20598         }
20599         out[N+K] = disp;
20600         out[N+K+1] = float(it);
20601         return  out;
20602 }
20603 #undef  cent
20604 #undef  assign
20605 */
20606 #define  cent(i)     out[i+N]
20607 #define  assign(i)   out[i]
20608 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20609         int nx = d->get_xsize();
20610         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20611         vector<float> out(N+K+2);
20612         if(N*(N-1)/2 != nx) {
20613                 //print  "  incorrect dimension"
20614                 return out;}
20615         //  assign random objects as centers
20616         for(int i=0; i<N; i++) assign(i) = float(i);
20617         // shuffle
20618         for(int i=0; i<N; i++) {
20619                 int j = Util::get_irand(0,N-1);
20620                 float temp = assign(i);
20621                 assign(i) = assign(j);
20622                 assign(j) = temp;
20623         }
20624         for(int k=0; k<K; k++) cent(k) = float(assign(k));
20625         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
20626         //
20627         for(int i=0; i<N; i++) assign(i) = 0.0f;
20628         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20629         bool change = true;
20630         int it = -1;
20631         int ct = -1;
20632         while ((change && disp < dispold) || ct > 0) {
20633 
20634                 change = false;
20635                 dispold = disp;
20636                 it++;
20637 
20638                 // dispersion is a sum of distance from objects to object center
20639                 disp = 0.0f;
20640                 ct = 0;
20641                 for(int i=0; i<N; i++) {
20642                         qm = 1.0e23f;
20643                         for(int k=0; k<K; k++) {
20644                                 if(float(i) == cent(k)) {
20645                                         qm = 0.0f;
20646                                         na = (float)k;
20647                                 } else {
20648                                         float dt = (*d)(mono(i,int(cent(k))));
20649                                         if(dt < qm) {
20650                                                 qm = dt;
20651                                                 na = (float)k;
20652                                         }
20653                                 }
20654                         }
20655 
20656 
20657                         // Simulated annealing
20658                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20659                             na = (float)(Util::get_irand(0, K));
20660                             qm = (*d)(mono(i,int(na)));
20661                             ct++;
20662                         }
20663 
20664                         disp += qm;
20665 
20666                         if(na != assign(i)) {
20667                                 assign(i) = na;
20668                                 change = true;
20669                         }
20670                 }
20671 
20672                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
20673                 T = T*F;
20674 
20675         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
20676                 //print disp
20677                 //print  assign
20678                 // find centers
20679                 for(int k=0; k<K; k++) {
20680                         qm = 1.0e23f;
20681                         for(int i=0; i<N; i++) {
20682                                 if(assign(i) == float(k)) {
20683                                         float q = 0.0;
20684                                         for(int j=0; j<N; j++) {
20685                                                 if(assign(j) == float(k)) {
20686                                                                 //it cannot be the same object
20687                                                         if(i != j)  q += (*d)(mono(i,j));
20688                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
20689                                                 }
20690                                         }
20691                                         if(q < qm) {
20692                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
20693                                                 qm = q;
20694                                                 cent(k) = float(i);
20695                                         }
20696                                 }
20697                         }
20698                 }
20699         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
20700         }
20701         out[N+K] = disp;
20702         out[N+K+1] = float(it);
20703         return  out;
20704 }
20705 #undef  cent
20706 #undef  assign
20707 /*
20708 #define  groupping(i,k)   group[i + k*m]
20709 vector<float> Util::cluster_equalsize(EMData* d, int m) {
20710         int nx = d->get_xsize();
20711         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20712         int K = N/m;
20713         //cout<<"  K  "<<K<<endl;
20714         vector<float> group(N+1);
20715         if(N*(N-1)/2 != nx) {
20716                 //print  "  incorrect dimension"
20717                 return group;}
20718         bool active[N];
20719         for(int i=0; i<N; i++) active[i] = true;
20720 
20721         float dm, qd;
20722         int   ppi, ppj;
20723         for(int k=0; k<K; k++) {
20724                 // find two most similiar objects among active
20725                 cout<<"  k  "<<k<<endl;
20726                 dm = 1.0e23;
20727                 for(int i=1; i<N; i++) {
20728                         if(active[i]) {
20729                                 for(int j=0; j<i; j++) {
20730                                         if(active[j]) {
20731                                                 qd = (*d)(mono(i,j));
20732                                                 if(qd < dm) {
20733                                                         dm = qd;
20734                                                         ppi = i;
20735                                                         ppj = j;
20736                                                 }
20737                                         }
20738                                 }
20739                         }
20740                 }
20741                 groupping(0,k) = float(ppi);
20742                 groupping(1,k) = float(ppj);
20743                 active[ppi] = false;
20744                 active[ppj] = false;
20745 
20746                 // find progressively objects most similar to those in the current list
20747                 for(int l=2; l<m; l++) {
20748                         //cout<<"  l  "<<l<<endl;
20749                         dm = 1.0e23;
20750                         for(int i=0; i<N; i++) {
20751                                 if(active[i]) {
20752                                         qd = 0.0;
20753                                         for(int j=0; j<l; j++) { //j in groupping[k]:
20754                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
20755                                                 int jj = int(groupping(j,k));
20756                         //cout<<"   "<<jj<<endl;
20757                                                 qd += (*d)(mono(i,jj));
20758                                         }
20759                                         if(qd < dm) {
20760                                                 dm = qd;
20761                                                 ppi = i;
20762                                         }
20763                                 }
20764                         }
20765                         groupping(l,k) = float(ppi);
20766                         active[ppi] = false;
20767                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
20768                 }
20769                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
20770         }
20771         // there might be remaining objects when N is not divisible by m, simply put them in one group
20772         if(N%m != 0) {
20773                 int j = K*m;
20774                 K++;
20775                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
20776                 for(int i=0; i<N; i++) {
20777                         if(active[i]) {
20778                                 group[j] = float(i);
20779                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
20780                                 j++;
20781                         }
20782                 }
20783         }
20784 
20785         int  cent[K];
20786          // find centers
20787         for(int k=0; k<K; k++) {
20788                 float qm = 1.0e23f;
20789                 for(int i=0; i<N; i++) {
20790                         if(group[i] == float(k)) {
20791                                 qd = 0.0;
20792                                 for(int j=0; j<N; j++) {
20793                                         if(group[j] == float(k)) {
20794                                                 //it cannot be the same object
20795                                                 if(i != j)  qd += (*d)(mono(i,j));
20796                                         }
20797                                 }
20798                                 if(qd < qm) {
20799                                         qm = qd;
20800                                         cent[k] = i;
20801                                 }
20802                         }
20803                 }
20804         }
20805         // dispersion is a sum of distances from objects to object center
20806         float disp = 0.0f;
20807         for(int i=0; i<N; i++) {
20808                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
20809         }
20810         group[N] = disp;
20811         return  group;
20812 }
20813 #undef  groupping
20814 */
20815 
20816 vector<float> Util::cluster_equalsize(EMData* d) {
20817         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
20818         int nx = d->get_xsize();
20819         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20820         int K = N/2;
20821         vector<float> group(N);
20822         if(N*(N-1)/2 != nx) {
20823                 //print  "  incorrect dimension"
20824                 return group;}
20825         //bool active[N];       //this does not compile in VS2005. --Grant Tang
20826         bool * active = new bool[N];
20827         for(int i=0; i<N; i++) active[i] = true;
20828 
20829         float dm, qd;
20830         int   ppi = 0, ppj = 0;
20831         for(int k=0; k<K; k++) {
20832                 // find pairs of most similiar objects among active
20833                 //cout<<"  k  "<<k<<endl;
20834                 dm = 1.0e23f;
20835                 for(int i=1; i<N; i++) {
20836                         if(active[i]) {
20837                                 for(int j=0; j<i; j++) {
20838                                         if(active[j]) {
20839                                                 qd = (*d)(i*(i - 1)/2 + j);
20840                                                 if(qd < dm) {
20841                                                         dm = qd;
20842                                                         ppi = i;
20843                                                         ppj = j;
20844                                                 }
20845                                         }
20846                                 }
20847                         }
20848                 }
20849                 group[2*k] = float(ppi);
20850                 group[1+2*k] = float(ppj);
20851                 active[ppi] = false;
20852                 active[ppj] = false;
20853         }
20854 
20855         delete [] active;
20856         active = NULL;
20857         return  group;
20858 }
20859 /*
20860 #define son(i,j)=i*(i-1)/2+j
20861 vector<float> Util::cluster_equalsize(EMData* d) {
20862         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
20863         int nx = d->get_xsize();
20864         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20865         int K = N/2;
20866         vector<float> group(N);
20867         if(N*(N-1)/2 != nx) {
20868                 //print  "  incorrect dimension"
20869                 return group;}
20870         //bool active[N];
20871         int  active[N];
20872         for(int i=0; i<N; i++) active[i] = i;
20873 
20874         float dm, qd;
20875         int   ppi = 0, ppj = 0, ln = N;
20876         for(int k=0; k<K; k++) {
20877                 // find pairs of most similiar objects among active
20878                 //cout<<"  k:  "<<k<<endl;
20879                 dm = 1.0e23;
20880                 for(int i=1; i<ln; i++) {
20881                         for(int j=0; j<i; j++) {
20882                                 //qd = (*d)(mono(active[i],active[j]));
20883                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
20884                                 if(qd < dm) {
20885                                         dm = qd;
20886                                         ppi = i;
20887                                         ppj = j;
20888                                 }
20889                         }
20890                 }
20891                 group[2*k]   = float(active[ppi]);
20892                 group[1+2*k] = float(active[ppj]);
20893                 //  Shorten the list
20894                 if(ppi > ln-3 || ppj > ln - 3) {
20895                         if(ppi > ln-3 && ppj > ln - 3) {
20896                         } else if(ppi > ln-3) {
20897                                 if(ppi == ln -1) active[ppj] = active[ln-2];
20898                                 else             active[ppj] = active[ln-1];
20899                         } else { // ppj>ln-3
20900                                 if(ppj == ln -1) active[ppi] = active[ln-2];
20901                                 else             active[ppi] = active[ln-1];
20902                         }
20903                 } else {
20904                         active[ppi] = active[ln-1];
20905                         active[ppj] = active[ln-2];
20906                 }
20907                 ln = ln - 2;
20908         }
20909         return  group;
20910 }
20911 
20912 */
20913 #define data(i,j) group[i*ny+j]
20914 vector<float> Util::vareas(EMData* d) {
20915         const float step=0.001f;
20916         int ny = d->get_ysize();
20917         //  input emdata should have size 2xN, where N is number of points
20918         //  output vector should be 2xN, first element is the number of elements
20919         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
20920         vector<float> group(2*ny);
20921         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20922         int K = int(1.0f/step) +1;
20923         int hit = 0;
20924         for(int kx=0; kx<=K; kx++) {
20925                 float tx = kx*step;
20926                 for(int ky=0; ky<=K; ky++) {
20927                         float ty = ky*step;
20928                         float dm = 1.0e23f;
20929                         for(int i=0; i<ny; i++) {
20930                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20931                                 if( qd < dm) {
20932                                         dm = qd;
20933                                         hit = i;
20934                                 }
20935                         }
20936                         data(0,hit) += 1.0f;
20937                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
20938                 }
20939         }
20940         return  group;
20941 }
20942 #undef data
20943 
20944 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20945 
20946         int nx = vol->get_xsize();
20947         int ny = vol->get_ysize();
20948         int nz = vol->get_zsize();
20949         float *vol_data = vol->get_data();
20950         int new_nx, new_ny;
20951 
20952         if (nz == 1)
20953                 throw ImageDimensionException("Error: Input must be a 3-D object");
20954         if ((dim < 1) || (dim > 3))
20955                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20956         if (((dim == 1) && (index < 0 || index > nx-1)) ||
20957           ((dim == 1) && (index < 0 || index > nx-1)) ||
20958           ((dim == 1) && (index < 0 || index > nx-1)))
20959                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20960 
20961         if (dim == 1) {
20962                 new_nx = ny;
20963                 new_ny = nz;
20964         } else if (dim == 2) {
20965                 new_nx = nx;
20966                 new_ny = nz;
20967         } else {
20968                 new_nx = nx;
20969                 new_ny = ny;
20970         }
20971 
20972         EMData *slice = new EMData();
20973         slice->set_size(new_nx, new_ny, 1);
20974         float *slice_data = slice->get_data();
20975 
20976         if (dim == 1) {
20977                 for (int x=0; x<new_nx; x++)
20978                         for (int y=0; y<new_ny; y++)
20979                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20980         } else if (dim == 2) {
20981                 for (int x=0; x<new_nx; x++)
20982                         for (int y=0; y<new_ny; y++)
20983                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20984         } else {
20985                 for (int x=0; x<new_nx; x++)
20986                         for (int y=0; y<new_ny; y++)
20987                                 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20988         }
20989 
20990         return slice;
20991 }
20992 
20993 void Util::image_mutation(EMData *img, float mutation_rate) {
20994         int nx = img->get_xsize();
20995         float min = img->get_attr("minimum");
20996         float max = img->get_attr("maximum");
20997         float* img_data = img->get_data();
20998         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20999         return;
21000 }
21001 
21002 
21003 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
21004 
21005         if (is_mirror != 0) {
21006                 for (int i=0; i<len_list; i++) {
21007                         int r = rand()%10000;
21008                         float f = r/10000.0f;
21009                         if (f < mutation_rate) list[i] = 1-list[i];
21010                 }
21011         } else {
21012                 map<int, vector<int> >  graycode;
21013                 map<vector<int>, int> rev_graycode;
21014                 vector <int> gray;
21015 
21016                 int K=1;
21017                 for (int i=0; i<L; i++) K*=2;
21018 
21019                 for (int k=0; k<K; k++) {
21020                         int shift = 0;
21021                         vector <int> gray;
21022                         for (int i=L-1; i>-1; i--) {
21023                                 int t = ((k>>i)%2-shift)%2;
21024                                 gray.push_back(t);
21025                                 shift += t-2;
21026                         }
21027                         graycode[k] = gray;
21028                         rev_graycode[gray] = k;
21029                 }
21030 
21031                 float gap = (K-1)/(max_val-min_val);
21032                 for (int i=0; i<len_list; i++) {
21033                         float val = list[i];
21034                         if (val < min_val) { val = min_val; }
21035                         else if  (val > max_val) { val = max_val; }
21036                         int k = int((val-min_val)*gap+0.5);
21037                         vector<int> gray = graycode[k];
21038                         bool changed = false;
21039                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
21040                                 int r = rand()%10000;
21041                                 float f = r/10000.0f;
21042                                 if (f < mutation_rate) {
21043                                         *p = 1-*p;
21044                                         changed = true;
21045                                 }
21046                         }
21047                         if (changed) {
21048                                 k = rev_graycode[gray];
21049                                 list[i] = k/gap+min_val;
21050                         }
21051                 }
21052         }
21053 
21054 }
21055 
21056 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
21057 
21058         if (is_mirror != 0) {
21059                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
21060                         int r = rand()%10000;
21061                         float f = r/10000.0f;
21062                         if (f < mutation_rate) *q = 1-*q;
21063                 }
21064         } else {
21065                 map<int, vector<int> >  graycode;
21066                 map<vector<int>, int> rev_graycode;
21067                 vector <int> gray;
21068 
21069                 int K=1;
21070                 for (int i=0; i<L; i++) K*=2;
21071 
21072                 for (int k=0; k<K; k++) {
21073                         int shift = 0;
21074                         vector <int> gray;
21075                         for (int i=L-1; i>-1; i--) {
21076                                 int t = ((k>>i)%2-shift)%2;
21077                                 gray.push_back(t);
21078                                 shift += t-2;
21079                         }
21080                         graycode[k] = gray;
21081                         rev_graycode[gray] = k;
21082                 }
21083 
21084                 float gap = (K-1)/(max_val-min_val);
21085                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
21086                         float val = *q;
21087                         if (val < min_val) { val = min_val; }
21088                         else if  (val > max_val) { val = max_val; }
21089                         int k = int((val-min_val)*gap+0.5);
21090                         vector<int> gray = graycode[k];
21091                         bool changed = false;
21092                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
21093                                 int r = rand()%10000;
21094                                 float f = r/10000.0f;
21095                                 if (f < mutation_rate) {
21096                                         *p = 1-*p;
21097                                         changed = true;
21098                                 }
21099                         }
21100                         if (changed) {
21101                                 k = rev_graycode[gray];
21102                                 *q = k/gap+min_val;
21103                         }
21104                 }
21105         }
21106         return list;
21107 }
21108 
21109 
21110 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
21111         //cout<<"sanitycheck called\n";
21112         int total_cost = *output;
21113         int num_matches = *(output+1);
21114 
21115         int cost=0;
21116         int* intx;
21117         int intx_size;
21118         int* intx_next(0);
21119         int intx_next_size = 0;
21120         int curclass;
21121         int curclass_size;
21122         //cout<<"cost by match: [";
21123         for(int i = 0; i < num_matches; i++){
21124                 curclass = *(output+2+ i*nParts);
21125                 // check feasibility
21126                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
21127                 *(argParts + Indices[curclass]+1) = -5;
21128                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
21129                 curclass_size = *(dimClasses+curclass)-2;
21130                 intx = new int[curclass_size];
21131                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
21132                 intx_size = curclass_size;
21133 
21134                 for (int j=1; j < nParts; j++){
21135                       curclass = *(output+2+ i*nParts+j);
21136                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
21137                       *(argParts + Indices[j*K+curclass]+1)=-5;
21138                       // compute the intersection of intx and class curclass of partition j of the i-th match
21139                       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);
21140                       intx_next = new int[intx_next_size];
21141                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
21142                       delete[] intx;
21143                       intx=intx_next;
21144                       intx_size= intx_next_size;
21145                 }
21146                 delete[] intx_next;
21147 
21148                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
21149                 //cout <<intx_next_size<<",";
21150                 cost = cost + intx_next_size;
21151         }
21152         //cout<<"]\n";
21153         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
21154 
21155         return 1;
21156 
21157 }
21158 
21159 
21160 // Given J, returns the J matches with the largest weight
21161 // matchlist has room for J matches
21162 // costlist has J elements to record cost of the J largest matches
21163 
21164 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
21165         
21166         // some temp variables
21167         bool flag = 0;
21168         int nintx;
21169         int* dummy(0);
21170         //int* ret;
21171         int* curbranch = new int[nParts];
21172         
21173         //initialize costlist to all 0
21174         for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
21175         
21176         
21177         for(int a=0; a<K; a++)
21178         {
21179         
21180                 // 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
21181                 if (*(argParts + Indices[a] + 1) < 1) continue;
21182                 if (*(dimClasses + a)-2 <= T) continue;
21183 
21184                 // 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
21185 
21186                 for( int i=1; i < nParts; i++){
21187                         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.
21188                         for(int j=0; j < K; j++){
21189                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
21190                                 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);
21191                                 if (nintx > T) flag=1;
21192                                 else *(argParts + Indices[i*K+j] + 1) =-4;
21193                         }
21194                         if (flag==0) {break;}
21195                 }
21196 
21197                 // explore determines J matchs with the largest weight greater than T where class in partition 0 is class a
21198                 *curbranch = a;
21199 
21200                 if (flag > 0) // Each partition has one or more active class
21201                         Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
21202                         *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
21203                         
21204                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
21205                 for( int i=1; i < nParts; i++){
21206                         for(int j=0; j < K; j++){
21207                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
21208 
21209                         }
21210                 }
21211         }
21212         
21213         delete[] curbranch;
21214 }
21215 
21216 // returns J largest matches
21217 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){
21218 
21219 // depth is the level which is going to be explored in the current iteration
21220         int* curintx2(0);
21221         int nintx = size_curintx;
21222         
21223         
21224         // 2. take the intx of next and cur. Prune if <= T
21225         if (depth >0){
21226                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
21227                 if (nintx <= T) return; //prune!
21228         }
21229 
21230         // 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
21231         if (depth == (nParts-1)) {
21232                 
21233                 int replace = 0;
21234                 int ind_smallest = -1;
21235                 int smallest_cost = -1;
21236                 
21237                 for (int jit = 0; jit < J; jit++){
21238                         if (*(costlist+jit) < nintx){
21239                                 replace = 1;
21240                                 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21241                                 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21242                         }       
21243                 }
21244                 
21245                 if (replace > 0){
21246                         // replace the smallest cost in matchlist with the current stuff
21247                         *(costlist + ind_smallest) = nintx;
21248                         for (int xit = 0; xit < nParts; xit++)
21249                                 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
21250                                 
21251                 }
21252                 
21253                 return; 
21254         }
21255         
21256 
21257         // 3. have not yet reached a leaf, and current weight is still greather than T, so keep on going.
21258 
21259         if (depth > 0){
21260                 curintx2 = new int[nintx]; // put the intersection set in here
21261                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
21262         }
21263 
21264         if (depth == 0){
21265                 // set curintx2 to curintx
21266                 curintx2 = new int[size_curintx];
21267                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
21268         }
21269 
21270 
21271         // recursion (non-leaf case)
21272         depth=depth+1;
21273         // we now consider each of the classes in partition depth and recurse upon each of them
21274         for (int i=0; i < K; i++){
21275 
21276                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
21277                 size_next = (*(dimClasses + depth*K+i ))-2;
21278                 if (size_next <= T) continue;
21279                 *(curbranch+depth) = i;
21280                 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
21281                         costlist, curbranch);
21282                 
21283         }
21284 
21285         delete[] curintx2;
21286 }
21287 
21288 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
21289         //cout<<"initial_prune\n";
21290         // simple initial pruning. For class indClass of partition indPart:
21291         // 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
21292         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
21293 
21294         // 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
21295 
21296         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
21297         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
21298 
21299         int* dummy(0);
21300         int* cref;
21301         int cref_size;
21302         int* ccomp;
21303         int ccomp_size;
21304         int nintx;
21305         for (int i=0; i < nParts; i++){
21306                 for (int j =0; j < K; j++){
21307 
21308                         // consider class Parts[i][j]
21309                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
21310                         cref_size = dimClasses[i*K+cref[0]]-2;
21311 
21312 
21313                         if (cref_size <= T){
21314                                 cref[0] = -1;
21315                                 continue;
21316                         }
21317                         bool done = 0;
21318                         for (int a = 0; a < nParts; a++){
21319                                 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
21320                                 bool hasActive=0;
21321                                 for (unsigned int b=0; b < Parts[a].size(); b++){
21322                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
21323                                         // remember first element of each class is the index of the class
21324                                         ccomp = Parts[a][b];
21325                                         ccomp_size= dimClasses[a*K+ccomp[0]]-2;
21326                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
21327 
21328 
21329                                         if (nintx <= T)
21330                                                 ccomp[1] = 0; // class Parts[a][b] is 'inactive' for cref
21331                                         else{
21332                                                 ccomp[1] = 1; // class Parts[a][b] is 'active' for cref
21333                                                 hasActive=1;
21334                                         }
21335                                 }
21336                                 // see if partition a has at least one active class.if not then we're done with cref
21337                                 if (hasActive < 1){
21338                                    done=1;
21339                                    break;
21340                                 }
21341 
21342                         }
21343 
21344                         if (done > 0){
21345                                 // remove class j from partition i
21346 
21347                                 cref[0] = -1; // mark for deletion later
21348                                 continue; // move on to class Parts[i][j+1]
21349                         }
21350 
21351                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
21352                         // 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.
21353 
21354                         // (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.
21355                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
21356                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
21357 
21358                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
21359                         //bool found = 1;
21360                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21361 
21362                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
21363                                 // Parts[i].erase(Parts[i].begin()+j);
21364                                 cref[0] = -1;
21365                         }
21366                 }
21367 
21368                 // Erase from Parts[i] all the classes that's being designated for erasure
21369 
21370                 for (int d = K-1; d > -1; d--){
21371                         if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21372                 }
21373 
21374         }
21375         //cout <<"number of classes left in each partition after initial prune\n";      
21376         // Print out how many classes are left in each partition
21377         //for (int i =0; i < nParts; i++)
21378         //      cout << Parts[i].size()<<", ";
21379         //cout << "\n";
21380 }
21381 
21382 
21383 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) {
21384 
21385 
21386         if (size_next <= T) return 0;
21387 
21388         // take the intx of next and cur
21389         int* curintx2(0);
21390         int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21391         if (nintx <= T) return 0;
21392 
21393         int old_depth=depth;
21394         if (depth == partref) depth = depth + 1; // we skip classes in partref
21395         if (depth == nParts &&  old_depth>0) return 1;
21396 
21397         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
21398 
21399         curintx2 = new int[nintx]; // put the intersection set in here
21400         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21401 
21402         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
21403 
21404         // we now consider each of the classes in partition (depth+1) in turn
21405         bool gt_thresh;
21406         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
21407 
21408         for (int i=0; i < num_classes; i++){
21409                 if (Parts[depth][i][1] < 1) continue; // class is not active so move on
21410                 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21411                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21412                 if (gt_thresh) { delete[] curintx2; return 1; }
21413         }
21414         delete[] curintx2;
21415         return 0;
21416 }
21417 
21418 
21419 
21420 
21421 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21422 int max_branching, float stmult, int branchfunc, int LIM) {
21423         
21424         
21425         // 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
21426         // 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
21427         // Make a vector of nParts vectors of K int* each
21428          int* Indices = new int[nParts*K];
21429          int ind_c = 0;
21430          for (int i=0; i < nParts; i++){
21431                  for(int j = 0; j < K; j++){
21432                          Indices[i*K + j] = ind_c;
21433                          ind_c = ind_c + dimClasses[i*K + j];
21434                  }
21435          }
21436 
21437         // do initial pruning on argParts and return the pruned partitions
21438 
21439         // Make a vector of nParts vectors of K int* each
21440         vector <vector <int*> > Parts(nParts,vector<int*>(K));
21441         ind_c = 0;
21442         int argParts_size=0;
21443         for (int i=0; i < nParts; i++){
21444                 for(int j = 0; j < K; j++){
21445                         Parts[i][j] = argParts + ind_c;
21446                         ind_c = ind_c + dimClasses[i*K + j];
21447                         argParts_size = argParts_size + dimClasses[i*K + j];
21448                 }
21449         }
21450 
21451         // in the following we call initial_prune with Parts which is a vector. This is not the most
21452         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
21453         // 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.....
21454 
21455         // comment out for testing
21456         Util::initial_prune(Parts, dimClasses, nParts, K, T);
21457         for(int i = 0; i < nParts; i++){
21458                 for(int j=0; j < K; j++){
21459                         argParts[Indices[i*K + j]+1] = -1;
21460                 }
21461         }
21462 
21463         int num_classes;
21464         int old_index;
21465         for(int i=0; i<nParts; i++){
21466                 num_classes = Parts[i].size();// number of classes in partition i after pruning
21467                 for (int j=0; j < num_classes; j++){
21468                         old_index = Parts[i][j][0];
21469                         //cout << "old_index: " << old_index<<"\n";
21470                         argParts[Indices[i*K + old_index]+1] = 1;
21471                 }
21472         }
21473 
21474 
21475         // if we're not doing mpi then keep going and call branchMPI and return the output
21476         //cout <<"begin partition matching\n";
21477         //int* dummy(0);
21478         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21479         
21480         //cout<<"total cost: "<<*output<<"\n";
21481         //cout<<"number of matches: "<<*(output+1)<<"\n";
21482         // 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
21483         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21484 
21485         delete[] Indices;
21486 
21487         // something is wrong with output of branchMPI!
21488         if (correct < 1){
21489                 cout << "something is wrong with output of branchMPI!\n";
21490                 vector<int> ret(1);
21491                 ret[0] = -1;
21492                 if (output != 0)  { delete[] output; output = 0; }
21493                 return ret;
21494         }
21495 
21496         // output is not nonsense, so now put it into a single dimension vector and return
21497         // 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
21498         // and the rest is the list of matches. output is one dimensional
21499 
21500         int output_size = 2 + output[1] * nParts;
21501         vector<int> ret(output_size);
21502         for (int i = 0; i < output_size; i++) {
21503                 ret[i]= output[i];
21504         }
21505         if (output != 0) { delete[] output; output = 0; }
21506         return ret;
21507 
21508 }
21509 
21510 
21511 int branch_all=0;
21512 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21513 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21514 
21515 //*************************************
21516 //testing search2
21517 if (1 == 0){
21518 cout <<"begin test search2\n";
21519 int* matchlist = new int[J*nParts];
21520 int* costlist = new int[J];
21521 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21522 Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
21523 
21524 for (int jit = 0; jit < J; jit++) {
21525   cout << *(costlist +jit)<<": ";
21526   for (int yit = 0; yit < nParts; yit++)
21527         cout << *(matchlist + jit*nParts + yit)<<",";
21528   cout <<"\n";  
21529 
21530 }
21531 cout <<"end test search2\n";
21532 int* output = new int[1];
21533 output[0] = 1;
21534 delete [] matchlist;
21535 delete [] costlist;
21536 return output;
21537 }
21538 //**************************************
21539 
21540         // Base Case: we're at a leaf, no more feasible matches possible
21541         if (curlevel > K -1){
21542                 int* output = new int[2];
21543                 output[0] = 0;
21544                 output[1] = 0;
21545                 return output;
21546         }
21547 
21548         // branch dynamically depending on results of search 2!
21549         
21550         int* matchlist = new int[J*nParts];
21551         int* costlist = new int[J];
21552         Util::search2(argParts, Indices, dimClasses, nParts, K,  T, matchlist, costlist, J);
21553         
21554         
21555         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
21556         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
21557 
21558         // if there are no feasible matches with cost gt T, then return 0
21559         for (int jit = 0; jit < J ; jit++){
21560         
21561                 if (costlist[jit] > T) break;
21562                 if (jit == J-1){
21563                         int* output = new int[2];
21564                         output[0] = 0;
21565                         output[1] = 0;
21566                         delete[] matchlist;
21567                         delete[] costlist;
21568                         return output;
21569                 }
21570         }
21571         
21572 
21573         
21574         // note that costlist and matchlist are NOT sorted by weight, and branch factor takes care of that...
21575         if (curlevel==0) branch_all = 0;
21576         
21577         int nBranches = -1;
21578 
21579         if (branchfunc == 0)
21580                 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
21581 
21582         if (branchfunc == 2)
21583                 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
21584 
21585         if (branchfunc == 3)
21586                 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
21587 
21588         if (branchfunc == 4)
21589                 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
21590 
21591         int* newcostlist= new int[nBranches];
21592         int* newmatchlist = new int[nBranches*nParts];
21593         for (int i=0; i<nBranches; i++){
21594                 newcostlist[i] = costlist[i];
21595                 for (int j=0; j< nParts; j++)
21596                         newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21597         }
21598 
21599         delete[] costlist;
21600         delete[] matchlist;
21601         
21602         //int* output = new int[2];//initialize to placeholder
21603         int* output = new int[2+K*nParts];//initialize to placeholder
21604         output[0] = 0;
21605         output[1] = 0;
21606         // some temporary variables
21607         int old_index;
21608         int totalcost;
21609         int nmatches;
21610         //int offset;
21611 
21612         for(int i=0; i < nBranches ; i++){
21613 
21614                 // consider the i-th match returned by findTopLargest
21615                 //if (newcostlist[i] <= T) continue;
21616 
21617                 // 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.
21618                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
21619 
21620                 for(int j=0; j < nParts; j++){
21621                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
21622                         old_index = newmatchlist[i*nParts + j];
21623                         argParts[Indices[j*K+old_index] + 1] = -2;
21624                 }
21625 
21626                 
21627                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21628                 J, max_branching, stmult,branchfunc, LIM);
21629                 
21630                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
21631                 totalcost = newcostlist[i] + ret[0];
21632 
21633                 //if (curlevel == 0) {
21634                 //      cout <<"totalcost*****************************************************************: "<<totalcost<<", costlist["<<i<<"]="<<newcostlist[i]<<", *ret="<<*ret<<", level: "<<curlevel<<"\n";
21635                         
21636                 //}
21637                 if (totalcost > output[0]) // option 1
21638                 {
21639                         nmatches = 1 + ret[1];
21640                         //delete[] output; // get rid of the old maxreturn
21641                         //output = new int[2+nmatches*nParts];
21642                         output[0] = totalcost;
21643                         output[1] = nmatches;
21644                         int nret = 2+(nmatches-1)*nParts;
21645                         for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21646                         for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21647                 }
21648 
21649 
21650                 delete[] ret;
21651 
21652                 // unmark the marked classes in preparation for the next iteration
21653 
21654                 for(int j=0; j < nParts; j++){
21655                         old_index = newmatchlist[i*nParts + j];
21656                         argParts[Indices[j*K+old_index] + 1] = 1;
21657                 }
21658 
21659         }
21660 
21661         delete[] newmatchlist;
21662         delete[] newcostlist;
21663         
21664         return output;
21665 }
21666 
21667 int* costlist_global;
21668 // make global costlist
21669 bool jiafunc(int i, int j){
21670         return (costlist_global[j] < costlist_global[i]) ;
21671 
21672 }
21673 // 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).
21674 // 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.
21675 // Branch on subsequent ones only if its infeasible with ALL the ones which we have previously decided to branch on.
21676 // 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.
21677 // 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.
21678 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21679         
21680         int ntot=0;
21681         for (int jit=0; jit < J; jit++){
21682                 if (*(costlist+jit) > T) ntot++;
21683         }
21684 
21685         int cur;
21686         // sort matchlist by cost
21687         int* indx = new int[J];
21688         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21689         vector<int> myindx (indx, indx+J);
21690         vector<int>::iterator it;
21691         costlist_global=costlist;
21692         sort(myindx.begin(), myindx.end(), jiafunc);
21693 
21694         // put matchlist in the order of mycost
21695         int* templist = new int[J];
21696         int* temp2list = new int[J*nParts];
21697         int next = 0;
21698         
21699         for (it=myindx.begin(); it!=myindx.end();++it){
21700                 cur = *(costlist + *it);
21701                 if (cur > T){
21702                         
21703                         templist[next] = cur;
21704                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21705                         next = next + 1;
21706                 }
21707         }
21708         
21709         for (int jit=0; jit < ntot; jit++){
21710                 *(costlist+jit)=*(templist + jit);
21711                 //cout <<*(costlist+jit)<<", ";
21712                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21713         }
21714         //cout <<"\n";
21715         
21716         delete [] indx;
21717         //compute the average 
21718         
21719         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21720         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21721         
21722         
21723         int B=1;
21724         int B_init=B;
21725         int infeasible=0;
21726         
21727         for (int i=B_init; i<ntot; i++){
21728                 if (i==ntot) continue;
21729                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21730                 // branch on
21731                 infeasible = 0;
21732                 if (LIM < 0) LIM = B;
21733                 for (int j=0; j<B; j++){
21734                         
21735                         for (int vit=0; vit<nParts; vit++){
21736                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21737                         }
21738                         if (infeasible >= LIM) break;
21739                 }
21740                 
21741                 if (infeasible >= LIM){
21742                         *(costlist+B)=*(templist+i);
21743                         for (int vit=0; vit < nParts; vit++)
21744                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21745                         B=B+1;  
21746                 }
21747         }
21748         
21749         delete [] templist;
21750         delete [] temp2list;
21751         //cout<<"**************************************** "<<B<<" ***************************\n";
21752         
21753         if (branch_all < max_branching){
21754                 if (B>1)
21755                         {branch_all = branch_all + B -1 ; }
21756         }
21757         else B=1;
21758         
21759         return B;
21760         
21761 
21762 }
21763 
21764 
21765 // 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.
21766 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21767         
21768         int ntot=0;
21769         for (int jit=0; jit < J; jit++){
21770                 if (*(costlist+jit) > T) ntot++;
21771         }
21772 
21773         int cur;
21774         // sort matchlist by cost
21775         int* indx = new int[J];
21776         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21777         vector<int> myindx (indx, indx+J);
21778         vector<int>::iterator it;
21779         costlist_global=costlist;
21780         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21781 
21782         // put matchlist in the order of mycost
21783         int* templist = new int[J];
21784         int* temp2list = new int[J*nParts];
21785         int next = 0;
21786         
21787         for (it=myindx.begin(); it!=myindx.end();++it){
21788                 cur = *(costlist + *it);
21789                 if (cur > T){
21790                         
21791                         templist[next] = cur;
21792                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21793                         next = next + 1;
21794                 }
21795         }
21796         
21797         for (int jit=0; jit < ntot; jit++){
21798                 *(costlist+jit)=*(templist + jit);
21799                 //cout <<*(costlist+jit)<<", ";
21800                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21801         }
21802         //cout <<"\n";
21803         
21804         delete [] indx;
21805         //compute the average 
21806         
21807         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21808         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21809         
21810         
21811         int B=1;
21812         int B_init=B;
21813         int infeasible=0;
21814         // if we're near the bottom of the tree then explore more... this is because the larger weights are not likely to change much,
21815         // whereas the smaller ones can have many permutations
21816         if (LIM < 0) LIM = ntot-1;
21817         for (int i=B_init; i<ntot; i++){
21818                 if (i==ntot) continue;
21819                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21820                 // branch on
21821                 infeasible = 0;
21822                 
21823                 for (int j=0; j<ntot; j++){
21824                         if (j == i) continue;
21825                         for (int vit=0; vit<nParts; vit++){
21826                                 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21827                         }
21828                         if (infeasible >= LIM) break;
21829                 }
21830                 
21831                 if (infeasible >= LIM){
21832                         *(costlist+B)=*(templist+i);
21833                         for (int vit=0; vit < nParts; vit++)
21834                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21835                         B=B+1;  
21836                 }
21837         }
21838         
21839         delete [] templist;
21840         delete [] temp2list;
21841         //cout<<"**************************************** "<<B<<" ***************************\n";
21842         
21843         
21844         if (branch_all < max_branching){
21845                 if (B>1)
21846                         {branch_all = branch_all + B-1;}
21847         }
21848         else B=1;
21849         
21850         return B;
21851         
21852 
21853 }
21854 
21855 // 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
21856 // match. Otherwise, we branch on similar weighted matches.
21857 // As before we always branch on the match with the largest cost so worst case we'll get greedy.
21858 // 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.
21859 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21860         int sum=0;
21861         float average =0;
21862         int ntot=0;
21863         for (int jit=0; jit < J; jit++){
21864                 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21865         }
21866         average = ((float)sum)/((float)ntot);
21867         int cur;
21868         // sort matchlist by cost
21869         int* indx = new int[J];
21870         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21871         vector<int> myindx (indx, indx+J);
21872         vector<int>::iterator it;
21873         costlist_global=costlist;
21874         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21875 
21876         // put matchlist in the order of mycost
21877         int* templist = new int[J];
21878         int* temp2list = new int[J*nParts];
21879         int next = 0;
21880         
21881         for (it=myindx.begin(); it!=myindx.end();++it){
21882                 cur = *(costlist + *it);
21883                 if (cur > T){
21884                         
21885                         templist[next] = cur;
21886                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21887                         next = next + 1;
21888                 }
21889         }
21890         
21891         for (int jit=0; jit < ntot; jit++){
21892                 *(costlist+jit)=*(templist + jit);
21893                 //cout <<*(costlist+jit)<<", ";
21894                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21895         }
21896         //cout <<"\n";
21897         
21898         delete [] indx;
21899         delete [] templist;
21900         delete [] temp2list;
21901         
21902         if (ntot == 1) return 1;
21903         
21904         // look at the average, standard dev etc. If standard dev very small, i.e., costs very similar, then branch on the similar
21905         // costs
21906         float sq_sum=0.0;
21907         //cout <<"costlist:";
21908         for (int i=0; i< ntot; i++){
21909                 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21910                 //cout <<*(costlist+i)<<", ";
21911         }       
21912         //cout <<"\n";
21913         
21914         float variance = sq_sum/ntot;
21915         float stdev = (float)pow((float)variance,(float)0.5);
21916         
21917         //cout <<"stdev: "<<int(stdev)<<"\n";
21918         
21919         int B=1;
21920         int largest = *costlist;
21921         //cout <<"largest: "<<largest<<"\n";
21922         for (int i=1; i<ntot; i++){
21923                 int cur = *(costlist+i);
21924                 if (largest-cur < (float)(stdev*stmult)) B++;
21925                 else break;
21926         
21927         }
21928         //cout <<"B: "<<B<<"\n";
21929         if (branch_all < max_branching){
21930                 if (B>1)
21931                         {branch_all = branch_all + B-1;}
21932         }
21933         else B=1;
21934         
21935         return B;
21936         
21937 
21938 }
21939 
21940 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21941         
21942         int ntot=0;
21943         for (int jit=0; jit < J; jit++){
21944                 if (*(costlist+jit) > T) ntot++;
21945         }
21946 
21947         int cur;
21948         // sort matchlist by cost
21949         int* indx = new int[J];
21950         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21951         vector<int> myindx (indx, indx+J);
21952         vector<int>::iterator it;
21953         costlist_global=costlist;
21954         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21955 
21956         // put matchlist in the order of mycost
21957         int* templist = new int[J];
21958         int* temp2list = new int[J*nParts];
21959         int next = 0;
21960         
21961         for (it=myindx.begin(); it!=myindx.end();++it){
21962                 cur = *(costlist + *it);
21963                 if (cur > T){
21964                         
21965                         templist[next] = cur;
21966                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21967                         next = next + 1;
21968                 }
21969         }
21970         
21971         for (int jit=0; jit < ntot; jit++){
21972                 *(costlist+jit)=*(templist + jit);
21973                 //cout <<*(costlist+jit)<<", ";
21974                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21975         }
21976         //cout <<"\n";
21977         
21978         for (int jit=1; jit < ntot; jit++){
21979         
21980              if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21981              
21982                      for (int vit=0; vit < nParts; vit++){
21983                              if ( matchlist[jit*nParts + vit] >  matchlist[vit])
21984                                  break;
21985                              if ( matchlist[jit*nParts + vit] ==  matchlist[vit])
21986                                  continue;
21987                              if ( matchlist[jit*nParts + vit] <  matchlist[vit])
21988                              {
21989                                  // swap
21990                                  for (int swp=0; swp < nParts; swp++){
21991                                        int tmp  = matchlist[swp];
21992                                        matchlist[swp]= matchlist[jit*nParts + swp];
21993                                        matchlist[jit*nParts + swp] = tmp;
21994                                  }
21995                                  break;
21996                              
21997                              }   
21998                      }
21999              }
22000         
22001         }
22002         
22003         
22004         delete [] indx;
22005         //compute the average 
22006         
22007         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
22008         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
22009         
22010         
22011         int B=1;
22012         int B_init=B;
22013         int infeasible=0;
22014         
22015         for (int i=B_init; i<ntot; i++){
22016                 if (i==ntot) continue;
22017                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
22018                 // branch on
22019                 infeasible = 0;
22020                 if (LIM < 0) LIM = B;
22021                 for (int j=0; j<B; j++){
22022                         
22023                         for (int vit=0; vit<nParts; vit++){
22024                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
22025                         }
22026                         if (infeasible >= LIM) break;
22027                 }
22028                 
22029                 if (infeasible >= LIM){
22030                         *(costlist+B)=*(templist+i);
22031                         for (int vit=0; vit < nParts; vit++)
22032                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
22033                         B=B+1;  
22034                 }
22035         }
22036         
22037         delete [] templist;
22038         delete [] temp2list;
22039         //cout<<"**************************************** "<<B<<" ***************************\n";
22040         
22041         if (branch_all < max_branching){
22042                 if (B>1)
22043                         {branch_all = branch_all + B -1 ; }
22044         }
22045         else B=1;
22046         
22047         return B;
22048         
22049 
22050 }

Generated on Fri Aug 10 16:35:31 2012 for EMAN2 by  doxygen 1.3.9.1