util_sparx.cpp

Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Pawel A.Penczek, 09/09/2006 (Pawel.A.Penczek@uth.tmc.edu)
00007  * Copyright (c) 2000-2006 The University of Texas - Houston Medical School
00008  *
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  *
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  *
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  *
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  *
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  *
00034  */
00035 
00036 #ifdef _WIN32
00037 #pragma warning(disable:4819)
00038 #endif  //_WIN32
00039 
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <cstdio>
00044 #include <cstdlib>
00045 #include <boost/format.hpp>
00046 #include "emdata.h"
00047 #include "util.h"
00048 #include "fundamentals.h"
00049 #include "lapackblas.h"
00050 #include "lbfgsb.h"
00051 using namespace EMAN;
00052 #include "steepest.h"
00053 #include "emassert.h"
00054 #include "randnum.h"
00055 
00056 #include <gsl/gsl_sf_bessel.h>
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <cmath>
00059 using namespace std;
00060 using std::complex;
00061 
00062 /* Subroutine */ 
00063 int circum_(double *, double *, double *, double *, int *);
00064 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00065 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00066 
00067 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00068 //  flip true:  find statistics under the mask (mask >0.5)
00069 //  flip false: find statistics ourside the mask (mask <0.5)
00070 {
00071         ENTERFUNC;
00072         vector<float> stats;
00073         float *Volptr, *maskptr,MAX,MIN;
00074         long double Sum1,Sum2;
00075         long count;
00076 
00077         MAX = -FLT_MAX;
00078         MIN =  FLT_MAX;
00079         count = 0L;
00080         Sum1  = 0.0L;
00081         Sum2  = 0.0L;
00082 
00083         if (mask == NULL) {
00084                 //Vol->update_stat();
00085                 stats.push_back(Vol->get_attr("mean"));
00086                 stats.push_back(Vol->get_attr("sigma"));
00087                 stats.push_back(Vol->get_attr("minimum"));
00088                 stats.push_back(Vol->get_attr("maximum"));
00089                 return stats;
00090         }
00091 
00092         /* Check if the sizes of the mask and image are same */
00093 
00094         size_t nx = Vol->get_xsize();
00095         size_t ny = Vol->get_ysize();
00096         size_t nz = Vol->get_zsize();
00097 
00098         size_t mask_nx = mask->get_xsize();
00099         size_t mask_ny = mask->get_ysize();
00100         size_t mask_nz = mask->get_zsize();
00101 
00102         if  (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00103                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00104 
00105  /*       if (nx != mask_nx ||
00106             ny != mask_ny ||
00107             nz != mask_nz  ) {
00108            // should throw an exception here!!! (will clean it up later CY)
00109            fprintf(stderr, "The dimension of the image does not match the dimension of the mask!\n");
00110            fprintf(stderr, " nx = %d, mask_nx = %d\n", nx, mask_nx);
00111            fprintf(stderr, " ny = %d, mask_ny = %d\n", ny, mask_ny);
00112            fprintf(stderr, " nz = %d, mask_nz = %d\n", nz, mask_nz);
00113            exit(1);
00114         }
00115  */
00116         Volptr = Vol->get_data();
00117         maskptr = mask->get_data();
00118 
00119         for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00120                 if ((maskptr[i]>0.5f) == flip) {
00121                         Sum1 += Volptr[i];
00122                         Sum2 += Volptr[i]*double(Volptr[i]);
00123                         MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00124                         MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00125                         count++;
00126                 }
00127         }
00128 
00129         if (count == 0) {
00130                 LOGERR("Invalid mask");
00131                 throw ImageFormatException( "Invalid mask");
00132         }
00133 
00134         float avg = static_cast<float>(Sum1/count);
00135         float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00136 
00137         stats.push_back(avg);
00138         stats.push_back(sig);
00139         stats.push_back(MIN);
00140         stats.push_back(MAX);
00141 
00142         return stats;
00143 }
00144 
00145 
00146 //----------------------------------------------------------------------------------------------------------
00147 
00148 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00149 {
00150         ENTERFUNC;
00151 
00152         if (!EMUtil::is_same_size(V1, V2)) {
00153                 LOGERR("images not same size");
00154                 throw ImageFormatException( "images not same size");
00155         }
00156 
00157         size_t nx = V1->get_xsize();
00158         size_t ny = V1->get_ysize();
00159         size_t nz = V1->get_zsize();
00160         size_t size = (size_t)nx*ny*nz;
00161 
00162         EMData *BD = new EMData();
00163         BD->set_size(nx, ny, nz);
00164 
00165         float *params = new float[2];
00166 
00167         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00168         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00169         int nvox = 0L;
00170 
00171         V1ptr = V1->get_data();
00172         V2ptr = V2->get_data();
00173         BDptr = BD->get_data();
00174 
00175 
00176         if(!mask){
00177                 EMData * Mask = new EMData();
00178                 Mask->set_size(nx,ny,nz);
00179                 Mask->to_one();
00180                 MASKptr = Mask->get_data();
00181         } else {
00182                 if (!EMUtil::is_same_size(V1, mask)) {
00183                         LOGERR("mask not same size");
00184                         throw ImageFormatException( "mask not same size");
00185                 }
00186 
00187                 MASKptr = mask->get_data();
00188         }
00189 
00190 
00191 
00192 //       calculation of S1,S2,S3,S3,nvox
00193 
00194         for (size_t i = 0L;i < size; i++) {
00195               if (MASKptr[i]>0.5f) {
00196                S1 += V1ptr[i]*V2ptr[i];
00197                S2 += V1ptr[i]*V1ptr[i];
00198                S3 += V2ptr[i];
00199                S4 += V1ptr[i];
00200                nvox ++;
00201               }
00202         }
00203 
00204         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00205                 A =1.0f ;
00206         } else {
00207                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00208         }
00209         B = static_cast<float> (A*S4  -  S3)/nvox;
00210 
00211         // calculation of the difference image
00212 
00213         for (size_t i = 0L;i < size; i++) {
00214              if (MASKptr[i]>0.5f) {
00215                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00216              }  else  {
00217                BDptr[i] = 0.f;
00218              }
00219         }
00220 
00221         BD->update();
00222 
00223         params[0] = A;
00224         params[1] = B;
00225 
00226         Dict BDnParams;
00227         BDnParams["imdiff"] = BD;
00228         BDnParams["A"] = params[0];
00229         BDnParams["B"] = params[1];
00230 
00231         EXITFUNC;
00232         return BDnParams;
00233  }
00234 
00235 //----------------------------------------------------------------------------------------------------------
00236 
00237 
00238 
00239 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00240 {
00241         ENTERFUNC;
00242         int Mid= (Size+1)/2;
00243 
00244         if (flag==0) { // This is the real function
00245                 EMData* ImBW = new EMData();
00246                 ImBW->set_size(Size,Size,1);
00247                 ImBW->to_zero();
00248 
00249                 float tempIm;
00250                 float x,y;
00251 
00252                 for (int ix=(1-Mid);  ix<Mid; ix++){
00253                         for (int iy=(1-Mid);  iy<Mid; iy++){
00254                                 x = (float)ix;
00255                                 y = (float)iy;
00256                         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)) );
00257                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00258                         }
00259                 }
00260                 ImBW->update();
00261                 ImBW->set_complex(false);
00262                 ImBW->set_ri(true);
00263 
00264                 return ImBW;
00265         }
00266         else if (flag==1) {  // This is the Fourier Transform
00267                 EMData* ImBWFFT = new EMData();
00268                 ImBWFFT ->set_size(2*Size,Size,1);
00269                 ImBWFFT ->to_zero();
00270 
00271                 float r,s;
00272 
00273                 for (int ir=(1-Mid);  ir<Mid; ir++){
00274                         for (int is=(1-Mid);  is<Mid; is++){
00275                                 r = (float)ir;
00276                                 s = (float)is;
00277                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00278                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00279                         }
00280                 }
00281                 ImBWFFT->update();
00282                 ImBWFFT->set_complex(true);
00283                 ImBWFFT->set_ri(true);
00284                 ImBWFFT->set_shuffled(true);
00285                 ImBWFFT->set_fftodd(true);
00286 
00287                 return ImBWFFT;
00288         }
00289         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00290                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00291                 float C=cos(alpha);
00292                 float S=sin(alpha);
00293                 float D= sqrt(S*S*b*b + C*C*a*a);
00294                 //float D2 = D*D;   PAP - to get rid of warning
00295 
00296                 float P = p * C *a*a/D ;
00297                 float Q = q * S *b*b/D ;
00298 
00299                 if (flag==2) {
00300                         EMData* pofalpha = new EMData();
00301                         pofalpha ->set_size(Size,1,1);
00302                         pofalpha ->to_zero();
00303 
00304                         float Norm0 =  D*(float)sqrt(2*pi);
00305                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00306                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00307                         float sD;
00308 
00309                         for (int is=(1-Mid);  is<Mid; is++){
00310                                 sD = is/D ;
00311                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00312                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00313                         }
00314                         pofalpha-> update();
00315                         pofalpha-> set_complex(false);
00316                         pofalpha-> set_ri(true);
00317 
00318                         return pofalpha;
00319                 }
00320                 if (flag==3) { // This is the projection in Fourier Space
00321                         float vD;
00322 
00323                         EMData* pofalphak = new EMData();
00324                         pofalphak ->set_size(2*Size,1,1);
00325                         pofalphak ->to_zero();
00326 
00327                         for (int iv=(1-Mid);  iv<Mid; iv++){
00328                                 vD = iv*D ;
00329                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00330                         }
00331                         pofalphak-> update();
00332                         pofalphak-> set_complex(false);
00333                         pofalphak-> set_ri(true);
00334 
00335                         return pofalphak;
00336                 }
00337         }
00338         else if (flag==4) {
00339                 cout <<" FH under construction";
00340                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00341                 EMData* TryFH= OutFT -> real2FH(4.0);
00342                 return TryFH;
00343         } else {
00344                 cout <<" flag must be 0,1,2,3, or 4";
00345         }
00346 
00347         EXITFUNC;
00348         return 0;
00349 }
00350 
00351 
00352 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00353 {
00354 
00355         float x0= x[0];
00356         float x1= x[1];
00357         float x2= x[2];
00358         float y0= y[0];
00359         float y1= y[1];
00360         float y2= y[2];
00361         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00362         float xn  = x[n];
00363         float xnm1= x[n-1];
00364         float xnm2= x[n-2];
00365         float yn  = y[n];
00366         float ynm1= y[n-1];
00367         float ynm2= y[n-2];
00368         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00369         float *y2d = new float[n];
00370         Util::spline(x,y,n,yp1,ypn,y2d);
00371         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00372         delete [] y2d;
00373         return;
00374 }
00375 
00376 
00377 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00378 {
00379         int i,k;
00380         float p, qn, sig, un, *u;
00381         u = new float[n-1];
00382 
00383         if (yp1 > .99e30){
00384                 y2[0]=u[0]=0.0;
00385         } else {
00386                 y2[0]=-.5f;
00387                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00388         }
00389 
00390         for (i=1; i < n-1; i++) {
00391                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00392                 p = sig*y2[i-1] + 2.0f;
00393                 y2[i]  = (sig-1.0f)/p;
00394                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00395                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00396         }
00397 
00398         if (ypn>.99e30){
00399                 qn=0; un=0;
00400         } else {
00401                 qn= .5f;
00402                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00403         }
00404         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00405         for (k=n-2; k>=0; k--){
00406                 y2[k]=y2[k]*y2[k+1]+u[k];
00407         }
00408         delete [] u;
00409 }
00410 
00411 
00412 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00413 {
00414         int klo, khi, k;
00415         float h, b, a;
00416 
00417 //      klo=0; // can try to put here
00418         for (int j=0; j<m;j++){
00419                 klo=0;
00420                 khi=n-1;
00421                 while (khi-klo >1) {
00422                         k=(khi+klo) >>1;
00423                         if  (xa[k]>xq[j]){ khi=k;}
00424                         else { klo=k;}
00425                 }
00426                 h=xa[khi]- xa[klo];
00427                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00428                 a =(xa[khi]-xq[j])/h;
00429                 b=(xq[j]-xa[klo])/h;
00430                 yq[j]=a*ya[klo] + b*ya[khi]
00431                         + ((a*a*a-a)*y2a[klo]
00432                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00433         }
00434 //      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]);
00435 }
00436 
00437 
00438 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00439                float *weightofkValsSorted, int Size, int *SizeReturned)
00440 {
00441         int iMax = (int) floor( (Size-1.0)/2 +.01);
00442         int CountMax = (iMax+2)*(iMax+1)/2;
00443         int Count=-1;
00444         float *kVals     = new float[CountMax];
00445         float *weightMat = new float[CountMax];
00446         int *PermMat     = new   int[CountMax];
00447         SizeReturned[0] = CountMax;
00448 
00449 //      printf("Aa \n");        fflush(stdout);
00450         for (int jkx=0; jkx< iMax+1; jkx++) {
00451                 for (int jky=0; jky< jkx+1; jky++) {
00452                         Count++;
00453                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00454                         weightMat[Count]=  1.0;
00455                         if (jkx!=0)  { weightMat[Count] *=2;}
00456                         if (jky!=0)  { weightMat[Count] *=2;}
00457                         if (jkx!=jky){ weightMat[Count] *=2;}
00458                         PermMat[Count]=Count+1;
00459                 }
00460         }
00461 
00462         int lkVals = Count+1;
00463 //      printf("Cc \n");fflush(stdout);
00464 
00465         sort_mat(&kVals[0],&kVals[Count],
00466              &PermMat[0],  &PermMat[Count]);  //PermMat is
00467                                 //also returned as well as kValsSorted
00468         fflush(stdout);
00469 
00470         int newInd;
00471 
00472         for (int iP=0; iP < lkVals ; iP++ ) {
00473                 newInd =  PermMat[iP];
00474                 PermMatTr[newInd-1] = iP+1;
00475         }
00476 
00477 //      printf("Ee \n"); fflush(stdout);
00478 
00479         int CountA=-1;
00480         int CountB=-1;
00481 
00482         while (CountB< (CountMax-1)) {
00483                 CountA++;
00484                 CountB++;
00485 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00486                 kValsSorted[CountA] = kVals[CountB] ;
00487                 if (CountB<(CountMax-1) ) {
00488                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00489                                 SizeReturned[0]--;
00490                                 for (int iP=0; iP < lkVals; iP++){
00491 //                                      printf("iP=%d \n", iP);fflush(stdout);
00492                                         if  (PermMatTr[iP]>CountA+1) {
00493                                                 PermMatTr[iP]--;
00494                                         }
00495                                 }
00496                                 CountB++;
00497                         }
00498                 }
00499         }
00500 
00501 
00502         for (int CountD=0; CountD < CountMax; CountD++) {
00503             newInd = PermMatTr[CountD];
00504             weightofkValsSorted[newInd-1] += weightMat[CountD];
00505         }
00506 
00507 }
00508 
00509 
00510 vector<float>
00511 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00512 {
00513         vector<float> angles;
00514         float psi = 0.0;
00515         if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00516                 t1 = 0.0f;
00517                 t2 = 90.0f;
00518         }
00519         if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00520                 p1 = 0.0f;
00521                 p2 = 359.9f;
00522         }
00523         bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00524         for (float theta = t1; theta <= t2; theta += delta) {
00525                 float detphi;
00526                 int lt;
00527                 if ((0.0 == theta)||(180.0 == theta)) {
00528                         detphi = 360.0f;
00529                         lt = 1;
00530                 } else {
00531                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00532                         lt = int((p2 - p1)/detphi)-1;
00533                         if (lt < 1) lt = 1;
00534                         detphi = (p2 - p1)/lt;
00535                 }
00536                 for (int i = 0; i < lt; i++) {
00537                         float phi = p1 + i*detphi;
00538                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00539                         angles.push_back(phi);
00540                         angles.push_back(theta);
00541                         angles.push_back(psi);
00542                 }
00543         }
00544         return angles;
00545 }
00546 
00547 
00548 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00549 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00550 {
00551 
00552 //  purpose: quadratic interpolation
00553 //
00554 //  parameters:       xx,yy treated as circularly closed.
00555 //                    fdata - image 1..nxdata, 1..nydata
00556 //
00557 //                    f3    fc       f0, f1, f2, f3 are the values
00558 //                     +             at the grid points.  x is the
00559 //                     + x           point at which the function
00560 //              f2++++f0++++f1       is to be estimated. (it need
00561 //                     +             not be in the first quadrant).
00562 //                     +             fc - the outer corner point
00563 //                    f4             nearest x.
00564 c
00565 //                                   f0 is the value of the fdata at
00566 //                                   fdata(i,j), it is the interior mesh
00567 //                                   point nearest  x.
00568 //                                   the coordinates of f0 are (x0,y0),
00569 //                                   the coordinates of f1 are (xb,y0),
00570 //                                   the coordinates of f2 are (xa,y0),
00571 //                                   the coordinates of f3 are (x0,yb),
00572 //                                   the coordinates of f4 are (x0,ya),
00573 //                                   the coordinates of fc are (xc,yc),
00574 c
00575 //                   o               hxa, hxb are the mesh spacings
00576 //                   +               in the x-direction to the left
00577 //                  hyb              and right of the center point.
00578 //                   +
00579 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00580 //                   +               in the y-direction.
00581 //                  hya
00582 //                   +               hxc equals either  hxb  or  hxa
00583 //                   o               depending on where the corner
00584 //                                   point is located.
00585 c
00586 //                                   construct the interpolant
00587 //                                   f = f0 + c1*(x-x0) +
00588 //                                       c2*(x-x0)*(x-x1) +
00589 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00590 //                                       + c5*(x-x0)*(y-y0)
00591 //
00592 //
00593 
00594     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00595     float quadri;
00596     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00597 
00598     x = xx;
00599     y = yy;
00600 
00601     // circular closure
00602         while ( x < 1.0 ) x += nxdata;
00603         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00604         while ( y < 1.0 ) y += nydata;
00605         while ( y >= (float)(nydata+1) )  y -= nydata;
00606 
00607 
00608     i   = (int) x;
00609     j   = (int) y;
00610 
00611     dx0 = x - i;
00612     dy0 = y - j;
00613 
00614     ip1 = i + 1;
00615     im1 = i - 1;
00616     jp1 = j + 1;
00617     jm1 = j - 1;
00618 
00619     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00620     if (im1 < 1)      im1 = im1 + nxdata;
00621     if (jp1 > nydata) jp1 = jp1 - nydata;
00622     if (jm1 < 1)      jm1 = jm1 + nydata;
00623 
00624     f0  = fdata(i,j);
00625     c1  = fdata(ip1,j) - f0;
00626     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00627     c3  = fdata(i,jp1) - f0;
00628     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00629 
00630     dxb = dx0 - 1;
00631     dyb = dy0 - 1;
00632 
00633     // hxc & hyc are either 1 or -1
00634     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00635     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00636 
00637     ic  = i + hxc;
00638     jc  = j + hyc;
00639 
00640     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00641     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00642 
00643     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00644             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00645 
00646     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00647 
00648     return quadri;
00649 }*/
00650 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00651 {
00652 //  purpose: quadratic interpolation
00653 //  Optimized for speed, circular closer removed, checking of ranges removed
00654         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00655         float  quadri;
00656         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00657 
00658         x = xx;
00659         y = yy;
00660 
00661         //     any xx and yy
00662         while ( x < 1.0 )                 x += nxdata;
00663         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00664         while ( y < 1.0 )                 y += nydata;
00665         while ( y >= (float)(nydata+1) )  y -= nydata;
00666 
00667         i   = (int) x;
00668         j   = (int) y;
00669 
00670         dx0 = x - i;
00671         dy0 = y - j;
00672 
00673         ip1 = i + 1;
00674         im1 = i - 1;
00675         jp1 = j + 1;
00676         jm1 = j - 1;
00677 
00678         if (ip1 > nxdata) ip1 -= nxdata;
00679         if (im1 < 1)      im1 += nxdata;
00680         if (jp1 > nydata) jp1 -= nydata;
00681         if (jm1 < 1)      jm1 += nydata;
00682 
00683         f0  = fdata(i,j);
00684         c1  = fdata(ip1,j) - f0;
00685         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00686         c3  = fdata(i,jp1) - f0;
00687         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00688 
00689         dxb = dx0 - 1;
00690         dyb = dy0 - 1;
00691 
00692         // hxc & hyc are either 1 or -1
00693         if (dx0 >= 0) hxc = 1; else hxc = -1;
00694         if (dy0 >= 0) hyc = 1; else hyc = -1;
00695 
00696         ic  = i + hxc;
00697         jc  = j + hyc;
00698 
00699         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00700         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00701 
00702         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00703                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00704 
00705 
00706         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00707 
00708         return quadri;
00709 }
00710 
00711 #undef fdata
00712 
00713 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00714 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00715 {
00716 //  purpose: quadratic interpolation
00717 //  Optimized for speed, circular closer removed, checking of ranges removed
00718         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00719         float  quadri;
00720         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00721 
00722         x = xx;
00723         y = yy;
00724 
00725         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00726         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00727               x = (float)xnew;
00728                   y = (float)ynew;
00729      }
00730 
00731 
00732         i   = (int) x;
00733         j   = (int) y;
00734 
00735         dx0 = x - i;
00736         dy0 = y - j;
00737 
00738         ip1 = i + 1;
00739         im1 = i - 1;
00740         jp1 = j + 1;
00741         jm1 = j - 1;
00742 
00743         if (ip1 > nxdata) ip1 -= nxdata;
00744         if (im1 < 1)      im1 += nxdata;
00745         if (jp1 > nydata) jp1 -= nydata;
00746         if (jm1 < 1)      jm1 += nydata;
00747 
00748         f0  = fdata(i,j);
00749         c1  = fdata(ip1,j) - f0;
00750         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00751         c3  = fdata(i,jp1) - f0;
00752         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00753 
00754         dxb = dx0 - 1;
00755         dyb = dy0 - 1;
00756 
00757         // hxc & hyc are either 1 or -1
00758         if (dx0 >= 0) hxc = 1; else hxc = -1;
00759         if (dy0 >= 0) hyc = 1; else hyc = -1;
00760 
00761         ic  = i + hxc;
00762         jc  = j + hyc;
00763 
00764         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00765         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00766 
00767         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00768                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00769 
00770 
00771         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00772 
00773         return quadri;
00774 }
00775 
00776 #undef fdata
00777 
00778 
00779 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00780         int K = kb.get_window_size();
00781         int kbmin = -K/2;
00782         int kbmax = -kbmin;
00783         int kbc = kbmax+1;
00784 
00785         float pixel =0.0f;
00786         float w=0.0f;
00787 
00788         delx = restrict1(delx, nx);
00789         int inxold = int(round(delx));
00790         if ( ny < 2 ) {  //1D
00791                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00792                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00793                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00794                 float tablex4 = kb.i0win_tab(delx-inxold);
00795                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00796                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00797                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00798 
00799                 int x1, x2, x3, x4, x5, x6, x7;
00800 
00801                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00802                         x1 = (inxold-3+nx)%nx;
00803                         x2 = (inxold-2+nx)%nx;
00804                         x3 = (inxold-1+nx)%nx;
00805                         x4 = (inxold  +nx)%nx;
00806                         x5 = (inxold+1+nx)%nx;
00807                         x6 = (inxold+2+nx)%nx;
00808                         x7 = (inxold+3+nx)%nx;
00809                 } else {
00810                         x1 = inxold-3;
00811                         x2 = inxold-2;
00812                         x3 = inxold-1;
00813                         x4 = inxold;
00814                         x5 = inxold+1;
00815                         x6 = inxold+2;
00816                         x7 = inxold+3;
00817                 }
00818 
00819                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00820                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00821                         data[x7]*tablex7 ;
00822 
00823                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00824         } else if ( nz < 2 ) {  // 2D
00825                 dely = restrict1(dely, ny);
00826                 int inyold = int(round(dely));
00827                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00828                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00829                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00830                 float tablex4 = kb.i0win_tab(delx-inxold);
00831                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00832                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00833                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00834 
00835                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00836                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00837                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00838                 float tabley4 = kb.i0win_tab(dely-inyold);
00839                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00840                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00841                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00842 
00843                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00844 
00845                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
00846                         x1 = (inxold-3+nx)%nx;
00847                         x2 = (inxold-2+nx)%nx;
00848                         x3 = (inxold-1+nx)%nx;
00849                         x4 = (inxold  +nx)%nx;
00850                         x5 = (inxold+1+nx)%nx;
00851                         x6 = (inxold+2+nx)%nx;
00852                         x7 = (inxold+3+nx)%nx;
00853 
00854                         y1 = ((inyold-3+ny)%ny)*nx;
00855                         y2 = ((inyold-2+ny)%ny)*nx;
00856                         y3 = ((inyold-1+ny)%ny)*nx;
00857                         y4 = ((inyold  +ny)%ny)*nx;
00858                         y5 = ((inyold+1+ny)%ny)*nx;
00859                         y6 = ((inyold+2+ny)%ny)*nx;
00860                         y7 = ((inyold+3+ny)%ny)*nx;
00861                 } else {
00862                         x1 = inxold-3;
00863                         x2 = inxold-2;
00864                         x3 = inxold-1;
00865                         x4 = inxold;
00866                         x5 = inxold+1;
00867                         x6 = inxold+2;
00868                         x7 = inxold+3;
00869 
00870                         y1 = (inyold-3)*nx;
00871                         y2 = (inyold-2)*nx;
00872                         y3 = (inyold-1)*nx;
00873                         y4 = inyold*nx;
00874                         y5 = (inyold+1)*nx;
00875                         y6 = (inyold+2)*nx;
00876                         y7 = (inyold+3)*nx;
00877                 }
00878 
00879                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00880                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00881                              data[x7+y1]*tablex7 ) * tabley1 +
00882                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00883                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00884                              data[x7+y2]*tablex7 ) * tabley2 +
00885                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00886                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00887                              data[x7+y3]*tablex7 ) * tabley3 +
00888                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00889                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00890                              data[x7+y4]*tablex7 ) * tabley4 +
00891                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00892                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00893                              data[x7+y5]*tablex7 ) * tabley5 +
00894                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00895                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00896                              data[x7+y6]*tablex7 ) * tabley6 +
00897                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00898                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00899                              data[x7+y7]*tablex7 ) * tabley7;
00900 
00901                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00902                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00903         } else {  //  3D
00904                 dely = restrict1(dely, ny);
00905                 int inyold = int(Util::round(dely));
00906                 delz = restrict1(delz, nz);
00907                 int inzold = int(Util::round(delz));
00908 
00909                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00910                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00911                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00912                 float tablex4 = kb.i0win_tab(delx-inxold);
00913                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00914                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00915                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00916 
00917                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00918                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00919                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00920                 float tabley4 = kb.i0win_tab(dely-inyold);
00921                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00922                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00923                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00924 
00925                 float tablez1 = kb.i0win_tab(delz-inzold+3);
00926                 float tablez2 = kb.i0win_tab(delz-inzold+2);
00927                 float tablez3 = kb.i0win_tab(delz-inzold+1);
00928                 float tablez4 = kb.i0win_tab(delz-inzold);
00929                 float tablez5 = kb.i0win_tab(delz-inzold-1);
00930                 float tablez6 = kb.i0win_tab(delz-inzold-2);
00931                 float tablez7 = kb.i0win_tab(delz-inzold-3);
00932 
00933                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00934 
00935                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
00936                         x1 = (inxold-3+nx)%nx;
00937                         x2 = (inxold-2+nx)%nx;
00938                         x3 = (inxold-1+nx)%nx;
00939                         x4 = (inxold  +nx)%nx;
00940                         x5 = (inxold+1+nx)%nx;
00941                         x6 = (inxold+2+nx)%nx;
00942                         x7 = (inxold+3+nx)%nx;
00943 
00944                         y1 = ((inyold-3+ny)%ny)*nx;
00945                         y2 = ((inyold-2+ny)%ny)*nx;
00946                         y3 = ((inyold-1+ny)%ny)*nx;
00947                         y4 = ((inyold  +ny)%ny)*nx;
00948                         y5 = ((inyold+1+ny)%ny)*nx;
00949                         y6 = ((inyold+2+ny)%ny)*nx;
00950                         y7 = ((inyold+3+ny)%ny)*nx;
00951 
00952                         z1 = ((inzold-3+nz)%nz)*nx*ny;
00953                         z2 = ((inzold-2+nz)%nz)*nx*ny;
00954                         z3 = ((inzold-1+nz)%nz)*nx*ny;
00955                         z4 = ((inzold  +nz)%nz)*nx*ny;
00956                         z5 = ((inzold+1+nz)%nz)*nx*ny;
00957                         z6 = ((inzold+2+nz)%nz)*nx*ny;
00958                         z7 = ((inzold+3+nz)%nz)*nx*ny;
00959                 } else {
00960                         x1 = inxold-3;
00961                         x2 = inxold-2;
00962                         x3 = inxold-1;
00963                         x4 = inxold;
00964                         x5 = inxold+1;
00965                         x6 = inxold+2;
00966                         x7 = inxold+3;
00967 
00968                         y1 = (inyold-3)*nx;
00969                         y2 = (inyold-2)*nx;
00970                         y3 = (inyold-1)*nx;
00971                         y4 = inyold*nx;
00972                         y5 = (inyold+1)*nx;
00973                         y6 = (inyold+2)*nx;
00974                         y7 = (inyold+3)*nx;
00975 
00976                         z1 = (inzold-3)*nx*ny;
00977                         z2 = (inzold-2)*nx*ny;
00978                         z3 = (inzold-1)*nx*ny;
00979                         z4 = inzold*nx*ny;
00980                         z5 = (inzold+1)*nx*ny;
00981                         z6 = (inzold+2)*nx*ny;
00982                         z7 = (inzold+3)*nx*ny;
00983                 }
00984 
00985                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00986                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00987                              data[x7+y1+z1]*tablex7 ) * tabley1 +
00988                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00989                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00990                              data[x7+y2+z1]*tablex7 ) * tabley2 +
00991                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00992                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00993                              data[x7+y3+z1]*tablex7 ) * tabley3 +
00994                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00995                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00996                              data[x7+y4+z1]*tablex7 ) * tabley4 +
00997                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00998                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00999                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01000                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01001                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01002                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01003                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01004                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01005                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01006                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01007                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01008                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01009                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01010                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01011                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01012                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01013                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01014                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01015                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01016                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01017                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01018                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01019                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01020                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01021                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01022                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01023                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01024                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01025                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01026                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01027                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01028                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01029                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01030                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01031                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01032                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01033                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01034                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01035                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01036                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01037                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01038                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01039                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01040                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01041                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01042                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01043                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01044                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01045                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01046                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01047                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01048                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01049                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01050                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01051                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01052                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01053                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01054                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01055                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01056                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01057                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01058                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01059                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01060                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01061                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01062                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01063                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01064                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01065                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01066                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01067                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01068                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01069                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01070                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01071                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01072                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01073                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01074                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01075                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01076                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01077                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01078                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01079                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01080                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01081                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01082                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01083                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01084                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01085                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01086                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01087                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01088                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01089                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01090                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01091                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01092                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01093                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01094                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01095                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01096                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01097                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01098                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01099                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01100                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01101                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01102                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01103                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01104                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01105                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01106                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01107                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01108                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01109                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01110                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01111                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01112                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01113                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01114                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01115                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01116                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01117                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01118                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01119                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01120                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01121                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01122                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01123                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01124                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01125                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01126                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01127                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01128                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01129                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01130                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01131                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01132 
01133                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01134                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01135                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01136         }
01137         return pixel/w;
01138 }
01139 
01140 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) {
01141         int K = kb.get_window_size();
01142         int kbmin = -K/2;
01143         int kbmax = -kbmin;
01144         int kbc = kbmax+1;
01145 
01146         float pixel =0.0f;
01147         float w=0.0f;
01148 
01149     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01150         delx = restrict1(delx, nx);
01151         int inxold = int(round(delx));
01152         if ( ny < 2 ) {  //1D
01153                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01154                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01155                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01156                 float tablex4 = kb.i0win_tab(delx-inxold);
01157                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01158                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01159                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01160 
01161                 int x1, x2, x3, x4, x5, x6, x7;
01162 
01163                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01164                         x1 = (inxold-3+nx)%nx;
01165                         x2 = (inxold-2+nx)%nx;
01166                         x3 = (inxold-1+nx)%nx;
01167                         x4 = (inxold  +nx)%nx;
01168                         x5 = (inxold+1+nx)%nx;
01169                         x6 = (inxold+2+nx)%nx;
01170                         x7 = (inxold+3+nx)%nx;
01171                 } else {
01172                         x1 = inxold-3;
01173                         x2 = inxold-2;
01174                         x3 = inxold-1;
01175                         x4 = inxold;
01176                         x5 = inxold+1;
01177                         x6 = inxold+2;
01178                         x7 = inxold+3;
01179                 }
01180 
01181                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01182                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01183                         data[x7]*tablex7 ;
01184 
01185                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01186         } else if ( nz < 2 ) {  // 2D
01187 
01188                 delx = argdelx;
01189                 // 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
01190                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01191                 delx = (float)xnew*2.0f;
01192                 dely = (float)ynew*2.0f;
01193                 }
01194 
01195                 int inxold = int(round(delx));
01196                 int inyold = int(round(dely));
01197 
01198                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01199                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01200                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01201                 float tablex4 = kb.i0win_tab(delx-inxold);
01202                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01203                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01204                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01205 
01206                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01207                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01208                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01209                 float tabley4 = kb.i0win_tab(dely-inyold);
01210                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01211                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01212                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01213 
01214                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01215 
01216                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01217                         x1 = (inxold-3+nx)%nx;
01218                         x2 = (inxold-2+nx)%nx;
01219                         x3 = (inxold-1+nx)%nx;
01220                         x4 = (inxold  +nx)%nx;
01221                         x5 = (inxold+1+nx)%nx;
01222                         x6 = (inxold+2+nx)%nx;
01223                         x7 = (inxold+3+nx)%nx;
01224 
01225                         y1 = ((inyold-3+ny)%ny)*nx;
01226                         y2 = ((inyold-2+ny)%ny)*nx;
01227                         y3 = ((inyold-1+ny)%ny)*nx;
01228                         y4 = ((inyold  +ny)%ny)*nx;
01229                         y5 = ((inyold+1+ny)%ny)*nx;
01230                         y6 = ((inyold+2+ny)%ny)*nx;
01231                         y7 = ((inyold+3+ny)%ny)*nx;
01232                 } else {
01233                         x1 = inxold-3;
01234                         x2 = inxold-2;
01235                         x3 = inxold-1;
01236                         x4 = inxold;
01237                         x5 = inxold+1;
01238                         x6 = inxold+2;
01239                         x7 = inxold+3;
01240 
01241                         y1 = (inyold-3)*nx;
01242                         y2 = (inyold-2)*nx;
01243                         y3 = (inyold-1)*nx;
01244                         y4 = inyold*nx;
01245                         y5 = (inyold+1)*nx;
01246                         y6 = (inyold+2)*nx;
01247                         y7 = (inyold+3)*nx;
01248                 }
01249 
01250                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01251                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01252                              data[x7+y1]*tablex7 ) * tabley1 +
01253                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01254                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01255                              data[x7+y2]*tablex7 ) * tabley2 +
01256                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01257                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01258                              data[x7+y3]*tablex7 ) * tabley3 +
01259                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01260                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01261                              data[x7+y4]*tablex7 ) * tabley4 +
01262                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01263                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01264                              data[x7+y5]*tablex7 ) * tabley5 +
01265                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01266                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01267                              data[x7+y6]*tablex7 ) * tabley6 +
01268                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01269                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01270                              data[x7+y7]*tablex7 ) * tabley7;
01271 
01272                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01273                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01274         } else {  //  3D
01275                 dely = restrict1(dely, ny);
01276                 int inyold = int(Util::round(dely));
01277                 delz = restrict1(delz, nz);
01278                 int inzold = int(Util::round(delz));
01279 
01280                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01281                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01282                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01283                 float tablex4 = kb.i0win_tab(delx-inxold);
01284                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01285                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01286                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01287 
01288                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01289                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01290                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01291                 float tabley4 = kb.i0win_tab(dely-inyold);
01292                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01293                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01294                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01295 
01296                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01297                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01298                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01299                 float tablez4 = kb.i0win_tab(delz-inzold);
01300                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01301                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01302                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01303 
01304                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01305 
01306                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01307                         x1 = (inxold-3+nx)%nx;
01308                         x2 = (inxold-2+nx)%nx;
01309                         x3 = (inxold-1+nx)%nx;
01310                         x4 = (inxold  +nx)%nx;
01311                         x5 = (inxold+1+nx)%nx;
01312                         x6 = (inxold+2+nx)%nx;
01313                         x7 = (inxold+3+nx)%nx;
01314 
01315                         y1 = ((inyold-3+ny)%ny)*nx;
01316                         y2 = ((inyold-2+ny)%ny)*nx;
01317                         y3 = ((inyold-1+ny)%ny)*nx;
01318                         y4 = ((inyold  +ny)%ny)*nx;
01319                         y5 = ((inyold+1+ny)%ny)*nx;
01320                         y6 = ((inyold+2+ny)%ny)*nx;
01321                         y7 = ((inyold+3+ny)%ny)*nx;
01322 
01323                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01324                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01325                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01326                         z4 = ((inzold  +nz)%nz)*nx*ny;
01327                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01328                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01329                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01330                 } else {
01331                         x1 = inxold-3;
01332                         x2 = inxold-2;
01333                         x3 = inxold-1;
01334                         x4 = inxold;
01335                         x5 = inxold+1;
01336                         x6 = inxold+2;
01337                         x7 = inxold+3;
01338 
01339                         y1 = (inyold-3)*nx;
01340                         y2 = (inyold-2)*nx;
01341                         y3 = (inyold-1)*nx;
01342                         y4 = inyold*nx;
01343                         y5 = (inyold+1)*nx;
01344                         y6 = (inyold+2)*nx;
01345                         y7 = (inyold+3)*nx;
01346 
01347                         z1 = (inzold-3)*nx*ny;
01348                         z2 = (inzold-2)*nx*ny;
01349                         z3 = (inzold-1)*nx*ny;
01350                         z4 = inzold*nx*ny;
01351                         z5 = (inzold+1)*nx*ny;
01352                         z6 = (inzold+2)*nx*ny;
01353                         z7 = (inzold+3)*nx*ny;
01354                 }
01355 
01356                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01357                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01358                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01359                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01360                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01361                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01362                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01363                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01364                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01365                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01366                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01367                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01368                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01369                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01370                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01371                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01372                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01373                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01374                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01375                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01376                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01377                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01378                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01379                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01380                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01381                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01382                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01383                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01384                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01385                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01386                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01387                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01388                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01389                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01390                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01391                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01392                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01393                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01394                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01395                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01396                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01397                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01398                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01399                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01400                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01401                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01402                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01403                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01404                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01405                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01406                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01407                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01408                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01409                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01410                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01411                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01412                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01413                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01414                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01415                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01416                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01417                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01418                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01419                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01420                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01421                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01422                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01423                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01424                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01425                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01426                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01427                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01428                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01429                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01430                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01431                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01432                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01433                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01434                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01435                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01436                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01437                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01438                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01439                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01440                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01441                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01442                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01443                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01444                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01445                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01446                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01447                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01448                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01449                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01450                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01451                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01452                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01453                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01454                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01455                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01456                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01457                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01458                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01459                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01460                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01461                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01462                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01463                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01464                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01465                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01466                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01467                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01468                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01469                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01470                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01471                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01472                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01473                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01474                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01475                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01476                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01477                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01478                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01479                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01480                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01481                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01482                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01483                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01484                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01485                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01486                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01487                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01488                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01489                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01490                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01491                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01492                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01493                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01494                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01495                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01496                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01497                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01498                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01499                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01500                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01501                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01502                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01503 
01504                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01505                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01506                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01507         }
01508         return pixel/w;
01509 }
01510 
01511 /*
01512 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01513 
01514         int nxreal = nx - 2;
01515         if (nxreal != ny)
01516                 throw ImageDimensionException("extractpoint requires ny == nx");
01517         int nhalf = nxreal/2;
01518         int kbsize = kb.get_window_size();
01519         int kbmin = -kbsize/2;
01520         int kbmax = -kbmin;
01521         bool flip = (nuxnew < 0.f);
01522         if (flip) {
01523                 nuxnew *= -1;
01524                 nuynew *= -1;
01525         }
01526         // put (xnew,ynew) on a grid.  The indices will be wrong for
01527         // the Fourier elements in the image, but the grid sizing will
01528         // be correct.
01529         int ixn = int(Util::round(nuxnew));
01530         int iyn = int(Util::round(nuynew));
01531         // set up some temporary weighting arrays
01532         float* wy0 = new float[kbmax - kbmin + 1];
01533         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01534         float* wx0 = new float[kbmax - kbmin + 1];
01535         float* wx = wx0 - kbmin;
01536         for (int i = kbmin; i <= kbmax; i++) {
01537                         int iyp = iyn + i;
01538                         wy[i] = kb.i0win_tab(nuynew - iyp);
01539                         int ixp = ixn + i;
01540                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01541         }
01542         // restrict loops to non-zero elements
01543         int iymin = 0;
01544         for (int iy = kbmin; iy <= -1; iy++) {
01545                 if (wy[iy] != 0.f) {
01546                         iymin = iy;
01547                         break;
01548                 }
01549         }
01550         int iymax = 0;
01551         for (int iy = kbmax; iy >= 1; iy--) {
01552                 if (wy[iy] != 0.f) {
01553                         iymax = iy;
01554                         break;
01555                 }
01556         }
01557         int ixmin = 0;
01558         for (int ix = kbmin; ix <= -1; ix++) {
01559                 if (wx[ix] != 0.f) {
01560                         ixmin = ix;
01561                         break;
01562                 }
01563         }
01564         int ixmax = 0;
01565         for (int ix = kbmax; ix >= 1; ix--) {
01566                 if (wx[ix] != 0.f) {
01567                         ixmax = ix;
01568                         break;
01569                 }
01570         }
01571         float wsum = 0.0f;
01572         for (int iy = iymin; iy <= iymax; iy++)
01573                 for (int ix = ixmin; ix <= ixmax; ix++)
01574                         wsum += wx[ix]*wy[iy];
01575 
01576         complex<float> result(0.f,0.f);
01577         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01578                 // (xin,yin) not within window border from the edge
01579                 for (int iy = iymin; iy <= iymax; iy++) {
01580                         int iyp = iyn + iy;
01581                         for (int ix = ixmin; ix <= ixmax; ix++) {
01582                                 int ixp = ixn + ix;
01583                                 float w = wx[ix]*wy[iy];
01584                                 complex<float> val = fimage->cmplx(ixp,iyp);
01585                                 result += val*w;
01586                         }
01587                 }
01588         } else {
01589                 // points that "stick out"
01590                 for (int iy = iymin; iy <= iymax; iy++) {
01591                         int iyp = iyn + iy;
01592                         for (int ix = ixmin; ix <= ixmax; ix++) {
01593                                 int ixp = ixn + ix;
01594                                 bool mirror = false;
01595                                 int ixt= ixp, iyt= iyp;
01596                                 if (ixt < 0) {
01597                                         ixt = -ixt;
01598                                         iyt = -iyt;
01599                                         mirror = !mirror;
01600                                 }
01601                                 if (ixt > nhalf) {
01602                                         ixt = nxreal - ixt;
01603                                         iyt = -iyt;
01604                                         mirror = !mirror;
01605                                 }
01606                                 if (iyt > nhalf-1)  iyt -= nxreal;
01607                                 if (iyt < -nhalf)   iyt += nxreal;
01608                                 float w = wx[ix]*wy[iy];
01609                                 complex<float> val = fimage->cmplx(ixt,iyt);
01610                                 if (mirror)  result += conj(val)*w;
01611                                 else         result += val*w;
01612                         }
01613                 }
01614         }
01615         if (flip)  result = conj(result)/wsum;
01616         else result /= wsum;
01617         delete [] wx0;
01618         delete [] wy0;
01619         return result;
01620 }*/
01621 
01622 /*
01623 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01624 
01625         int nxreal = nx - 2;
01626         if (nxreal != ny)
01627                 throw ImageDimensionException("extractpoint requires ny == nx");
01628         int nhalf = nxreal/2;
01629         bool flip = false;
01630         if (nuxnew < 0.f) {
01631                 nuxnew *= -1;
01632                 nuynew *= -1;
01633                 flip = true;
01634         }
01635         if (nuynew >= nhalf-0.5)  {
01636                 nuynew -= nxreal;
01637         } else if (nuynew < -nhalf-0.5) {
01638                 nuynew += nxreal;
01639         }
01640 
01641         // put (xnew,ynew) on a grid.  The indices will be wrong for
01642         // the Fourier elements in the image, but the grid sizing will
01643         // be correct.
01644         int ixn = int(Util::round(nuxnew));
01645         int iyn = int(Util::round(nuynew));
01646 
01647         // set up some temporary weighting arrays
01648         static float wy[7];
01649         static float wx[7];
01650 
01651         float iynn = nuynew - iyn;
01652         wy[0] = kb.i0win_tab(iynn+3);
01653         wy[1] = kb.i0win_tab(iynn+2);
01654         wy[2] = kb.i0win_tab(iynn+1);
01655         wy[3] = kb.i0win_tab(iynn);
01656         wy[4] = kb.i0win_tab(iynn-1);
01657         wy[5] = kb.i0win_tab(iynn-2);
01658         wy[6] = kb.i0win_tab(iynn-3);
01659 
01660         float ixnn = nuxnew - ixn;
01661         wx[0] = kb.i0win_tab(ixnn+3);
01662         wx[1] = kb.i0win_tab(ixnn+2);
01663         wx[2] = kb.i0win_tab(ixnn+1);
01664         wx[3] = kb.i0win_tab(ixnn);
01665         wx[4] = kb.i0win_tab(ixnn-1);
01666         wx[5] = kb.i0win_tab(ixnn-2);
01667         wx[6] = kb.i0win_tab(ixnn-3);
01668 
01669         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]);
01670 
01671         complex<float> result(0.f,0.f);
01672         for (int iy = 0; iy < 7; iy++) {
01673                 int iyp = iyn + iy - 3 ;
01674                 for (int ix = 0; ix < 7; ix++) {
01675                         int ixp = ixn + ix - 3;
01676                         float w = wx[ix]*wy[iy];
01677                         complex<float> val = fimage->cmplx(ixp,iyp);
01678                         result += val*w;
01679                 }
01680         }
01681 
01682         if (flip)  result = conj(result)/wsum;
01683         else result /= wsum;
01684 
01685         return result;
01686 }*/
01687 
01688 
01689 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01690 
01691         int nxreal = nx - 2;
01692         if (nxreal != ny)
01693                 throw ImageDimensionException("extractpoint requires ny == nx");
01694         int nhalf = nxreal/2;
01695         bool flip = (nuxnew < 0.f);
01696         if (flip) {
01697                 nuxnew *= -1;
01698                 nuynew *= -1;
01699         }
01700         if (nuynew >= nhalf-0.5)  {
01701                 nuynew -= nxreal;
01702         } else if (nuynew < -nhalf-0.5) {
01703                 nuynew += nxreal;
01704         }
01705 
01706         // put (xnew,ynew) on a grid.  The indices will be wrong for
01707         // the Fourier elements in the image, but the grid sizing will
01708         // be correct.
01709         int ixn = int(Util::round(nuxnew));
01710         int iyn = int(Util::round(nuynew));
01711 
01712         // set up some temporary weighting arrays
01713         static float wy[7];
01714         static float wx[7];
01715 
01716         float iynn = nuynew - iyn;
01717         wy[0] = kb.i0win_tab(iynn+3);
01718         wy[1] = kb.i0win_tab(iynn+2);
01719         wy[2] = kb.i0win_tab(iynn+1);
01720         wy[3] = kb.i0win_tab(iynn);
01721         wy[4] = kb.i0win_tab(iynn-1);
01722         wy[5] = kb.i0win_tab(iynn-2);
01723         wy[6] = kb.i0win_tab(iynn-3);
01724 
01725         float ixnn = nuxnew - ixn;
01726         wx[0] = kb.i0win_tab(ixnn+3);
01727         wx[1] = kb.i0win_tab(ixnn+2);
01728         wx[2] = kb.i0win_tab(ixnn+1);
01729         wx[3] = kb.i0win_tab(ixnn);
01730         wx[4] = kb.i0win_tab(ixnn-1);
01731         wx[5] = kb.i0win_tab(ixnn-2);
01732         wx[6] = kb.i0win_tab(ixnn-3);
01733 
01734         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]);
01735 
01736         complex<float> result(0.f,0.f);
01737         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01738                 // (xin,yin) not within window border from the edge
01739                 for (int iy = 0; iy < 7; iy++) {
01740                         int iyp = iyn + iy - 3 ;
01741                         for (int ix = 0; ix < 7; ix++) {
01742                                 int ixp = ixn + ix - 3;
01743                                 float w = wx[ix]*wy[iy];
01744                                 complex<float> val = fimage->cmplx(ixp,iyp);
01745                                 result += val*w;
01746                         }
01747                 }
01748         } else {
01749                 // points that "stick out"
01750                 for (int iy = 0; iy < 7; iy++) {
01751                         int iyp = iyn + iy - 3;
01752                         for (int ix = 0; ix < 7; ix++) {
01753                                 int ixp = ixn + ix - 3;
01754                                 bool mirror = false;
01755                                 int ixt = ixp, iyt = iyp;
01756                                 if (ixt < 0) {
01757                                         ixt = -ixt;
01758                                         iyt = -iyt;
01759                                         mirror = !mirror;
01760                                 }
01761                                 if (ixt > nhalf) {
01762                                         ixt = nxreal - ixt;
01763                                         iyt = -iyt;
01764                                         mirror = !mirror;
01765                                 }
01766                                 if (iyt > nhalf-1)  iyt -= nxreal;
01767                                 if (iyt < -nhalf)   iyt += nxreal;
01768                                 float w = wx[ix]*wy[iy];
01769                                 complex<float> val = fimage->cmplx(ixt,iyt);
01770                                 if (mirror)  result += conj(val)*w;
01771                                 else         result += val*w;
01772                         }
01773                 }
01774         }
01775         if (flip)  result = conj(result)/wsum;
01776         else result /= wsum;
01777         return result;
01778 }
01779 
01780 /*
01781 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01782 
01783         int nxreal = nx - 2;
01784         if (nxreal != ny)
01785                 throw ImageDimensionException("extractpoint requires ny == nx");
01786         int nhalf = nxreal/2;
01787         bool flip = (nuxnew < 0.f);
01788         if (flip) {
01789                 nuxnew *= -1;
01790                 nuynew *= -1;
01791         }
01792         // put (xnew,ynew) on a grid.  The indices will be wrong for
01793         // the Fourier elements in the image, but the grid sizing will
01794         // be correct.
01795         int ixn = int(Util::round(nuxnew));
01796         int iyn = int(Util::round(nuynew));
01797         // set up some temporary weighting arrays
01798         static float wy[7];
01799         static float wx[7];
01800 
01801         float iynn = nuynew - iyn;
01802         wy[0] = kb.i0win_tab(iynn+3);
01803         wy[1] = kb.i0win_tab(iynn+2);
01804         wy[2] = kb.i0win_tab(iynn+1);
01805         wy[3] = kb.i0win_tab(iynn);
01806         wy[4] = kb.i0win_tab(iynn-1);
01807         wy[5] = kb.i0win_tab(iynn-2);
01808         wy[6] = kb.i0win_tab(iynn-3);
01809 
01810         float ixnn = nuxnew - ixn;
01811         wx[0] = kb.i0win_tab(ixnn+3);
01812         wx[1] = kb.i0win_tab(ixnn+2);
01813         wx[2] = kb.i0win_tab(ixnn+1);
01814         wx[3] = kb.i0win_tab(ixnn);
01815         wx[4] = kb.i0win_tab(ixnn-1);
01816         wx[5] = kb.i0win_tab(ixnn-2);
01817         wx[6] = kb.i0win_tab(ixnn-3);
01818 
01819         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]);
01820 
01821         complex<float> result(0.f,0.f);
01822 
01823         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01824                 // (xin,yin) not within window border from the edge
01825                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01826                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01827                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01828                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01829                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
01830                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
01831                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
01832                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
01833                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
01834                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
01835                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
01836                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
01837                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
01838                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
01839                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
01840                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
01841                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
01842                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
01843                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
01844                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
01845                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
01846                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
01847                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
01848                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
01849                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
01850                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
01851                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
01852                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
01853                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
01854                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
01855                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
01856                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
01857                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
01858                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
01859                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
01860                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
01861                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
01862                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
01863                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
01864                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
01865                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
01866                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
01867                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
01868                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
01869                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
01870                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
01871                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
01872                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
01873                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
01874 
01875         } else {
01876                 // points that "stick out"
01877                 for (int iy = 0; iy < 7; iy++) {
01878                         int iyp = iyn + iy - 3;
01879                         for (int ix = 0; ix < 7; ix++) {
01880                                 int ixp = ixn + ix - 3;
01881                                 bool mirror = false;
01882                                 int ixt= ixp, iyt= iyp;
01883                                 if (ixt < 0) {
01884                                         ixt = -ixt;
01885                                         iyt = -iyt;
01886                                         mirror = !mirror;
01887                                 }
01888                                 if (ixt > nhalf) {
01889                                         ixt = nxreal - ixt;
01890                                         iyt = -iyt;
01891                                         mirror = !mirror;
01892                                 }
01893                                 if (iyt > nhalf-1)  iyt -= nxreal;
01894                                 if (iyt < -nhalf)   iyt += nxreal;
01895                                 float w = wx[ix]*wy[iy];
01896                                 complex<float> val = fimage->cmplx(ixt,iyt);
01897                                 if (mirror)  result += conj(val)*w;
01898                                 else         result += val*w;
01899                         }
01900                 }
01901         }
01902         if (flip)  result = conj(result)/wsum;
01903         else result /= wsum;
01904         return result;
01905 }*/
01906 
01907 
01908 float Util::triquad(float R, float S, float T, float* fdata)
01909 {
01910 
01911     const float C2 = 0.5f;    //1.0 / 2.0;
01912     const float C4 = 0.25f;   //1.0 / 4.0;
01913     const float C8 = 0.125f;  //1.0 / 8.0;
01914 
01915     float  RS   = R * S;
01916     float  ST   = S * T;
01917     float  RT   = R * T;
01918     float  RST  = R * ST;
01919 
01920     float  RSQ  = 1-R*R;
01921     float  SSQ  = 1-S*S;
01922     float  TSQ  = 1-T*T;
01923 
01924     float  RM1  = (1-R);
01925     float  SM1  = (1-S);
01926     float  TM1  = (1-T);
01927 
01928     float  RP1  = (1+R);
01929     float  SP1  = (1+S);
01930     float  TP1  = (1+T);
01931 
01932     float triquad =
01933     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
01934         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
01935         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
01936         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
01937         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
01938         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
01939         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
01940         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
01941         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
01942 //
01943         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
01944         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
01945         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
01946         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
01947                       RSQ  * SSQ  * TSQ * fdata[13] +
01948         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
01949         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
01950         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
01951         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
01952  //
01953         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
01954         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
01955         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
01956         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
01957         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
01958         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
01959         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
01960         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
01961         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
01962      return triquad;
01963 }
01964 
01965 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01966                 : M(M_), fc(fc_), ntable(ntable_) {
01967         // Sinc-Blackman kernel
01968         build_sBtable();
01969 }
01970 
01971 void Util::sincBlackman::build_sBtable() {
01972         sBtable.resize(ntable+1);
01973         int ltab = int(round(float(ntable)/1.25f));
01974         int M2 = M/2;
01975         fltb = float(ltab)/M2;
01976         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01977         float x = 1.0e-7f;
01978         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)));
01979         for (int i=1; i <= ltab; i++) {
01980                 x = float(i)/fltb;
01981                 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)));
01982                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
01983         }
01984 }
01985 
01986 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01987                                          int N_, float vtable_, int ntable_)
01988                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01989                   ntable(ntable_) {
01990         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
01991         if (0.f == v) v = float(K)/2;
01992         if (0.f == vtable) vtable = v;
01993         alphar = alpha*r;
01994         fac = static_cast<float>(twopi)*alphar*v;
01995         vadjust = 1.0f*v;
01996         facadj = static_cast<float>(twopi)*alphar*vadjust;
01997         build_I0table();
01998 }
01999 
02000 float Util::KaiserBessel::i0win(float x) const {
02001         float val0 = float(gsl_sf_bessel_I0(facadj));
02002         float absx = fabs(x);
02003         if (absx > vadjust) return 0.f;
02004         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02005         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02006         return res;
02007 }
02008 
02009 void Util::KaiserBessel::build_I0table() {
02010         i0table.resize(ntable+1); // i0table[0:ntable]
02011         int ltab = int(round(float(ntable)/1.25f));
02012         fltb = float(ltab)/(K/2);
02013         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02014         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02015         for (int i=0; i <= ltab; i++) {
02016                 float s = float(i)/fltb/N;
02017                 if (s < vadjust) {
02018                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02019                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02020                 } else {
02021                         i0table[i] = 0.f;
02022                 }
02023 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02024         }
02025 }
02026 
02027 float Util::KaiserBessel::I0table_maxerror() {
02028         float maxdiff = 0.f;
02029         for (int i = 1; i <= ntable; i++) {
02030                 float diff = fabs(i0table[i] - i0table[i-1]);
02031                 if (diff > maxdiff) maxdiff = diff;
02032         }
02033         return maxdiff;
02034 }
02035 
02036 float Util::KaiserBessel::sinhwin(float x) const {
02037         float val0 = sinh(fac)/fac;
02038         float absx = fabs(x);
02039         if (0.0 == x) {
02040                 float res = 1.0f;
02041                 return res;
02042         } else if (absx == alphar) {
02043                 return 1.0f/val0;
02044         } else if (absx < alphar) {
02045                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02046                 float facrt = fac*rt;
02047                 float res = (sinh(facrt)/facrt)/val0;
02048                 return res;
02049         } else {
02050                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02051                 float facrt = fac*rt;
02052                 float res = (sin(facrt)/facrt)/val0;
02053                 return res;
02054         }
02055 }
02056 
02057 float Util::FakeKaiserBessel::i0win(float x) const {
02058         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02059         float absx = fabs(x);
02060         if (absx > vadjust) return 0.f;
02061         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02062         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02063         return res;
02064 }
02065 
02066 void Util::FakeKaiserBessel::build_I0table() {
02067         i0table.resize(ntable+1); // i0table[0:ntable]
02068         int ltab = int(round(float(ntable)/1.1f));
02069         fltb = float(ltab)/(K/2);
02070         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02071         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02072         for (int i=0; i <= ltab; i++) {
02073                 float s = float(i)/fltb/N;
02074                 if (s < vadjust) {
02075                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02076                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02077                 } else {
02078                         i0table[i] = 0.f;
02079                 }
02080         }
02081 }
02082 
02083 float Util::FakeKaiserBessel::sinhwin(float x) const {
02084         float val0 = sinh(fac)/fac;
02085         float absx = fabs(x);
02086         if (0.0 == x) {
02087                 float res = 1.0f;
02088                 return res;
02089         } else if (absx == alphar) {
02090                 return 1.0f/val0;
02091         } else if (absx < alphar) {
02092                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02093                 float facrt = fac*rt;
02094                 float res = (sinh(facrt)/facrt)/val0;
02095                 return res;
02096         } else {
02097                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02098                 float facrt = fac*rt;
02099                 float res = (sin(facrt)/facrt)/val0;
02100                 return res;
02101         }
02102 }
02103 
02104 #if 0 // 1-st order KB window
02105 float Util::FakeKaiserBessel::sinhwin(float x) const {
02106         //float val0 = sinh(fac)/fac;
02107         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02108         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02109         float absx = fabs(x);
02110         if (0.0 == x) {
02111                 //float res = 1.0f;
02112                 float res = val0;
02113                 return res;
02114         } else if (absx == alphar) {
02115                 //return 1.0f/val0;
02116                 return prefix;
02117         } else if (absx < alphar) {
02118                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02119                 //float facrt = fac*rt;
02120                 float facrt = facadj*rt;
02121                 //float res = (sinh(facrt)/facrt)/val0;
02122                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02123                 return res;
02124         } else {
02125                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02126                 //float facrt = fac*rt;
02127                 float facrt = facadj*rt;
02128                 //float res = (sin(facrt)/facrt)/val0;
02129                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02130                 return res;
02131         }
02132 }
02133 #endif // 0
02134 
02135 
02136 
02137 #define  circ(i)         circ[i-1]
02138 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02139 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02140 
02141 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02142         int nsam = image->get_xsize();
02143         int nrow = image->get_ysize();
02144         int nring = numr.size()/3;
02145         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02146         EMData* out = new EMData();
02147         out->set_size(lcirc,1,1);
02148         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02149         float *xim  = image->get_data();
02150         float *circ = out->get_data();
02151 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02152    return out;
02153 }
02154 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02155           float *circ, int lcirc, int nring, char mode)
02156 {*/
02157 /*
02158 c
02159 c  purpose:
02160 c
02161 c  resmaple to polar coordinates
02162 c
02163 */
02164         //  dimension         xim(nsam,nrow),circ(lcirc)
02165         //  integer           numr(3,nring)
02166 
02167         double dfi, dpi;
02168         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02169         float  yq, xold, yold, fi, x, y;
02170 
02171         ns2 = nsam/2+1;
02172         nr2 = nrow/2+1;
02173         dpi = 2.0*atan(1.0);
02174 
02175         for (i=1;i<=nring;i++) {
02176                 // radius of the ring
02177                 inr = numr(1,i);
02178                 yq  = static_cast<float>(inr);
02179                 l   = numr(3,i);
02180                 if (mode == 'h' || mode == 'H')  lt = l/2;
02181                 else                             lt = l/4;
02182 
02183                 nsim           = lt-1;
02184                 dfi            = dpi/(nsim+1);
02185                 kcirc          = numr(2,i);
02186                 xold           = 0.0f;
02187                 yold           = static_cast<float>(inr);
02188                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02189                 xold           = static_cast<float>(inr);
02190                 yold           = 0.0f;
02191                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192 
02193                 if (mode == 'f' || mode == 'F') {
02194                         xold              = 0.0f;
02195                         yold              = static_cast<float>(-inr);
02196                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02197                         xold              = static_cast<float>(-inr);
02198                         yold              = 0.0f;
02199                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02200                 }
02201 
02202                 for (j=1;j<=nsim;j++) {
02203                         fi               = static_cast<float>(dfi*j);
02204                         x                = sin(fi)*yq;
02205                         y                = cos(fi)*yq;
02206                         xold             = x;
02207                         yold             = y;
02208                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02209                         xold             =  y;
02210                         yold             = -x;
02211                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212 
02213                         if (mode == 'f' || mode == 'F')  {
02214                                 xold                = -x;
02215                                 yold                = -y;
02216                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02217                                 xold                = -y;
02218                                 yold                =  x;
02219                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02220                         }
02221                 }
02222         }
02223         return  out;
02224 }
02225 
02226 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02227         int nsam = image->get_xsize();
02228         int nrow = image->get_ysize();
02229         int nring = numr.size()/3;
02230         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02231         EMData* out = new EMData();
02232         out->set_size(lcirc,1,1);
02233         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02234         float *xim  = image->get_data();
02235         float *circ = out->get_data();
02236         double dpi, dfi;
02237         int    it, jt, inr, l, nsim, kcirc, lt;
02238         float  xold, yold, fi, x, y;
02239 
02240         //     cns2 and cnr2 are predefined centers
02241         //     no need to set to zero, all elements are defined
02242         dpi = 2*atan(1.0);
02243         for (it=1; it<=nring; it++) {
02244                 // radius of the ring
02245                 inr = numr(1,it);
02246 
02247                 // "F" means a full circle interpolation
02248                 // "H" means a half circle interpolation
02249 
02250                 l = numr(3,it);
02251                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02252                 else                              lt = l / 4;
02253 
02254                 nsim  = lt - 1;
02255                 dfi   = dpi / (nsim+1);
02256                 kcirc = numr(2,it);
02257                 xold  = 0.0f+cns2;
02258                 yold  = inr+cnr2;
02259 
02260                 Assert( kcirc <= lcirc );
02261                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02262 
02263                 xold  = inr+cns2;
02264                 yold  = 0.0f+cnr2;
02265                 Assert( lt+kcirc <= lcirc );
02266                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02267 
02268                 if ( mode == 'f' || mode == 'F' ) {
02269                         xold = 0.0f+cns2;
02270                         yold = -inr+cnr2;
02271                         Assert( lt+lt+kcirc <= lcirc );
02272                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02273 
02274                         xold = -inr+cns2;
02275                         yold = 0.0f+cnr2;
02276                         Assert(lt+lt+lt+kcirc <= lcirc );
02277                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02278                 }
02279 
02280                 for (jt=1; jt<=nsim; jt++) {
02281                         fi   = static_cast<float>(dfi * jt);
02282                         x    = sin(fi) * inr;
02283                         y    = cos(fi) * inr;
02284 
02285                         xold = x+cns2;
02286                         yold = y+cnr2;
02287 
02288                         Assert( jt+kcirc <= lcirc );
02289                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02290 
02291                         xold = y+cns2;
02292                         yold = -x+cnr2;
02293 
02294                         Assert( jt+lt+kcirc <= lcirc );
02295                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02296 
02297                         if ( mode == 'f' || mode == 'F' ) {
02298                                 xold = -x+cns2;
02299                                 yold = -y+cnr2;
02300 
02301                                 Assert( jt+lt+lt+kcirc <= lcirc );
02302                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02303 
02304                                 xold = -y+cns2;
02305                                 yold = x+cnr2;
02306 
02307                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02308                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02309                         }
02310                 } // end for jt
02311         } //end for it
02312         return out;
02313 }
02314 
02315 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02316 {
02317 /*
02318 c  purpose: linear interpolation
02319   Optimized for speed, circular closer removed, checking of ranges removed
02320 */
02321     float bilinear;
02322     int   ixold, iyold;
02323 
02324 /*
02325         float xdif, ydif, xrem, yrem;
02326         ixold   = (int) floor(xold);
02327         iyold   = (int) floor(yold);
02328         ydif = yold - iyold;
02329         yrem = 1.0f - ydif;
02330 
02331         //  May want to insert if?
02332 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02333 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02334 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02335         xdif = xold - ixold;
02336         xrem = 1.0f- xdif;
02337 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02338 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02339 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02340         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02341                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02342 
02343     return bilinear;
02344 }
02345 */
02346         float xdif, ydif;
02347 
02348         ixold   = (int) xold;
02349         iyold   = (int) yold;
02350         ydif = yold - iyold;
02351 
02352         //  May want to insert it?
02353 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02354 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02355 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02356         xdif = xold - ixold;
02357         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02358                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02359                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02360 
02361         return bilinear;
02362 }
02363 
02364 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02365              int  *numr, float *circ, int , int  nring, char  mode) {
02366         double dpi, dfi;
02367         int    it, jt, inr, l, nsim, kcirc, lt;
02368         float   xold, yold, fi, x, y;
02369 
02370         //     cns2 and cnr2 are predefined centers
02371         //     no need to set to zero, all elements are defined
02372 
02373         dpi = 2*atan(1.0);
02374         for (it=1; it<=nring; it++) {
02375                 // radius of the ring
02376                 inr = numr(1,it);
02377 
02378                 l = numr(3,it);
02379                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02380                 else                              lt = l / 4;
02381 
02382                 nsim  = lt - 1;
02383                 dfi   = dpi / (nsim+1);
02384                 kcirc = numr(2,it);
02385 
02386 
02387                 xold  = 0.0f+cns2;
02388                 yold  = inr+cnr2;
02389 
02390                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391 
02392                 xold  = inr+cns2;
02393                 yold  = 0.0f+cnr2;
02394                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395 
02396                 if ( mode == 'f' || mode == 'F' ) {
02397                         xold = 0.0f+cns2;
02398                         yold = -inr+cnr2;
02399                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400 
02401                         xold = -inr+cns2;
02402                         yold = 0.0f+cnr2;
02403                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02404                 }
02405 
02406                 for (jt=1; jt<=nsim; jt++) {
02407                         fi   = static_cast<float>(dfi * jt);
02408                         x    = sin(fi) * inr;
02409                         y    = cos(fi) * inr;
02410 
02411                         xold = x+cns2;
02412                         yold = y+cnr2;
02413                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414 
02415                         xold = y+cns2;
02416                         yold = -x+cnr2;
02417                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418 
02419                         if ( mode == 'f' || mode == 'F' ) {
02420                                 xold = -x+cns2;
02421                                 yold = -y+cnr2;
02422                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02423 
02424                                 xold = -y+cns2;
02425                                 yold = x+cnr2;
02426                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02427                         }
02428                 } // end for jt
02429         } //end for it
02430 }
02431 /*
02432 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02433              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02434 {
02435    double dpi, dfi;
02436    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02437    float  yq, fi, x, y;
02438 
02439    //     cns2 and cnr2 are predefined centers
02440    //     no need to set to zero, all elements are defined
02441 
02442    dpi = 2*atan(1.0);
02443    for (it=1; it<=nring; it++) {
02444       // radius of the ring
02445       inr = numr(1,it);
02446       yq  = inr;
02447 
02448       l = numr(3,it);
02449       if ( mode == 'h' || mode == 'H' ) {
02450          lt = l / 2;
02451       }
02452       else { // if ( mode == 'f' || mode == 'F' )
02453          lt = l / 4;
02454       }
02455 
02456       nsim  = lt - 1;
02457       dfi   = dpi / (nsim+1);
02458       kcirc = numr(2,it);
02459 
02460 
02461         xold = (int) (0.0+cns2);
02462         yold = (int) (inr+cnr2);
02463 
02464         circ(kcirc) = xim(xold, yold);
02465 
02466       xold = (int) (inr+cns2);
02467       yold = (int) (0.0+cnr2);
02468       circ(lt+kcirc) = xim(xold, yold);
02469 
02470       if ( mode == 'f' || mode == 'F' ) {
02471          xold  = (int) (0.0+cns2);
02472          yold = (int) (-inr+cnr2);
02473          circ(lt+lt+kcirc) = xim(xold, yold);
02474 
02475          xold  = (int) (-inr+cns2);
02476          yold = (int) (0.0+cnr2);
02477          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02478       }
02479 
02480       for (jt=1; jt<=nsim; jt++) {
02481          fi   = dfi * jt;
02482          x    = sin(fi) * yq;
02483          y    = cos(fi) * yq;
02484 
02485          xold  = (int) (x+cns2);
02486          yold = (int) (y+cnr2);
02487          circ(jt+kcirc) = xim(xold, yold);
02488 
02489          xold  = (int) (y+cns2);
02490          yold = (int) (-x+cnr2);
02491          circ(jt+lt+kcirc) = xim(xold, yold);
02492 
02493          if ( mode == 'f' || mode == 'F' ) {
02494             xold  = (int) (-x+cns2);
02495             yold = (int) (-y+cnr2);
02496             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02497 
02498             xold  = (int) (-y+cns2);
02499             yold = (int) (x+cnr2);
02500             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02501          }
02502       } // end for jt
02503    } //end for it
02504 }
02505 */
02506 //xim((int) floor(xold), (int) floor(yold))
02507 #undef  xim
02508 
02509 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02510 // input image is twice the size of the original image
02511         int nring = numr.size()/3;
02512         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02513         EMData* out = new EMData();
02514         out->set_size(lcirc,1,1);
02515         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02516         float *circ = out->get_data();
02517         float *fimage = image->get_data();
02518         int nx = image->get_xsize();
02519         int ny = image->get_ysize();
02520         int nz = image->get_zsize();
02521         double dpi, dfi;
02522         int    it, jt, inr, l, nsim, kcirc, lt;
02523         float  yq, xold, yold, fi, x, y;
02524 
02525         //     cns2 and cnr2 are predefined centers
02526         //     no need to set to zero, all elements are defined
02527 
02528         dpi = 2*atan(1.0);
02529         for (it=1;it<=nring;it++) {
02530                 // radius of the ring
02531                 inr = numr(1,it);
02532                 yq  = static_cast<float>(inr);
02533 
02534                 l = numr(3,it);
02535                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02536                 else                               lt = l / 4;
02537 
02538                 nsim  = lt - 1;
02539                 dfi   = dpi / (nsim+1);
02540                 kcirc = numr(2,it);
02541                 xold  = 0.0f;
02542                 yold  = static_cast<float>(inr);
02543                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02545 
02546                 xold  = static_cast<float>(inr);
02547                 yold  = 0.0f;
02548                 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02549 //      circ(lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02550 
02551         if ( mode == 'f' || mode == 'F' ) {
02552                 xold = 0.0f;
02553                 yold = static_cast<float>(-inr);
02554                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02555 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02556 
02557                 xold = static_cast<float>(-inr);
02558                 yold = 0.0f;
02559                 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);
02560 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02561         }
02562 
02563         for (jt=1;jt<=nsim;jt++) {
02564                 fi   = static_cast<float>(dfi * jt);
02565                 x    = sin(fi) * yq;
02566                 y    = cos(fi) * yq;
02567 
02568                 xold = x;
02569                 yold = y;
02570                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02572 
02573                 xold = y;
02574                 yold = -x;
02575                 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02576 //         circ(jt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02577 
02578         if ( mode == 'f' || mode == 'F' ) {
02579                 xold = -x;
02580                 yold = -y;
02581                 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);
02582 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02583 
02584                 xold = -y;
02585                 yold = x;
02586                 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);
02587 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02588         }
02589         } // end for jt
02590         } //end for it
02591         return  out;
02592 }
02593 
02594 /*
02595 
02596         A set of 1-D power-of-two FFTs
02597         Pawel & Chao 01/20/06
02598 
02599 fftr_q(xcmplx,nv)
02600   single precision
02601 
02602  dimension xcmplx(2,iabs(nv)/2);
02603  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02604  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02605 
02606 
02607 fftr_d(xcmplx,nv)
02608   double precision
02609 
02610  dimension xcmplx(2,iabs(nv)/2);
02611  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02612  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02613 
02614 
02615 
02616 */
02617 #define  tab1(i)      tab1[i-1]
02618 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02619 #define  br(i)        br[i-1]
02620 #define  bi(i)        bi[i-1]
02621 //-----------------------------------------
02622 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02623 {
02624         double rni,sgn,tr1,tr2,ti1,ti2;
02625         double cc,c,ss,s,t,x2,x3,x4,x5;
02626         int    b3,b4,b5,b6,b7,b56;
02627         int    n, k, l, j, i, ix0, ix1, status=0;
02628 
02629         const double tab1[] = {
02630                 9.58737990959775e-5,
02631                 1.91747597310703e-4,
02632                 3.83495187571395e-4,
02633                 7.66990318742704e-4,
02634                 1.53398018628476e-3,
02635                 3.06795676296598e-3,
02636                 6.13588464915449e-3,
02637                 1.22715382857199e-2,
02638                 2.45412285229123e-2,
02639                 4.90676743274181e-2,
02640                 9.80171403295604e-2,
02641                 1.95090322016128e-1,
02642                 3.82683432365090e-1,
02643                 7.07106781186546e-1,
02644                 1.00000000000000,
02645         };
02646 
02647         n=(int)pow(2.0f,ln);
02648 
02649         k=abs(ks);
02650         l=16-ln;
02651         b3=n*k;
02652         b6=b3;
02653         b7=k;
02654         if (ks > 0) {
02655                 sgn=1.0f;
02656         } else {
02657                 sgn=-1.0f;
02658                 rni=1.0f/(float)(n);
02659                 j=1;
02660                 for (i=1; i<=n; i++) {
02661                         br(j)=br(j)*rni;
02662                         bi(j)=bi(j)*rni;
02663                         j=j+k;
02664                 }
02665         }
02666 
02667 L12:
02668    b6=b6/2;
02669    b5=b6;
02670    b4=2*b6;
02671    b56=b5-b6;
02672 
02673 L14:
02674    tr1=br(b5+1);
02675    ti1=bi(b5+1);
02676    tr2=br(b56+1);
02677    ti2=bi(b56+1);
02678 
02679    br(b5+1)=tr2-tr1;
02680    bi(b5+1)=ti2-ti1;
02681    br(b56+1)=tr1+tr2;
02682    bi(b56+1)=ti1+ti2;
02683 
02684    b5=b5+b4;
02685    b56=b5-b6;
02686    if ( b5 <= b3 )  goto  L14;
02687    if ( b6 == b7 )  goto  L20;
02688 
02689    b4=b7;
02690    cc=2.0f*pow(tab1(l),2);
02691    c=1.0f-cc;
02692    l++;
02693    ss=sgn*tab1(l);
02694    s=ss;
02695 
02696 L16:
02697    b5=b6+b4;
02698    b4=2*b6;
02699    b56=b5-b6;
02700 
02701 L18:
02702    tr1=br(b5+1);
02703    ti1=bi(b5+1);
02704    tr2=br(b56+1);
02705    ti2=bi(b56+1);
02706    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02707    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02708    br(b56+1)=tr1+tr2;
02709    bi(b56+1)=ti1+ti2;
02710 
02711    b5=b5+b4;
02712    b56=b5-b6;
02713    if ( b5 <= b3 )  goto  L18;
02714    b4=b5-b6;
02715    b5=b4-b3;
02716    c=-c;
02717    b4=b6-b5;
02718    if ( b5 < b4 )  goto  L16;
02719    b4=b4+b7;
02720    if ( b4 >= b5 ) goto  L12;
02721 
02722    t=c-cc*c-ss*s;
02723    s=s+ss*c-cc*s;
02724    c=t;
02725    goto  L16;
02726 
02727 L20:
02728    ix0=b3/2;
02729    b3=b3-b7;
02730    b4=0;
02731    b5=0;
02732    b6=ix0;
02733    ix1=0;
02734    if (b6 == b7) goto EXIT;
02735 
02736 L22:
02737    b4=b3-b4;
02738    b5=b3-b5;
02739    x2=br(b4+1);
02740    x3=br(b5+1);
02741    x4=bi(b4+1);
02742    x5=bi(b5+1);
02743    br(b4+1)=x3;
02744    br(b5+1)=x2;
02745    bi(b4+1)=x5;
02746    bi(b5+1)=x4;
02747    if(b6 < b4)  goto  L22;
02748 
02749 L24:
02750    b4=b4+b7;
02751    b5=b6+b5;
02752    x2=br(b4+1);
02753    x3=br(b5+1);
02754    x4=bi(b4+1);
02755    x5=bi(b5+1);
02756    br(b4+1)=x3;
02757    br(b5+1)=x2;
02758    bi(b4+1)=x5;
02759    bi(b5+1)=x4;
02760    ix0=b6;
02761 
02762 L26:
02763    ix0=ix0/2;
02764    ix1=ix1-ix0;
02765    if( ix1 >= 0)  goto L26;
02766 
02767    ix0=2*ix0;
02768    b4=b4+b7;
02769    ix1=ix1+ix0;
02770    b5=ix1;
02771    if ( b5 >= b4)  goto  L22;
02772    if ( b4 < b6)   goto  L24;
02773 
02774 EXIT:
02775    status = 0;
02776 }
02777 
02778 // -----------------------------------------------------------------
02779 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02780 {
02781         //  dimension  br(1),bi(1)
02782 
02783         int b3,b4,b5,b6,b7,b56;
02784         int n, k, l, j, i, ix0, ix1;
02785         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02786         int status=0;
02787 
02788         const float tab1[] = {
02789                 9.58737990959775e-5f,
02790                 1.91747597310703e-4f,
02791                 3.83495187571395e-4f,
02792                 7.66990318742704e-4f,
02793                 1.53398018628476e-3f,
02794                 3.06795676296598e-3f,
02795                 6.13588464915449e-3f,
02796                 1.22715382857199e-2f,
02797                 2.45412285229123e-2f,
02798                 4.90676743274181e-2f,
02799                 9.80171403295604e-2f,
02800                 1.95090322016128e-1f,
02801                 3.82683432365090e-1f,
02802                 7.07106781186546e-1f,
02803                 1.00000000000000f,
02804         };
02805 
02806         n=(int)pow(2.0f,ln);
02807 
02808         k=abs(ks);
02809         l=16-ln;
02810         b3=n*k;
02811         b6=b3;
02812         b7=k;
02813         if( ks > 0 ) {
02814                 sgn=1.0f;
02815         } else {
02816                 sgn=-1.0f;
02817                 rni=1.0f/(float)n;
02818                 j=1;
02819                 for (i=1; i<=n; i++) {
02820                         br(j)=br(j)*rni;
02821                         bi(j)=bi(j)*rni;
02822                         j=j+k;
02823                 }
02824         }
02825 L12:
02826    b6=b6/2;
02827    b5=b6;
02828    b4=2*b6;
02829    b56=b5-b6;
02830 L14:
02831    tr1=br(b5+1);
02832    ti1=bi(b5+1);
02833 
02834    tr2=br(b56+1);
02835    ti2=bi(b56+1);
02836 
02837    br(b5+1)=tr2-tr1;
02838    bi(b5+1)=ti2-ti1;
02839    br(b56+1)=tr1+tr2;
02840    bi(b56+1)=ti1+ti2;
02841 
02842    b5=b5+b4;
02843    b56=b5-b6;
02844    if ( b5 <= b3 )  goto  L14;
02845    if ( b6 == b7 )  goto  L20;
02846 
02847    b4=b7;
02848    cc=2.0f*pow(tab1(l),2);
02849    c=1.0f-cc;
02850    l++;
02851    ss=sgn*tab1(l);
02852    s=ss;
02853 L16:
02854    b5=b6+b4;
02855    b4=2*b6;
02856    b56=b5-b6;
02857 L18:
02858    tr1=br(b5+1);
02859    ti1=bi(b5+1);
02860    tr2=br(b56+1);
02861    ti2=bi(b56+1);
02862    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02863    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02864    br(b56+1)=tr1+tr2;
02865    bi(b56+1)=ti1+ti2;
02866 
02867    b5=b5+b4;
02868    b56=b5-b6;
02869    if(b5 <= b3)  goto L18;
02870    b4=b5-b6;
02871    b5=b4-b3;
02872    c=-c;
02873    b4=b6-b5;
02874    if(b5 < b4)  goto  L16;
02875    b4=b4+b7;
02876    if(b4 >= b5) goto  L12;
02877 
02878    t=c-cc*c-ss*s;
02879    s=s+ss*c-cc*s;
02880    c=t;
02881    goto  L16;
02882 L20:
02883    ix0=b3/2;
02884    b3=b3-b7;
02885    b4=0;
02886    b5=0;
02887    b6=ix0;
02888    ix1=0;
02889    if ( b6 == b7) goto EXIT;
02890 L22:
02891    b4=b3-b4;
02892    b5=b3-b5;
02893    x2=br(b4+1);
02894    x3=br(b5+1);
02895    x4=bi(b4+1);
02896    x5=bi(b5+1);
02897    br(b4+1)=x3;
02898    br(b5+1)=x2;
02899    bi(b4+1)=x5;
02900    bi(b5+1)=x4;
02901    if (b6 < b4) goto  L22;
02902 L24:
02903    b4=b4+b7;
02904    b5=b6+b5;
02905    x2=br(b4+1);
02906    x3=br(b5+1);
02907    x4=bi(b4+1);
02908    x5=bi(b5+1);
02909    br(b4+1)=x3;
02910    br(b5+1)=x2;
02911    bi(b4+1)=x5;
02912    bi(b5+1)=x4;
02913    ix0=b6;
02914 L26:
02915    ix0=ix0/2;
02916    ix1=ix1-ix0;
02917    if(ix1 >= 0)  goto  L26;
02918 
02919    ix0=2*ix0;
02920    b4=b4+b7;
02921    ix1=ix1+ix0;
02922    b5=ix1;
02923    if (b5 >= b4)  goto  L22;
02924    if (b4 < b6)   goto  L24;
02925 EXIT:
02926    status = 0;
02927 }
02928 
02929 void  Util::fftr_q(float *xcmplx, int nv)
02930 {
02931    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02932 
02933         int nu, inv, nu1, n, isub, n2, i1, i2, i;
02934         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02935 
02936         const float tab1[] = {
02937                 9.58737990959775e-5f,
02938                 1.91747597310703e-4f,
02939                 3.83495187571395e-4f,
02940                 7.66990318742704e-4f,
02941                 1.53398018628476e-3f,
02942                 3.06795676296598e-3f,
02943                 6.13588464915449e-3f,
02944                 1.22715382857199e-2f,
02945                 2.45412285229123e-2f,
02946                 4.90676743274181e-2f,
02947                 9.80171403295604e-2f,
02948                 1.95090322016128e-1f,
02949                 3.82683432365090e-1f,
02950                 7.07106781186546e-1f,
02951                 1.00000000000000f,
02952         };
02953 
02954         nu=abs(nv);
02955         inv=nv/nu;
02956         nu1=nu-1;
02957         n=(int)pow(2.f,nu1);
02958         isub=16-nu1;
02959 
02960         ss=-tab1(isub);
02961         cc=-2.0f*pow(tab1(isub-1),2.f);
02962         c=1.0f;
02963         s=0.0f;
02964         n2=n/2;
02965         if ( inv > 0) {
02966                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02967                 tr=xcmplx(1,1);
02968                 ti=xcmplx(2,1);
02969                 xcmplx(1,1)=tr+ti;
02970                 xcmplx(2,1)=tr-ti;
02971                 for (i=1;i<=n2;i++) {
02972                         i1=i+1;
02973                         i2=n-i+1;
02974                         tr1=xcmplx(1,i1);
02975                         tr2=xcmplx(1,i2);
02976                         ti1=xcmplx(2,i1);
02977                         ti2=xcmplx(2,i2);
02978                         t=(cc*c-ss*s)+c;
02979                         s=(cc*s+ss*c)+s;
02980                         c=t;
02981                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02982                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02983                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02984                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02985                 }
02986         } else {
02987                 tr=xcmplx(1,1);
02988                 ti=xcmplx(2,1);
02989                 xcmplx(1,1)=0.5f*(tr+ti);
02990                 xcmplx(2,1)=0.5f*(tr-ti);
02991                 for (i=1; i<=n2; i++) {
02992                         i1=i+1;
02993                         i2=n-i+1;
02994                         tr1=xcmplx(1,i1);
02995                         tr2=xcmplx(1,i2);
02996                         ti1=xcmplx(2,i1);
02997                         ti2=xcmplx(2,i2);
02998                         t=(cc*c-ss*s)+c;
02999                         s=(cc*s+ss*c)+s;
03000                         c=t;
03001                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03002                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03003                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03004                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03005                 }
03006                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03007         }
03008 }
03009 
03010 // -------------------------------------------
03011 void  Util::fftr_d(double *xcmplx, int nv)
03012 {
03013         // double precision  x(2,1)
03014         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03015         double tr1,tr2,ti1,ti2,tr,ti;
03016         double cc,c,ss,s,t;
03017         const double tab1[] = {
03018                 9.58737990959775e-5,
03019                 1.91747597310703e-4,
03020                 3.83495187571395e-4,
03021                 7.66990318742704e-4,
03022                 1.53398018628476e-3,
03023                 3.06795676296598e-3,
03024                 6.13588464915449e-3,
03025                 1.22715382857199e-2,
03026                 2.45412285229123e-2,
03027                 4.90676743274181e-2,
03028                 9.80171403295604e-2,
03029                 1.95090322016128e-1,
03030                 3.82683432365090e-1,
03031                 7.07106781186546e-1,
03032                 1.00000000000000,
03033         };
03034 
03035         nu=abs(nv);
03036         inv=nv/nu;
03037         nu1=nu-1;
03038         n=(int)pow(2.0f,nu1);
03039         isub=16-nu1;
03040         ss=-tab1(isub);
03041         cc=-2.0*pow(tab1(isub-1),2);
03042         c=1.0f;
03043         s=0.0f;
03044         n2=n/2;
03045 
03046         if ( inv > 0 ) {
03047                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03048                 tr=xcmplx(1,1);
03049                 ti=xcmplx(2,1);
03050                 xcmplx(1,1)=tr+ti;
03051                 xcmplx(2,1)=tr-ti;
03052                 for (i=1;i<=n2;i++) {
03053                         i1=i+1;
03054                         i2=n-i+1;
03055                         tr1=xcmplx(1,i1);
03056                         tr2=xcmplx(1,i2);
03057                         ti1=xcmplx(2,i1);
03058                         ti2=xcmplx(2,i2);
03059                         t=(cc*c-ss*s)+c;
03060                         s=(cc*s+ss*c)+s;
03061                         c=t;
03062                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03063                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03064                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03065                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03066                 }
03067         } else {
03068                 tr=xcmplx(1,1);
03069                 ti=xcmplx(2,1);
03070                 xcmplx(1,1)=0.5*(tr+ti);
03071                 xcmplx(2,1)=0.5*(tr-ti);
03072                 for (i=1; i<=n2; i++) {
03073                         i1=i+1;
03074                         i2=n-i+1;
03075                         tr1=xcmplx(1,i1);
03076                         tr2=xcmplx(1,i2);
03077                         ti1=xcmplx(2,i1);
03078                         ti2=xcmplx(2,i2);
03079                         t=(cc*c-ss*s)+c;
03080                         s=(cc*s+ss*c)+s;
03081                         c=t;
03082                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03083                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03084                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03085                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03086                 }
03087                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03088         }
03089 }
03090 #undef  tab1
03091 #undef  xcmplx
03092 #undef  br
03093 #undef  bi
03094 
03095 
03096 void Util::Frngs(EMData* circp, vector<int> numr){
03097         int nring = numr.size()/3;
03098         float *circ = circp->get_data();
03099         int i, l;
03100         for (i=1; i<=nring;i++) {
03101 
03102 #ifdef _WIN32
03103                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03104 #else
03105                 l=(int)(log2(numr(3,i)));
03106 #endif  //_WIN32
03107 
03108                 fftr_q(&circ(numr(2,i)),l);
03109         }
03110 }
03111 
03112 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03113         int nring = numr.size()/3;
03114         float *circ = circp->get_data();
03115         int i, l;
03116         for (i=1; i<=nring;i++) {
03117 
03118 #ifdef _WIN32
03119                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03120 #else
03121                 l=(int)(log2(numr(3,i)));
03122 #endif  //_WIN32
03123 
03124                 fftr_q(&circ(numr(2,i)),-l);
03125         }
03126 }
03127 #undef  circ
03128 
03129 #define  b(i)            b[i-1]
03130 void Util::prb1d(double *b, int npoint, float *pos) {
03131         double  c2,c3;
03132         int     nhalf;
03133 
03134         nhalf = npoint/2 + 1;
03135         *pos  = 0.0;
03136 
03137         if (npoint == 7) {
03138                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03139                      - 6.*b(6) + 31.*b(7);
03140                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03141         }
03142         else if (npoint == 5) {
03143                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03144                    + 46.*b(5) ) / (-70.);
03145                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03146         }
03147         else if (npoint == 3) {
03148                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03149                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03150         }
03151         //else if (npoint == 9) {
03152         else  { // at least one has to be true!!
03153                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03154                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03155                      + 1092.*b(9) ) / (-4620.);
03156                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03157                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03158         }
03159         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03160 }
03161 #undef  b
03162 
03163 #define  circ1(i)        circ1[i-1]
03164 #define  circ2(i)        circ2[i-1]
03165 #define  t(i)            t[i-1]
03166 #define  q(i)            q[i-1]
03167 #define  b(i)            b[i-1]
03168 #define  t7(i)           t7[i-1]
03169 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03170         //  neg = 0 straight,  neg = 1 mirrored
03171         int nring = numr.size()/3;
03172         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03173         int maxrin = numr[numr.size()-1];
03174         double qn;   float  tot;
03175         float *circ1 = circ1p->get_data();
03176         float *circ2 = circ2p->get_data();
03177 /*
03178 c checks single position, neg is flag for checking mirrored position
03179 c
03180 c  input - fourier transforms of rings!
03181 c  first set is conjugated (mirrored) if neg
03182 c  circ1 already multiplied by weights!
03183 c       automatic arrays
03184         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03185         double precision  q(maxrin)
03186         double precision  t7(-3:3)
03187 */
03188         float *t;
03189         double t7[7], *q;
03190         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03191         float  pos;
03192 
03193 #ifdef _WIN32
03194         ip = -(int)(log((float)maxrin)/log(2.0f));
03195 #else
03196         ip = -(int) (log2(maxrin));
03197 #endif  //_WIN32
03198 
03199         q = (double*)calloc(maxrin, sizeof(double));
03200         t = (float*)calloc(maxrin, sizeof(float));
03201 
03202 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03203         for (i=1; i<=nring; i++) {
03204                 numr3i = numr(3,i);
03205                 numr2i = numr(2,i);
03206 
03207                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03208 
03209                 if (numr3i != maxrin) {
03210                          // test .ne. first for speed on some compilers
03211                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03212                         t(2)            = 0.0;
03213 
03214                         if (neg) {
03215                                 // first set is conjugated (mirrored)
03216                                 for (j=3;j<=numr3i;j=j+2) {
03217                                         jc = j+numr2i-1;
03218                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03219                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03220                                 }
03221                         } else {
03222                                 for (j=3;j<=numr3i;j=j+2) {
03223                                         jc = j+numr2i-1;
03224                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03225                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03226                                 }
03227                         }
03228                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03229                 } else {
03230                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03231                         if (neg) {
03232                                 // first set is conjugated (mirrored)
03233                                 for (j=3;j<=maxrin;j=j+2) {
03234                                         jc = j+numr2i-1;
03235                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03236                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03237                                 }
03238                         } else {
03239                                 for (j=3;j<=maxrin;j=j+2) {
03240                                         jc = j+numr2i-1;
03241                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03242                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03243                                 }
03244                         }
03245                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03246                 }
03247         }
03248 
03249         fftr_d(q,ip);
03250 
03251         qn = -1.0e20;
03252         for (j=1;j<=maxrin;j++) {
03253            if (q(j) >= qn) {
03254                   qn = q(j); jtot = j;
03255            }
03256         }
03257 
03258         for (k=-3; k<=3; k++) {
03259                 j = (jtot+k+maxrin-1)%maxrin + 1;
03260                 t7(k+4) = q(j);
03261         }
03262 
03263         prb1d(t7,7,&pos);
03264 
03265         tot = (float)jtot + pos;
03266 
03267         if (q) free(q);
03268         if (t) free(t);
03269 
03270         Dict retvals;
03271         retvals["qn"] = qn;
03272         retvals["tot"] = tot;
03273         return  retvals;
03274 }
03275 
03276 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03277    //  neg = 0 straight,  neg = 1 mirrored
03278         int nring = numr.size()/3;
03279         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03280         int maxrin = numr[numr.size()-1];
03281         double qn;   float  tot;
03282         float *circ1 = circ1p->get_data();
03283         float *circ2 = circ2p->get_data();
03284 /*
03285 c checks single position, neg is flag for checking mirrored position
03286 c
03287 c  input - fourier transforms of rings!
03288 c  first set is conjugated (mirrored) if neg
03289 c  multiplication by weights!
03290 c       automatic arrays
03291         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03292         double precision  q(maxrin)
03293         double precision  t7(-3:3)
03294 */
03295         float *t;
03296         double t7[7], *q;
03297         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03298         float  pos;
03299 
03300 #ifdef _WIN32
03301         ip = -(int)(log((float)maxrin)/log(2.0f));
03302 #else
03303         ip = -(int) (log2(maxrin));
03304 #endif  //_WIN32
03305 
03306         q = (double*)calloc(maxrin, sizeof(double));
03307         t = (float*)calloc(maxrin, sizeof(float));
03308 
03309 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03310         for (i=1;i<=nring;i++) {
03311                 numr3i = numr(3,i);
03312                 numr2i = numr(2,i);
03313 
03314                 t(1) = circ1(numr2i) * circ2(numr2i);
03315 
03316                 if (numr3i != maxrin) {
03317                         // test .ne. first for speed on some compilers
03318                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03319                         t(2)      = 0.0;
03320 
03321                         if (neg) {
03322                                 // first set is conjugated (mirrored)
03323                                 for (j=3; j<=numr3i; j=j+2) {
03324                                         jc = j+numr2i-1;
03325                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03326                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03327                                 }
03328                         } else {
03329                                 for (j=3; j<=numr3i; j=j+2) {
03330                                         jc = j+numr2i-1;
03331                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03332                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03333                                 }
03334                         }
03335                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03336                 } else {
03337                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03338                         if (neg) {
03339                                 // first set is conjugated (mirrored)
03340                                 for (j=3; j<=maxrin; j=j+2) {
03341                                         jc = j+numr2i-1;
03342                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03343                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03344                                 }
03345                         } else {
03346                                 for (j=3; j<=maxrin; j=j+2) {
03347                                 jc = j+numr2i-1;
03348                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03349                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03350                                 }
03351                         }
03352                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03353                 }
03354         }
03355 
03356         fftr_d(q,ip);
03357 
03358         qn = -1.0e20;
03359         for (j=1;j<=maxrin;j++) {
03360                 //cout << j << "  " << q(j) << endl;
03361                 if (q(j) >= qn) {
03362                         qn = q(j);
03363                         jtot = j;
03364                 }
03365         }
03366 
03367         for (k=-3; k<=3; k++) {
03368                 j = (jtot+k+maxrin-1)%maxrin + 1;
03369                 t7(k+4) = q(j);
03370         }
03371 
03372         prb1d(t7,7,&pos);
03373 
03374         tot = (float)jtot + pos;
03375 
03376         //if (q) free(q);
03377         if (t) free(t);
03378 
03379         Dict retvals;
03380         //tot = 1;
03381         //qn = q(1);
03382         retvals["qn"] = qn;
03383         retvals["tot"] = tot;
03384 
03385         if (q) free(q);
03386 
03387         return  retvals;
03388 }
03389 
03390 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03391         int nring = numr.size()/3;
03392         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03393         int maxrin = numr[numr.size()-1];
03394         double qn; float tot; double qm; float tmt;
03395         float *circ1 = circ1p->get_data();
03396         float *circ2 = circ2p->get_data();
03397 /*
03398 c
03399 c  checks both straight & mirrored positions
03400 c
03401 c  input - fourier transforms of rings!!
03402 c  circ1 already multiplied by weights!
03403 c
03404 */
03405 
03406         // dimension             circ1(lcirc),circ2(lcirc)
03407 
03408         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03409         double *t, *q, t7[7];
03410 
03411         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03412         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03413 
03414         qn  = 0.0f;
03415         qm  = 0.0f;
03416         tot = 0.0f;
03417         tmt = 0.0f;
03418 #ifdef _WIN32
03419         ip = -(int)(log((float)maxrin)/log(2.0f));
03420 #else
03421         ip = -(int)(log2(maxrin));
03422 #endif  //_WIN32
03423   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03424 
03425         //  c - straight  = circ1 * conjg(circ2)
03426         //  zero q array
03427 
03428         q = (double*)calloc(maxrin,sizeof(double));
03429 
03430         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03431         //   zero t array
03432         t = (double*)calloc(maxrin,sizeof(double));
03433 
03434    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03435         for (i=1; i<=nring; i++) {
03436 
03437                 numr3i = numr(3,i);   // Number of samples of this ring
03438                 numr2i = numr(2,i);   // The beginning point of this ring
03439 
03440                 t1   = circ1(numr2i) * circ2(numr2i);
03441                 q(1) += t1;
03442                 t(1) += t1;
03443 
03444                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03445                 if (numr3i == maxrin)  {
03446                         q(2) += t1;
03447                         t(2) += t1;
03448                 } else {
03449                         q(numr3i+1) += t1;
03450                         t(numr3i+1) += t1;
03451                 }
03452 
03453                 for (j=3; j<=numr3i; j += 2) {
03454                         jc     = j+numr2i-1;
03455 
03456 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03457 //                                ----- -----    ----- -----
03458 //                                 t1     t2      t3    t4
03459 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03460 //                                    ----- -----    ----- -----
03461 //                                     t1    t2       t3    t4
03462 
03463                         c1     = circ1(jc);
03464                         c2     = circ1(jc+1);
03465                         d1     = circ2(jc);
03466                         d2     = circ2(jc+1);
03467 
03468                         t1     = c1 * d1;
03469                         t2     = c2 * d2;
03470                         t3     = c1 * d2;
03471                         t4     = c2 * d1;
03472 
03473                         q(j)   += t1 + t2;
03474                         q(j+1) += -t3 + t4;
03475                         t(j)   += t1 - t2;
03476                         t(j+1) += -t3 - t4;
03477                 }
03478         }
03479         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03480         fftr_d(q,ip);
03481 
03482         qn  = -1.0e20;
03483         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03484                 if (q(j) >= qn) {
03485                         qn  = q(j);
03486                         jtot = j;
03487                 }
03488         }
03489 
03490         for (k=-3; k<=3; k++) {
03491                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03492                 t7(k+4) = q(j);
03493         }
03494 
03495         // interpolate
03496         prb1d(t7,7,&pos);
03497         tot = (float)(jtot)+pos;
03498         // Do not interpolate
03499         //tot = (float)(jtot);
03500 
03501         // mirrored
03502         fftr_d(t,ip);
03503 
03504         // find angle
03505         qm = -1.0e20;
03506         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03507                 if ( t(j) >= qm ) {
03508                         qm   = t(j);
03509                         jtot = j;
03510                 }
03511         }
03512 
03513         for (k=-3; k<=3; k++)  {
03514                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03515                 t7(k+4) = t(j);
03516         }
03517 
03518         // interpolate
03519 
03520         prb1d(t7,7,&pos);
03521         tmt = float(jtot) + pos;
03522         // Do not interpolate
03523         //tmt = float(jtot);
03524 
03525         free(t);
03526         free(q);
03527 
03528         Dict retvals;
03529         retvals["qn"] = qn;
03530         retvals["tot"] = tot;
03531         retvals["qm"] = qm;
03532         retvals["tmt"] = tmt;
03533         return retvals;
03534 }
03535 
03536 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03537         int nring = numr.size()/3;
03538         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03539         int maxrin = numr[numr.size()-1];
03540         double qn; float tot; double qm; float tmt;
03541         float *circ1 = circ1p->get_data();
03542         float *circ2 = circ2p->get_data();
03543 /*
03544 c
03545 c  checks both straight & mirrored positions
03546 c
03547 c  input - fourier transforms of rings!!
03548 c  circ1 already multiplied by weights!
03549 c
03550 */
03551 
03552         // dimension             circ1(lcirc),circ2(lcirc)
03553 
03554         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03555         double *t, *q;
03556 
03557         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03558         float t1, t2, t3, t4, c1, c2, d1, d2;
03559 
03560         qn  = 0.0f;
03561         qm  = 0.0f;
03562         tot = 0.0f;
03563         tmt = 0.0f;
03564 #ifdef _WIN32
03565         ip = -(int)(log((float)maxrin)/log(2.0f));
03566 #else
03567         ip = -(int)(log2(maxrin));
03568 #endif  //_WIN32
03569   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03570 
03571         //  c - straight  = circ1 * conjg(circ2)
03572         //  zero q array
03573 
03574         q = (double*)calloc(maxrin,sizeof(double));
03575 
03576         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03577         //   zero t array
03578         t = (double*)calloc(maxrin,sizeof(double));
03579 
03580    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03581         for (i=1; i<=nring; i++) {
03582 
03583                 numr3i = numr(3,i);   // Number of samples of this ring
03584                 numr2i = numr(2,i);   // The beginning point of this ring
03585 
03586                 t1   = circ1(numr2i) * circ2(numr2i);
03587                 q(1) += t1;
03588                 t(1) += t1;
03589 
03590                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03591                 if (numr3i == maxrin)  {
03592                         q(2) += t1;
03593                         t(2) += t1;
03594                 } else {
03595                         q(numr3i+1) += t1;
03596                         t(numr3i+1) += t1;
03597                 }
03598 
03599                 for (j=3; j<=numr3i; j += 2) {
03600                         jc     = j+numr2i-1;
03601 
03602 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03603 //                                ----- -----    ----- -----
03604 //                                 t1     t2      t3    t4
03605 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03606 //                                    ----- -----    ----- -----
03607 //                                     t1    t2       t3    t4
03608 
03609                         c1     = circ1(jc);
03610                         c2     = circ1(jc+1);
03611                         d1     = circ2(jc);
03612                         d2     = circ2(jc+1);
03613 
03614                         t1     = c1 * d1;
03615                         t2     = c2 * d2;
03616                         t3     = c1 * d2;
03617                         t4     = c2 * d1;
03618 
03619                         q(j)   += t1 + t2;
03620                         q(j+1) += -t3 + t4;
03621                         t(j)   += t1 - t2;
03622                         t(j+1) += -t3 - t4;
03623                 }
03624         }
03625         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03626         fftr_d(q,ip);
03627 
03628         qn  = -1.0e20;
03629 
03630         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03631         int jstep = static_cast<int>(delta/360.0*maxrin);
03632         if (jstep < 1) { jstep = 1; }
03633 
03634         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03635                 if (q(j) >= qn) {
03636                         qn  = q(j);
03637                         jtot = j;
03638                 }
03639         }
03640 
03641         //for (k=-3; k<=3; k++) {
03642         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03643         //      t7(k+4) = q(j);
03644         //}
03645 
03646         // interpolate
03647         //prb1d(t7,7,&pos);
03648         //tot = (float)(jtot)+pos;
03649         // Do not interpolate
03650         tot = (float)(jtot);
03651 
03652         // mirrored
03653         fftr_d(t,ip);
03654 
03655         // find angle
03656         qm = -1.0e20;
03657         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03658                 if ( t(j) >= qm ) {
03659                         qm   = t(j);
03660                         jtot = j;
03661                 }
03662         }
03663 
03664         //for (k=-3; k<=3; k++)  {
03665         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03666         //      t7(k+4) = t(j);
03667         //}
03668 
03669         // interpolate
03670 
03671         //prb1d(t7,7,&pos);
03672         //tmt = float(jtot) + pos;
03673         // Do not interpolate
03674         tmt = float(jtot);
03675 
03676         free(t);
03677         free(q);
03678 
03679         Dict retvals;
03680         retvals["qn"] = qn;
03681         retvals["tot"] = tot;
03682         retvals["qm"] = qm;
03683         retvals["tmt"] = tmt;
03684         return retvals;
03685 }
03686 
03687 
03688 Dict Util::Crosrng_psi_0_180_no_mirror(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03689         int nring = numr.size()/3;
03690         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03691         int maxrin = numr[numr.size()-1];
03692         double qn; float tot;
03693         float *circ1 = circ1p->get_data();
03694         float *circ2 = circ2p->get_data();
03695 
03696         // dimension             circ1(lcirc),circ2(lcirc)
03697 
03698         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03699         double  *q, t7[7];
03700 
03701         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03702         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03703 
03704         qn  = 0.0f;
03705         tot = 0.0f;
03706 #ifdef _WIN32
03707         ip = -(int)(log((float)maxrin)/log(2.0f));
03708 #else
03709         ip = -(int)(log2(maxrin));
03710 #endif  //_WIN32
03711   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03712 
03713         //  c - straight  = circ1 * conjg(circ2)
03714         //  zero q array
03715 
03716         q = (double*)calloc(maxrin,sizeof(double));
03717 
03718    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03719         for (i=1; i<=nring; i++) {
03720 
03721                 numr3i = numr(3,i);   // Number of samples of this ring
03722                 numr2i = numr(2,i);   // The beginning point of this ring
03723 
03724                 t1   = circ1(numr2i) * circ2(numr2i);
03725                 q(1) += t1;
03726                 
03727 
03728                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03729                 if (numr3i == maxrin)  {
03730                         q(2) += t1;
03731                         
03732                 } else {
03733                         q(numr3i+1) += t1;
03734                 }
03735 
03736                 for (j=3; j<=numr3i; j += 2) {
03737                         jc     = j+numr2i-1;
03738 
03739 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03740 //                                ----- -----    ----- -----
03741 //                                 t1     t2      t3    t4
03742 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03743 //                                    ----- -----    ----- -----
03744 //                                     t1    t2       t3    t4
03745 
03746                         c1     = circ1(jc);
03747                         c2     = circ1(jc+1);
03748                         d1     = circ2(jc);
03749                         d2     = circ2(jc+1);
03750 
03751                         t1     = c1 * d1;
03752                         t2     = c2 * d2;
03753                         t3     = c1 * d2;
03754                         t4     = c2 * d1;
03755 
03756                         q(j)   += t1 + t2;
03757                         q(j+1) += -t3 + t4;
03758                 
03759                 }
03760         }
03761         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03762         fftr_d(q,ip);
03763 
03764         int psi_range  = int(psi_max/360.0*maxrin+0.5);
03765         const int psi_0 = 0;
03766         int psi_180    = int(  180.0/360.0*maxrin+0.5);
03767 
03768         qn  = -1.0e20;
03769         for (k=-psi_range; k<=psi_range; k++) {
03770                 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;
03771                 if (q(j) >= qn) {
03772                         qn  = q(j);
03773                         jtot = j;
03774                 }
03775         }
03776 
03777         for (k=-psi_range; k<=psi_range; k++) {
03778                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03779                 if (q(j) >= qn) {
03780                         qn  = q(j);
03781                         jtot = j;
03782                 }
03783         }
03784 
03785         for (k=-3; k<=3; k++) {
03786                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03787                 t7(k+4) = q(j);
03788         }
03789 
03790         // interpolate
03791         prb1d(t7,7,&pos);
03792         tot = (float)(jtot)+pos;
03793         // Do not interpolate
03794         //tot = (float)(jtot);
03795 
03796         free(q);
03797 
03798         Dict retvals;
03799         retvals["qn"] = qn;
03800         retvals["tot"] = tot;
03801         
03802         return retvals;
03803 }
03804 
03805 
03806 
03807 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
03808 // flag 0 - straight, 1 - mirror
03809 
03810         int nring = numr.size()/3;
03811         int maxrin = numr[numr.size()-1];
03812         double qn; float tot;
03813         float *circ1 = circ1p->get_data();
03814         float *circ2 = circ2p->get_data();
03815 
03816         double *q;
03817 
03818         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03819         float t1, t2, t3, t4, c1, c2, d1, d2;
03820 
03821         qn  = 0.0f;
03822         tot = 0.0f;
03823 #ifdef _WIN32
03824         ip = -(int)(log((float)maxrin)/log(2.0f));
03825 #else
03826         ip = -(int)(log2(maxrin));
03827 #endif  //_WIN32
03828 
03829         //  c - straight  = circ1 * conjg(circ2)
03830         //  zero q array
03831 
03832         q = (double*)calloc(maxrin,sizeof(double));
03833         int neg = 1-2*flag;
03834    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03835         
03836         for (i=1; i<=nring; i++) {
03837 
03838                 numr3i = numr(3,i);   // Number of samples of this ring
03839                 numr2i = numr(2,i);   // The beginning point of this ring
03840 
03841                 t1   = circ1(numr2i) * circ2(numr2i);
03842                 q(1) += t1;
03843 
03844                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03845                 if (numr3i == maxrin)  {
03846                         q(2) += t1;
03847                 } else {
03848                         q(numr3i+1) += t1;
03849                 }
03850 
03851                 for (j=3; j<=numr3i; j += 2) {
03852                         jc     = j+numr2i-1;
03853 
03854         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03855         //                                ----- -----    ----- -----
03856         //                                 t1     t2      t3    t4
03857 
03858                         c1     = circ1(jc);
03859                         c2     = circ1(jc+1);
03860                         d1     = circ2(jc);
03861                         d2     = circ2(jc+1);
03862 
03863                         t1     = c1 * d1;
03864                         t3     = c1 * d2;
03865                         t2     = c2 * d2;
03866                         t4     = c2 * d1;
03867 
03868                         q(j)   += t1 + t2*neg;
03869                         q(j+1) += -t3 + t4*neg;
03870                 }
03871         }
03872          
03873         
03874         fftr_d(q,ip);
03875 
03876         qn  = -1.0e20;
03877         int psi_pos = int(psi/360.0*maxrin+0.5);
03878         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03879         
03880         for (k=-psi_range; k<=psi_range; k++) {
03881                 j = (k+psi_pos+maxrin-1)%maxrin+1;
03882                 if (q(j) >= qn) {
03883                         qn  = q(j);
03884                         jtot = j;
03885                 }
03886         }
03887 
03888         tot = (float)(jtot);
03889         free(q);
03890 
03891         Dict retvals;
03892         retvals["qn"] = qn;
03893         retvals["tot"] = tot;
03894         return retvals;
03895 }
03896 
03897 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
03898 // Computes both straight and mirrored
03899 
03900         int nring = numr.size()/3;
03901         int maxrin = numr[numr.size()-1];
03902         double qn; float tot; double qm; float tmt;
03903         float *circ1 = circ1p->get_data();
03904         float *circ2 = circ2p->get_data();
03905 
03906         double *t, *q;
03907 
03908         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03909         float t1, t2, t3, t4, c1, c2, d1, d2;
03910 
03911         qn  = 0.0f;
03912         qm  = 0.0f;
03913         tot = 0.0f;
03914         tmt = 0.0f;
03915 #ifdef _WIN32
03916         ip = -(int)(log((float)maxrin)/log(2.0f));
03917 #else
03918         ip = -(int)(log2(maxrin));
03919 #endif  //_WIN32
03920 
03921         //  c - straight  = circ1 * conjg(circ2)
03922         //  zero q array
03923 
03924         q = (double*)calloc(maxrin,sizeof(double));
03925         
03926         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03927         //   zero t array
03928         t = (double*)calloc(maxrin,sizeof(double));
03929         
03930    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03931         
03932         for (i=1; i<=nring; i++) {
03933 
03934                 numr3i = numr(3,i);   // Number of samples of this ring
03935                 numr2i = numr(2,i);   // The beginning point of this ring
03936 
03937                 t1   = circ1(numr2i) * circ2(numr2i);
03938                 q(1) += t1;
03939                 t(1) += t1;
03940                 
03941                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03942                 if (numr3i == maxrin)  {
03943                         q(2) += t1;
03944                         t(2) += t1;
03945                 } else {
03946                         q(numr3i+1) += t1;
03947                         t(numr3i+1) += t1;
03948                 }
03949 
03950                 for (j=3; j<=numr3i; j += 2) {
03951                         jc     = j+numr2i-1;
03952 
03953         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03954         //                                ----- -----    ----- -----
03955         //                                 t1     t2      t3    t4
03956 
03957                         c1     = circ1(jc);
03958                         c2     = circ1(jc+1);
03959                         d1     = circ2(jc);
03960                         d2     = circ2(jc+1);
03961 
03962                         t1     = c1 * d1;
03963                         t3     = c1 * d2;
03964                         t2     = c2 * d2;
03965                         t4     = c2 * d1;
03966 
03967                         q(j)   += t1 + t2;
03968                         q(j+1) += -t3 + t4;
03969                         t(j)   += t1 - t2;
03970                         t(j+1) += -t3 - t4;
03971                 }
03972         }
03973          
03974         
03975         fftr_d(q,ip);
03976 
03977         qn  = -1.0e20;
03978         int psi_pos = int(psi/360.0*maxrin+0.5);
03979         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03980         
03981         for (k=-psi_range; k<=psi_range; k++) {
03982                 j = (k+psi_pos+maxrin-1)%maxrin+1;
03983                 if (q(j) >= qn) {
03984                         qn  = q(j);
03985                         jtot = j;
03986                 }
03987         }
03988 
03989         tot = (float)(jtot);
03990         free(q);
03991 
03992     // mirrored
03993         fftr_d(t,ip);
03994         
03995         qm  = -1.0e20;
03996         
03997         for (k=-psi_range; k<=psi_range; k++) {
03998                 j = (k+psi_pos+maxrin-1)%maxrin+1;
03999                 if (t(j) >= qm) {
04000                         qm  = t(j);
04001                         jtot = j;
04002                 }
04003         }
04004 
04005         tmt = (float)(jtot);
04006         free(t);
04007          
04008         Dict retvals;
04009         retvals["qn"] = qn;
04010         retvals["tot"] = tot;
04011         retvals["qm"] = qm;
04012         retvals["tmt"] = tmt;
04013         return retvals;
04014 }
04015 
04016 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04017         int nring = numr.size()/3;
04018         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04019         int maxrin = numr[numr.size()-1];
04020         double qn; float tot;
04021         float *circ1 = circ1p->get_data();
04022         float *circ2 = circ2p->get_data();
04023 /*
04024 c
04025 c  checks only straight position
04026 c
04027 c  input - fourier transforms of rings!!
04028 c  circ1 already multiplied by weights!
04029 c
04030 */
04031 
04032         // dimension             circ1(lcirc),circ2(lcirc)
04033 
04034         // q(maxrin), t7(-3:3)  //maxrin+2 removed
04035         double *q, t7[7];
04036 
04037         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04038         float c1, c2, d1, d2, pos;
04039 
04040         qn  = 0.0;
04041         tot = 0.0;
04042 #ifdef _WIN32
04043         ip = -(int)(log((float)maxrin)/log(2.0f));
04044 #else
04045    ip = -(int)(log2(maxrin));
04046 #endif  //_WIN32
04047         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04048 
04049         //  c - straight  = circ1 * conjg(circ2)
04050         //  zero q array
04051 
04052         q = (double*)calloc(maxrin,sizeof(double));
04053 
04054                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04055         for (i=1; i<=nring; i++) {
04056 
04057                 numr3i = numr(3,i);   // Number of samples of this ring
04058                 numr2i = numr(2,i);   // The beginning point of this ring
04059 
04060                 q(1) += circ1(numr2i) * circ2(numr2i);
04061 
04062                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04063                 else  q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04064 
04065                 for (j=3; j<=numr3i; j += 2) {
04066                         jc     = j+numr2i-1;
04067 
04068 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04069 //                                ----- -----    ----- -----
04070 //                                 t1     t2      t3    t4
04071 
04072                         c1     = circ1(jc);
04073                         c2     = circ1(jc+1);
04074                         d1     = circ2(jc);
04075                         d2     = circ2(jc+1);
04076 
04077                         q(j)   += c1 * d1 + c2 * d2;
04078                         q(j+1) += -c1 * d2 + c2 * d1;
04079                 }
04080         }
04081 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04082         fftr_d(q,ip);
04083 
04084         qn  = -1.0e20;
04085         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04086                 if (q(j) >= qn) {
04087                         qn  = q(j);
04088                         jtot = j;
04089                 }
04090         }
04091 
04092         for (k=-3; k<=3; k++)  {
04093                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04094                 t7(k+4) = q(j);
04095         }
04096 
04097         // interpolate
04098         prb1d(t7,7,&pos);
04099         tot = (float)(jtot)+pos;
04100         // Do not interpolate
04101         //*tot = (float)(jtot);
04102 
04103         free(q);
04104 
04105         Dict retvals;
04106         retvals["qn"] = qn;
04107         retvals["tot"] = tot;
04108         return retvals;
04109 }
04110 
04111 #define  dout(i,j)        dout[i+maxrin*j]
04112 #define  circ1b(i)        circ1b[i-1]
04113 #define  circ2b(i)        circ2b[i-1]
04114 
04115 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04116 
04117    // dimension         circ1(lcirc),circ2(lcirc)
04118 
04119         int   ip, jc, numr3i, numr2i, i, j;
04120         float t1, t2, t3, t4, c1, c2, d1, d2;
04121 
04122         int nring = numr.size()/3;
04123         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04124         int maxrin = numr[numr.size()-1];
04125 
04126         float* circ1b = circ1->get_data();
04127         float* circ2b = circ2->get_data();
04128 
04129         // t(maxrin), q(maxrin)  // removed +2
04130         double *t, *q;
04131 
04132         q = (double*)calloc(maxrin,sizeof(double));
04133         t = (double*)calloc(maxrin,sizeof(double));
04134 
04135 #ifdef _WIN32
04136         ip = -(int)(log((float)maxrin)/log(2.0f));
04137 #else
04138         ip = -(int)(log2(maxrin));
04139 #endif  //_WIN32
04140 
04141         //  q - straight  = circ1 * conjg(circ2)
04142 
04143         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04144 
04145         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04146 
04147         for (i=1; i<=nring; i++) {
04148 
04149                 numr3i = numr(3,i);
04150                 numr2i = numr(2,i);
04151 
04152                 t1   = circ1b(numr2i) * circ2b(numr2i);
04153                 q(1) = q(1)+t1;
04154                 t(1) = t(1)+t1;
04155 
04156                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04157                 if (numr3i == maxrin)  {
04158                         q(2) += t1;
04159                         t(2) += t1;
04160                 } else {
04161                         q(numr3i+1) += t1;
04162                         t(numr3i+1) += t1;
04163                 }
04164 
04165                 for (j=3; j<=numr3i; j=j+2) {
04166                         jc     = j+numr2i-1;
04167 
04168                         c1     = circ1b(jc);
04169                         c2     = circ1b(jc+1);
04170                         d1     = circ2b(jc);
04171                         d2     = circ2b(jc+1);
04172 
04173                         t1     = c1 * d1;
04174                         t3     = c1 * d2;
04175                         t2     = c2 * d2;
04176                         t4     = c2 * d1;
04177 
04178                         q(j)   += t1 + t2;
04179                         q(j+1) += - t3 + t4;
04180                         t(j)   += t1 - t2;
04181                         t(j+1) += - t3 - t4;
04182                 }
04183         }
04184 
04185         // straight
04186         fftr_d(q,ip);
04187 
04188         // mirrored
04189         fftr_d(t,ip);
04190 
04191         EMData* out = new EMData();
04192         out->set_size(maxrin,2,1);
04193         float *dout = out->get_data();
04194         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04195         //out->set_size(maxrin,1,1);
04196         //float *dout = out->get_data();
04197         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04198         free(t);
04199         free(q);
04200         return out;
04201 }
04202 
04203 
04204 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04205 
04206         int maxrin = numr[numr.size()-1];
04207 
04208         vector<float> r(2*maxrin);
04209 
04210         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04211 
04212         return r;
04213 }
04214 
04215 #define  dout(i,j)        dout[i+maxrin*j]
04216 #define  circ1b(i)        circ1b[i-1]
04217 #define  circ2b(i)        circ2b[i-1]
04218 
04219 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04220 
04221    // dimension         circ1(lcirc),circ2(lcirc)
04222 
04223         int   ip, jc, numr3i, numr2i, i, j;
04224         float t1, t2, t3, t4, c1, c2, d1, d2;
04225 
04226         int nring = numr.size()/3;
04227         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04228         int maxrin = numr[numr.size()-1];
04229 
04230         float* circ1b = circ1->get_data();
04231         float* circ2b = circ2->get_data();
04232 
04233 #ifdef _WIN32
04234         ip = -(int)(log((float)maxrin)/log(2.0f));
04235 #else
04236         ip = -(int)(log2(maxrin));
04237 #endif  //_WIN32
04238         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04239 
04240         //  q - straight  = circ1 * conjg(circ2)
04241 
04242         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04243 
04244         for (i=1; i<=nring; i++) {
04245 
04246                 numr3i = numr(3,i);
04247                 numr2i = numr(2,i);
04248 
04249                 t1   = circ1b(numr2i) * circ2b(numr2i);
04250                 q(1) += t1;
04251                 t(1) += t1;
04252 
04253                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04254                 if (numr3i == maxrin)  {
04255                         q(2) += t1;
04256                         t(2) += t1;
04257                 } else {
04258                         q(numr3i+1) += t1;
04259                         t(numr3i+1) += t1;
04260                 }
04261 
04262                 for (j=3; j<=numr3i; j=j+2) {
04263                         jc     = j+numr2i-1;
04264 
04265                         c1     = circ1b(jc);
04266                         c2     = circ1b(jc+1);
04267                         d1     = circ2b(jc);
04268                         d2     = circ2b(jc+1);
04269 
04270                         t1     = c1 * d1;
04271                         t3     = c1 * d2;
04272                         t2     = c2 * d2;
04273                         t4     = c2 * d1;
04274 
04275                         q(j)   += t1 + t2;
04276                         q(j+1) += -t3 + t4;
04277                         t(j)   += t1 - t2;
04278                         t(j+1) += -t3 - t4;
04279                 }
04280         }
04281         // straight
04282         fftr_q(q,ip);
04283         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04284 
04285         // mirrored
04286         fftr_q(t,ip);
04287 }
04288 
04289 
04290 
04291 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04292 {
04293 
04294         int   ip, jc, numr3i, numr2i, i, j;
04295         float t1, t2, t3, t4, c1, c2, d1, d2;
04296 
04297         int nring = numr.size()/3;
04298         int maxrin = numr[numr.size()-1];
04299 
04300         float* circ1b = circ1->get_data();
04301         float* circ2b = circ2->get_data();
04302 
04303         double *q;
04304 
04305         q = (double*)calloc(maxrin,sizeof(double));
04306 
04307 #ifdef _WIN32
04308         ip = -(int)(log((float)maxrin)/log(2.0f));
04309 #else
04310         ip = -(int)(log2(maxrin));
04311 #endif  //_WIN32
04312 
04313          //  q - straight  = circ1 * conjg(circ2)
04314 
04315         for (i=1;i<=nring;i++) {
04316 
04317                 numr3i = numr(3,i);
04318                 numr2i = numr(2,i);
04319 
04320                 t1   = circ1b(numr2i) * circ2b(numr2i);
04321                 q(1) = q(1)+t1;
04322 
04323                 if (numr3i == maxrin)  {
04324                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04325                         q(2) = q(2)+t1;
04326                 } else {
04327                         t1              = circ1b(numr2i+1) * circ2b(numr2i+1);
04328                         q(numr3i+1) = q(numr3i+1)+t1;
04329                 }
04330 
04331                 for (j=3;j<=numr3i;j=j+2) {
04332                         jc     = j+numr2i-1;
04333 
04334                         c1     = circ1b(jc);
04335                         c2     = circ1b(jc+1);
04336                         d1     = circ2b(jc);
04337                         d2     = circ2b(jc+1);
04338 
04339                         t1     = c1 * d1;
04340                         t3     = c1 * d2;
04341                         t2     = c2 * d2;
04342                         t4     = c2 * d1;
04343 
04344                         q(j)   = q(j)   + t1 + t2;
04345                         q(j+1) = q(j+1) - t3 + t4;
04346                 }
04347         }
04348 
04349         // straight
04350         fftr_d(q,ip);
04351 
04352         EMData* out = new EMData();
04353         out->set_size(maxrin,1,1);
04354         float *dout = out->get_data();
04355         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04356         free(q);
04357         return out;
04358 
04359 }
04360 
04361 
04362 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04363 {
04364 
04365         int   ip, jc, numr3i, numr2i, i, j;
04366         float t1, t2, t3, t4, c1, c2, d1, d2;
04367 
04368         int nring = numr.size()/3;
04369         int maxrin = numr[numr.size()-1];
04370 
04371         float* circ1b = circ1->get_data();
04372         float* circ2b = circ2->get_data();
04373 
04374         double *t;
04375 
04376         t = (double*)calloc(maxrin,sizeof(double));
04377 
04378 #ifdef _WIN32
04379         ip = -(int)(log((float)maxrin)/log(2.0f));
04380 #else
04381         ip = -(int)(log2(maxrin));
04382 #endif  //_WIN32
04383 
04384          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04385 
04386         for (i=1;i<=nring;i++) {
04387 
04388                 numr3i = numr(3,i);
04389                 numr2i = numr(2,i);
04390 
04391                 t1   = circ1b(numr2i) * circ2b(numr2i);
04392                 t(1) = t(1)+t1;
04393 
04394                 if (numr3i == maxrin)  {
04395                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04396                         t(2) = t(2)+t1;
04397                 }
04398 
04399                 for (j=3;j<=numr3i;j=j+2) {
04400                         jc     = j+numr2i-1;
04401 
04402                         c1     = circ1b(jc);
04403                         c2     = circ1b(jc+1);
04404                         d1     = circ2b(jc);
04405                         d2     = circ2b(jc+1);
04406 
04407                         t1     = c1 * d1;
04408                         t3     = c1 * d2;
04409                         t2     = c2 * d2;
04410                         t4     = c2 * d1;
04411 
04412                         t(j)   = t(j)   + t1 - t2;
04413                         t(j+1) = t(j+1) - t3 - t4;
04414                 }
04415         }
04416 
04417         // mirrored
04418         fftr_d(t,ip);
04419 
04420         EMData* out = new EMData();
04421         out->set_size(maxrin,1,1);
04422         float *dout = out->get_data();
04423         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04424         free(t);
04425         return out;
04426 
04427 }
04428 
04429 #undef circ1b
04430 #undef circ2b
04431 #undef dout
04432 
04433 #undef  circ1
04434 #undef  circ2
04435 #undef  t
04436 #undef  q
04437 #undef  b
04438 #undef  t7
04439 
04440 
04441 #define    QUADPI                   3.141592653589793238462643383279502884197
04442 #define    PI2                      2*QUADPI
04443 
04444 float Util::ener(EMData* ave, vector<int> numr) {
04445         ENTERFUNC;
04446         long double ener,en;
04447 
04448         int nring = numr.size()/3;
04449         float *aveptr = ave->get_data();
04450 
04451         ener = 0.0;
04452         for (int i=1; i<=nring; i++) {
04453                 int numr3i = numr(3,i);
04454                 int np     = numr(2,i)-1;
04455                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04456                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04457                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04458                 ener += en/numr3i;
04459         }
04460         EXITFUNC;
04461         return static_cast<float>(ener);
04462 }
04463 
04464 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04465         ENTERFUNC;
04466         long double ener, en;
04467         float arg, cs, si;
04468 
04469         int nima = data.size();
04470         int nring = numr.size()/3;
04471         int maxrin = numr(3,nring);
04472 
04473         ener = 0.0;
04474         for (int i=1; i<=nring; i++) {
04475                 int numr3i = numr(3,i);
04476                 int np     = numr(2,i)-1;
04477                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04478                 float temp1 = 0.0, temp2 = 0.0;
04479                 for (int kk=0; kk<nima; kk++) {
04480                         float *ptr = data[kk]->get_data();
04481                         temp1 += ptr[np];
04482                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04483                 }
04484                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04485                 for (int j=2; j<numr3i; j+=2) {
04486                         float tempr = 0.0, tempi = 0.0;
04487                         for (int kk=0; kk<nima; kk++) {
04488                                 float *ptr = data[kk]->get_data();
04489                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04490                                 cs = cos(arg);
04491                                 si = sin(arg);
04492                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04493                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04494                         }
04495                         en += tq*(tempr*tempr+tempi*tempi);
04496                 }
04497                 ener += en/numr3i;
04498         }
04499         EXITFUNC;
04500         return static_cast<float>(ener);
04501 }
04502 
04503 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04504         int nring = numr.size()/3;
04505         float *ave = avep->get_data();
04506         float *dat = datp->get_data();
04507         int i, j, numr3i, np;
04508         float  arg, cs, si;
04509         int maxrin = numr(3,nring);
04510         if(mirror == 1) { //for mirrored data has to be conjugated
04511                 for (i=1; i<=nring; i++) {
04512                         numr3i = numr(3,i);
04513                         np     = numr(2,i)-1;
04514                         ave[np]   += dat[np];
04515                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04516                         for (j=2; j<numr3i; j=j+2) {
04517                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04518                                 cs = cos(arg);
04519                                 si = sin(arg);
04520                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04521                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04522                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04523                         }
04524                 }
04525         } else {
04526                 for (i=1; i<=nring; i++) {
04527                         numr3i = numr(3,i);
04528                         np     = numr(2,i)-1;
04529                         ave[np]   += dat[np];
04530                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04531                         for (j=2; j<numr3i; j=j+2) {
04532                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04533                                 cs = cos(arg);
04534                                 si = sin(arg);
04535                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04536                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04537                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04538                         }
04539                 }
04540         }
04541         avep->update();
04542         EXITFUNC;
04543 }
04544 
04545 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04546         int nring = numr.size()/3;
04547         float *ave = avep->get_data();
04548         float *dat = datp->get_data();
04549         int i, j, numr3i, np;
04550         float  arg, cs, si;
04551         int maxrin = numr(3,nring);
04552         if(mirror == 1) { //for mirrored data has to be conjugated
04553                 for (i=1; i<=nring; i++) {
04554                         numr3i = numr(3,i);
04555                         np     = numr(2,i)-1;
04556                         ave[np]   -= dat[np];
04557                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04558                         for (j=2; j<numr3i; j=j+2) {
04559                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04560                                 cs = cos(arg);
04561                                 si = sin(arg);
04562                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04563                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04564                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04565                         }
04566                 }
04567         } else {
04568                 for (i=1; i<=nring; i++) {
04569                         numr3i = numr(3,i);
04570                         np     = numr(2,i)-1;
04571                         ave[np]   -= dat[np];
04572                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04573                         for (j=2; j<numr3i; j=j+2) {
04574                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04575                                 cs = cos(arg);
04576                                 si = sin(arg);
04577                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04578                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04579                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04580                         }
04581                 }
04582         }
04583         avep->update();
04584         EXITFUNC;
04585 }
04586 
04587 
04588 #undef    QUADPI
04589 #undef    PI2
04590 
04591 #undef  numr
04592 #undef  circ
04593 
04594 
04595 #define QUADPI   3.141592653589793238462643383279502884197
04596 #define PI2      QUADPI*2
04597 #define deg_rad  QUADPI/180.0
04598 #define rad_deg  180.0/QUADPI
04599 
04600 struct ori_t
04601 {
04602     int iphi;
04603     int itht;
04604     int id;
04605 };
04606 
04607 
04608 struct cmpang
04609 {
04610     bool operator()( const ori_t& a, const ori_t& b )
04611     {
04612         if( a.itht != b.itht )
04613         {
04614             return a.itht < b.itht;
04615         }
04616 
04617         return a.iphi < b.iphi;
04618     }
04619 };
04620 
04621 
04622 vector<double> Util::cml_weights(const vector<float>& cml){
04623         static const int NBIN = 100;
04624         int nline=cml.size()/2;
04625         vector<double> weights(nline);
04626 
04627         vector<ori_t> angs(nline);
04628         for( int i=0; i < nline; ++i ) {
04629                 angs[i].iphi = int( NBIN*cml[2*i] );
04630                 angs[i].itht = int( NBIN*cml[2*i+1] );
04631                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04632                 angs[i].id = i;
04633         }
04634 
04635         //std::cout << "# of angs: " << angs.size() << std::endl;
04636 
04637         std::sort( angs.begin(), angs.end(), cmpang() );
04638 
04639         vector<float> newphi;
04640         vector<float> newtht;
04641         vector< vector<int> > indices;
04642 
04643         int curt_iphi = -1;
04644         int curt_itht = -1;
04645         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04646                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04647                         Assert( indices.size() > 0 );
04648                         indices.back().push_back(angs[i].id);
04649                 } else {
04650                         curt_iphi = angs[i].iphi;
04651                         curt_itht = angs[i].itht;
04652 
04653                         newphi.push_back( float(curt_iphi)/NBIN );
04654                         newtht.push_back( float(curt_itht)/NBIN );
04655                         indices.push_back( vector<int>(1,angs[i].id) );
04656                 }
04657         }
04658 
04659         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04660 
04661 
04662         int num_agl = newphi.size();
04663 
04664         if(num_agl>2) {
04665                 vector<double> w=Util::vrdg(newphi, newtht);
04666 
04667                 Assert( w.size()==newphi.size() );
04668                 Assert( indices.size()==newphi.size() );
04669 
04670                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04671                     /*
04672                     std::cout << "phi,tht,w,n: ";
04673                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04674                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04675                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04676                     std::cout << indices[i].size() << "(";
04677                     */
04678 
04679                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04680                             int id = indices[i][j];
04681                             weights[id] = w[i]/indices[i].size();
04682                             //std::cout << id << " ";
04683                     }
04684 
04685                     //std::cout << ")" << std::endl;
04686 
04687                 }
04688         } else {
04689                 cout<<"warning in Util.cml_weights"<<endl;
04690                 double val = PI2/float(nline);
04691                 for(int i=0; i<nline; i++)  weights[i]=val;
04692         }
04693 
04694         return weights;
04695 
04696 }
04697 
04698 /****************************************************
04699  * New code for common-lines
04700  ****************************************************/
04701 
04702 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04703 {
04704         int i;
04705         int nx=img->get_xsize();
04706         float *img_ptr  = img->get_data();
04707         float *line_ptr = line->get_data();
04708         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04709         img->update();
04710 }
04711 
04712 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04713         int j;
04714         int nx = sino->get_xsize();
04715         int i = nx * pos_line;
04716         float r1, r2;
04717         float *line_ptr = line->get_data();
04718         float *sino_ptr = sino->get_data();
04719         for (j=ilf;j<=ihf; j += 2) {
04720                 r1 = line_ptr[j];
04721                 r2 = line_ptr[j + 1];
04722                 sino_ptr[i + j - ilf] = r1;
04723                 sino_ptr[i + j - ilf + 1] = r2;
04724                 sino_ptr[i + nx * nblines + j - ilf] = r1;
04725                 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04726         }
04727         sino->update();
04728 }
04729 
04730 vector<double> Util::cml_init_rot(vector<float> Ori){
04731         int nb_ori = Ori.size() / 4;
04732         int i, ind;
04733         float ph, th, ps;
04734         double cph, cth, cps, sph, sth, sps;
04735         vector<double> Rot(nb_ori*9);
04736         for (i=0; i<nb_ori; ++i){
04737                 ind = i*4;
04738                 // spider convention phi=psi-90, psi=phi+90
04739                 ph = Ori[ind+2]-90;
04740                 th = Ori[ind+1];
04741                 ps = Ori[ind]+90;
04742                 ph *= deg_rad;
04743                 th *= deg_rad;
04744                 ps *= deg_rad;
04745                 // pre-calculate some trigo stuffs
04746                 cph = cos(ph);
04747                 cth = cos(th);
04748                 cps = cos(ps);
04749                 sph = sin(ph);
04750                 sth = sin(th);
04751                 sps = sin(ps);
04752                 // fill rotation matrix
04753                 ind = i*9;
04754                 Rot[ind] = cph*cps-cth*sps*sph;
04755                 Rot[ind+1] = cph*sps+cth*cps*sph;
04756                 Rot[ind+2] = sth*sph;
04757                 Rot[ind+3] = -sph*cps-cth*sps*cph;
04758                 Rot[ind+4] = -sph*sps+cth*cps*cph;
04759                 Rot[ind+5] = sth*cph;
04760                 Rot[ind+6] = sth*sps;
04761                 Rot[ind+7] = -sth*cps;
04762                 Rot[ind+8] = cth;
04763         }
04764 
04765         return Rot;
04766 }
04767 
04768 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04769         float ph, ps;
04770         double cph, cth, cps, sph, sth, sps;
04771         int ind = iprj*9;
04772         // spider convention phi=psi-90, psi=phi+90
04773         ph = nps-90;
04774         ps = nph+90;
04775         ph *= deg_rad;
04776         th *= deg_rad;
04777         ps *= deg_rad;
04778         // pre-calculate some trigo stuffs
04779         cph = cos(ph);
04780         cth = cos(th);
04781         cps = cos(ps);
04782         sph = sin(ph);
04783         sth = sin(th);
04784         sps = sin(ps);
04785         // fill rotation matrix
04786         Rot[ind] = (float)(cph*cps-cth*sps*sph);
04787         Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04788         Rot[ind+2] = (float)(sth*sph);
04789         Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04790         Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04791         Rot[ind+5] = (float)(sth*cph);
04792         Rot[ind+6] = (float)(sth*sps);
04793         Rot[ind+7] = (float)(-sth*cps);
04794         Rot[ind+8] = (float)(cth);
04795 
04796         return Rot;
04797 }
04798 
04799 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04800         vector<int> com(2*(n_prj - 1));
04801         int a = i_prj*9;
04802         int i, b, c;
04803         int n1=0, n2=0;
04804         float vmax = 1 - 1.0e-6f;
04805         double r11, r12, r13, r23, r31, r32, r33;
04806 
04807         c = 0;
04808         for (i=0; i<n_prj; ++i){
04809                 if (i!=i_prj){
04810                         b = i*9;
04811                         // this is equivalent to R = A*B'
04812                         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04813                         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04814                         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04815                         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04816                         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04817                         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04818                         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04819                         if (r33 > vmax) {
04820                             n2 = 270;
04821                             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04822                         }
04823                         else if (r33 < -vmax) {
04824                             n2 = 270;
04825                             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04826                         } else {
04827                             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04828                             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04829                             if (n1 < 0) {n1 += 360;}
04830                             if (n2 <= 0) {n2 = abs(n2);}
04831                             else {n2 = 360 - n2;}
04832                         }
04833 
04834                         if (n1 >= 360){n1 = n1 % 360;}
04835                         if (n2 >= 360){n2 = n2 % 360;}
04836 
04837                         // store common-lines
04838                         b = c*2;
04839                         com[b] = n1;
04840                         com[b+1] = n2;
04841                         ++c;
04842                 }
04843         }
04844 
04845     return com;
04846 
04847 }
04848 
04849 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04850         vector<int> com(2*n_lines);
04851         int a=0, b, c, l;
04852         int n1=0, n2=0, mem=-1;
04853         float vmax = 1 - 1.0e-6f;
04854         double r11, r12, r13, r23, r31, r32, r33;
04855         c = 0;
04856         for (l=0; l<n_lines; ++l){
04857                 c = 2*l;
04858                 if (seq[c]!=mem){
04859                     mem = seq[c];
04860                     a = seq[c]*9;
04861                 }
04862                 b = seq[c+1]*9;
04863 
04864                 // this is equivalent to R = A*B'
04865                 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04866                 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04867                 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04868                 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04869                 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04870                 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04871                 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04872                 if (r33 > vmax) {
04873                     n2 = 270;
04874                     n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04875                 }
04876                 else if (r33 < -vmax) {
04877                     n2 = 270;
04878                     n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04879                 } else {
04880                     n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04881                     n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04882                     if (n1 < 0) {n1 += 360;}
04883                     if (n2 <= 0) {n2 = abs(n2);}
04884                     else {n2 = 360 - n2;}
04885                 }
04886                 if (n1 >= 360){n1 = n1 % 360;}
04887                 if (n2 >= 360){n2 = n2 % 360;}
04888 
04889                 // store common-lines
04890                 com[c] = n1;
04891                 com[c+1] = n2;
04892         }
04893 
04894         return com;
04895 
04896 }
04897 
04898 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04899         // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04900         vector<double> cml(2*nlines); // [phi, theta] / line
04901         float ph1, th1;
04902         float ph2, th2;
04903         double nx, ny, nz;
04904         double norm;
04905         double sth1=0, sph1=0, cth1=0, cph1=0;
04906         double sth2, sph2, cth2, cph2;
04907         int l, ind, c;
04908         int mem = -1;
04909         for (l=0; l<nlines; ++l){
04910                 c = 2*l;
04911                 if (seq[c]!=mem){
04912                         mem = seq[c];
04913                         ind = 4*seq[c];
04914                         ph1 = Ori[ind]*deg_rad;
04915                         th1 = Ori[ind+1]*deg_rad;
04916                         sth1 = sin(th1);
04917                         sph1 = sin(ph1);
04918                         cth1 = cos(th1);
04919                         cph1 = cos(ph1);
04920                 }
04921                 ind = 4*seq[c+1];
04922                 ph2 = Ori[ind]*deg_rad;
04923                 th2 = Ori[ind+1]*deg_rad;
04924                 sth2 = sin(th2);
04925                 cth2 = cos(th2);
04926                 sph2 = sin(ph2);
04927                 cph2 = cos(ph2);
04928                 // cross product
04929                 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04930                 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04931                 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04932                 norm = sqrt(nx*nx+ny*ny+nz*nz);
04933                 nx /= norm;
04934                 ny /= norm;
04935                 nz /= norm;
04936                 // apply mirror if need
04937                 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04938                 // compute theta and phi
04939                 cml[c+1] = acos(nz);
04940                 if (cml[c+1] == 0) {cml[c] = 0;}
04941                 else {
04942                         cml[c+1] *= rad_deg;
04943                         if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04944                         cml[c] = rad_deg * atan2(nx, ny);
04945                         cml[c] = fmod(360 + cml[c], 360);
04946 
04947                 }
04948         }
04949 
04950         return cml;
04951 }
04952 
04953 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04954         double res = 0;
04955         double buf = 0;
04956         float* line_1;
04957         float* line_2;
04958         int i, n, ind;
04959         int lnlen = data[0]->get_xsize();
04960         for (n=0; n<n_lines; ++n) {
04961                 ind = n*2;
04962                 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04963                 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04964                 buf = 0;
04965                 for (i=0; i<lnlen; ++i) {
04966                     buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04967                 }
04968                 res += buf * weights[n];
04969         }
04970 
04971         return res;
04972 
04973 }
04974 
04975 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04976                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04977         // res: [best_disc, best_ipsi]
04978         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04979         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04980         vector<double> res(2);
04981         int lnlen = data[0]->get_xsize();
04982         int end = 2*(n_prj-1);
04983         double disc, buf, bdisc, tmp;
04984         int n, i, ipsi, ind, bipsi, c;
04985         float* line_1;
04986         float* line_2;
04987         bdisc = 1.0e6;
04988         bipsi = -1;
04989         // loop psi
04990         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04991                 // discrepancy
04992                 disc = 0;
04993                 c = 0;
04994                 for (n=0; n<n_prj; ++n) {
04995                         if(n!=iprj) {
04996                                 ind = 2*c;
04997                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04998                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04999                                 buf = 0;
05000                                 for (i=0; i<lnlen; ++i) {
05001                                         tmp = line_1[i]-line_2[i];
05002                                         buf += tmp*tmp;
05003                                 }
05004                                 disc += buf * weights[iw[c]];
05005                                 ++c;
05006                         }
05007                 }
05008                 // select the best value
05009                 if (disc <= bdisc) {
05010                         bdisc = disc;
05011                         bipsi = ipsi;
05012                 }
05013                 // update common-lines
05014                 for (i=0; i<end; i+=2){
05015                         com[i] += d_psi;
05016                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05017                 }
05018         }
05019         res[0] = bdisc;
05020         res[1] = float(bipsi);
05021 
05022         return res;
05023 }
05024 
05025 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
05026                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05027         // res: [best_disc, best_ipsi]
05028         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
05029         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
05030         vector<double> res(2);
05031         int lnlen = data[0]->get_xsize();
05032         int end = 2*(n_prj-1);
05033         double disc, buf, bdisc, tmp;
05034         int n, i, ipsi, ind, bipsi, c;
05035         float* line_1;
05036         float* line_2;
05037         bdisc = 1.0e6;
05038         bipsi = -1;
05039         // loop psi
05040         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05041                 // discrepancy
05042                 disc = 0;
05043                 c = 0;
05044                 for (n=0; n<n_prj; ++n) {
05045                         if(n!=iprj) {
05046                                 ind = 2*c;
05047                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05048                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05049                                 buf = 0;
05050                                 for (i=0; i<lnlen; ++i) {
05051                                         tmp = line_1[i]-line_2[i];
05052                                         buf += tmp*tmp;
05053                                 }
05054                                 disc += buf;
05055                                 ++c;
05056                         }
05057                 }
05058                 // select the best value
05059                 if (disc <= bdisc) {
05060                         bdisc = disc;
05061                         bipsi = ipsi;
05062                 }
05063                 // update common-lines
05064                 for (i=0; i<end; i+=2){
05065                         com[i] += d_psi;
05066                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05067                 }
05068         }
05069         res[0] = bdisc;
05070         res[1] = float(bipsi);
05071 
05072         return res;
05073 }
05074 
05075 #undef  QUADPI
05076 #undef  PI2
05077 #undef  deg_rad
05078 #undef  rad_deg
05079 
05080 /****************************************************
05081  * END OF NEW CODE FOR COMMON-LINES
05082  ****************************************************/
05083 
05084 // helper function for k-means
05085 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05086         ENTERFUNC;
05087 
05088         int nima = data.size();
05089         vector<float> res(nima);
05090         double result = 0.;
05091         double valmin = 1.0e20;
05092         int valpos = -1;
05093 
05094         for (int kk=0; kk<nima; kk++){
05095         result = 0;
05096 
05097         float *y_data = data[kk]->get_data();
05098         float *x_data = image->get_data();
05099         long totsize = image->get_xsize()*image->get_ysize();
05100         for (long i = 0; i < totsize; i++) {
05101             double temp = x_data[i]- y_data[i];
05102             result += temp*temp;
05103         }
05104         result /= totsize;
05105         res[kk] = (float)result;
05106 
05107         if(result<valmin) {valmin = result; valpos = kk;}
05108 
05109         }
05110 
05111         Dict retvals;
05112         retvals["dist"] = res;
05113         retvals["pos"]  = valpos;
05114 
05115         EXITFUNC;
05116         return retvals;
05117 
05118 }
05119 
05120 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05121         ENTERFUNC;
05122 
05123         int nima = data.size();
05124         vector<float> res(nima);
05125         double result = 0.;
05126         double valmin = 1.0e20;
05127         int valpos = -1;
05128 
05129         for (int kk=0; kk<nima; kk++){
05130         result = 0;
05131         //validate_input_args(image, data[kk]);
05132 
05133         float *y_data = data[kk]->get_data();
05134         float *x_data = image->get_data();
05135 
05136         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05137         int nx  = data[kk]->get_xsize();
05138         int ny  = data[kk]->get_ysize();
05139         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05140         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05141 
05142         int ixb = 2*((nx+1)%2);
05143         int iyb = ny%2;
05144         int iz = 0;
05145 
05146         for ( int iy = 0; iy <= ny-1; iy++) {
05147             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05148                 int ii = ix + (iy  + iz * ny)* lsd2;
05149                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05150             }
05151         }
05152         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05153             int ii = (iy  + iz * ny)* lsd2;
05154             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05155             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05156         }
05157         if(nx%2 == 0) {
05158             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05159                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05160                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05161                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05162             }
05163 
05164         }
05165         result *= 2;
05166         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05167         if(ny%2 == 0) {
05168             int ii = (ny/2  + iz * ny)* lsd2;
05169             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05170         }
05171         if(nx%2 == 0) {
05172             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05173             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05174             if(ny%2 == 0) {
05175                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05176                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05177             }
05178         }
05179 
05180         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05181         res[kk] = (float)result;
05182 
05183         if(result<valmin) {valmin = result; valpos = kk;}
05184 
05185         }
05186 
05187         Dict retvals;
05188         retvals["dist"] = res;
05189         retvals["pos"]  = valpos;
05190 
05191         EXITFUNC;
05192         return retvals;
05193 }
05194 
05195 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05196     long int d2 = group2[s2 - 1] - group2[0];
05197     long int p2 = 0;
05198     long int i1 = 0;
05199     long int i2 = 0;
05200     long int max = 0;
05201     long int cont = 0;
05202     long int i = 0;
05203     int stop1 = 0;
05204     int stop2 = 0;
05205 
05206     for (i=0; i<s1; i++) {
05207         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05208         if (p2 >= s2) {p2 = s2 - 1;}
05209         i1 = p2;
05210         i2 = p2;
05211         max = s2;
05212         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05213 
05214         stop1 = 0;
05215         stop2 = 0;
05216         while (max--) {
05217             if (group1[i] == group2[i1]) {
05218                 if (flag) {stb[cont] = group1[i];}
05219                 cont++;
05220                 break;
05221             }
05222             if (group2[i1] < group1[i]) {stop1=1;}
05223             if (group1[i] == group2[i2]) {
05224                 if (flag) {stb[cont] = group1[i];}
05225                 cont++;
05226                 break;
05227             }
05228             if (group2[i2] > group1[i]) {stop2=1;}
05229             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05230 
05231             if (stop1 & stop2) {break;}
05232             i1--;
05233             i2++;
05234             if (i1 < 0) {i1 = 0;}
05235             if (i2 >= s2) {i2 = s2 - 1;}
05236         }
05237         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05238     }
05239 
05240     return cont;
05241 }
05242 
05243 
05244 
05245 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*(size_t)nx]
05246 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05247 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05248 {
05249         /* Exception Handle */
05250         if (!img) {
05251                 throw NullPointerException("NULL input image");
05252         }
05253         /* ============================== */
05254 
05255         // Get the size of the input image
05256         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05257         /* ============================== */
05258 
05259 
05260         /* Exception Handle */
05261         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)
05262         {
05263                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05264                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05265         }
05266         /* ============================== */
05267 
05268 
05269         /*    Calculation of the start point */
05270         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05271         /* ============================*/
05272 
05273 
05274         /* Calculation of the size of the decimated image */
05275         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05276         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05277         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05278         if(r1>1){r1=1;}
05279         if(r2>1){r2=1;}
05280         if(r3>1){r3=1;}
05281         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05282         /* ===========================================*/
05283 
05284 
05285         EMData* img2 = new EMData();
05286         img2->set_size(new_nx,new_ny,new_nz);
05287         float *new_ptr = img2->get_data();
05288         float *old_ptr = img->get_data();
05289         int iptr, jptr, kptr = 0;
05290         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05291                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05292                         for (int i=new_st_x; i<nx; i+=x_step) {
05293                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05294                         iptr++;}
05295                 jptr++;}
05296         kptr++;}
05297         img2->update();
05298         return img2;
05299 }
05300 #undef old_ptr
05301 #undef new_ptr
05302 
05303 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05304 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05305 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05306 {
05307         /* Exception Handle */
05308         if (!img) throw NullPointerException("NULL input image");
05309         /* ============================== */
05310 
05311         // Get the size of the input image
05312         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05313         /* ============================== */
05314 
05315         /* Exception Handle */
05316         if(new_nx>nx || new_ny>ny || new_nz>nz)
05317                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05318         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)
05319                 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05320         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))))
05321                 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05322         /* ============================== */
05323 
05324         /*    Calculation of the start point */
05325         int  new_st_x = nx/2-new_nx/2 + x_offset,
05326              new_st_y = ny/2-new_ny/2 + y_offset,
05327              new_st_z = nz/2-new_nz/2 + z_offset;
05328         /* ============================== */
05329 
05330         /* Exception Handle */
05331         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05332                 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05333         /* ============================== */
05334 
05335         EMData* wind = img->copy_empty_head();
05336         wind->set_size(new_nx, new_ny, new_nz);
05337         float *outp=wind->get_data();
05338         float *inp=img->get_data();
05339 
05340         for (int k=0; k<new_nz; k++)
05341                 for(int j=0; j<new_ny; j++)
05342                         for(int i=0; i<new_nx; i++)
05343                                 outp(i,j,k) = inp(i,j,k);
05344         wind->update();
05345         return wind;
05346 }
05347 #undef inp
05348 #undef outp
05349 
05350 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05351 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05352 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)
05353 {
05354         /* Exception Handle */
05355         if (!img)  throw NullPointerException("NULL input image");
05356         /* ============================== */
05357 
05358         // Get the size of the input image
05359         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05360         /* ============================== */
05361 
05362         /* Exception Handle */
05363         if(new_nx<nx || new_ny<ny || new_nz<nz)
05364                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05365         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)
05366                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05367         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))))
05368                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05369         /* ============================== */
05370 
05371         EMData* pading = img->copy_head();
05372         pading->set_size(new_nx, new_ny, new_nz);
05373         float *inp  = img->get_data();
05374         float *outp = pading->get_data();
05375 
05376 
05377         /* Calculation of the average and the circumference values for background substitution
05378         =======================================================================================*/
05379         float background;
05380 
05381         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05382         else if (strcmp(params,"circumference")==0) {
05383                 float sum1=0.0f;
05384                 size_t cnt=0;
05385                 for(int i=0;i<nx;i++) {
05386                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05387                         cnt+=2;
05388                 }
05389                 if(nz-1 == 0) {
05390                         for (int j=1;j<ny-1;j++) {
05391                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05392                                 cnt+=2;
05393                         }
05394                 } else {
05395                         for (int k=1;k<nz-1;k++) {
05396                                 for (int j=1;j<ny-1;j++) {
05397                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05398                                         cnt+=2;
05399                                 }
05400                         }
05401                 }
05402                 background = sum1/cnt;
05403         } else {
05404                 background = static_cast<float>( atof( params ) );
05405         }
05406         /*=====================================================================================*/
05407 
05408          /*Initial Padding */
05409         int new_st_x=0,new_st_y=0,new_st_z=0;
05410         for (int k=0;k<new_nz;k++)
05411                 for(int j=0;j<new_ny;j++)
05412                         for (int i=0;i<new_nx;i++)
05413                                 outp(i,j,k)=background;
05414         /*============================== */
05415 
05416         /*    Calculation of the start point */
05417         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05418         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05419         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05420         /* ============================== */
05421 
05422         for (int k=0;k<nz;k++)
05423                 for(int j=0;j<ny;j++)
05424                         for(int i=0;i<nx;i++)
05425                                 outp(i,j,k)=inp(i,j,k);
05426         pading->update();
05427         return pading;
05428 }
05429 #undef inp
05430 #undef outp
05431 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05432 
05433 void Util::colreverse(float* beg, float* end, int nx) {
05434         float* tmp = new float[nx];
05435         int n = (end - beg)/nx;
05436         int nhalf = n/2;
05437         for (int i = 0; i < nhalf; i++) {
05438                 // swap col i and col n-1-i
05439                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05440                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05441                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05442         }
05443         delete[] tmp;
05444 }
05445 
05446 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05447 {
05448         int nxy = nx*ny;
05449         colreverse(beg, end, nxy);
05450 }
05451 
05452 
05453 void Util::cyclicshift(EMData *image, Dict params) {
05454 
05455         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05456 
05457         int dx = params["dx"];
05458         int dy = params["dy"];
05459         int dz = params["dz"];
05460 
05461         // The reverse trick we're using shifts to the left (a negative shift)
05462         int nx = image->get_xsize();
05463         dx %= nx;
05464         if (dx < 0) dx += nx;
05465         int ny = image->get_ysize();
05466         dy %= ny;
05467         if (dy < 0) dy += ny;
05468         int nz = image->get_zsize();
05469         dz %= nz;
05470         if (dz < 0) dz += nz;
05471 
05472         int mx = -(dx - nx);
05473         int my = -(dy - ny);
05474         int mz = -(dz - nz);
05475 
05476         float* data = image->get_data();
05477         // x-reverses
05478         if (mx != 0) {
05479                 for (int iz = 0; iz < nz; iz++)
05480                        for (int iy = 0; iy < ny; iy++) {
05481                                 // reverses for column iy
05482                                 size_t offset = nx*iy + (size_t)nx*ny*iz; // starting location for column iy in slice iz
05483                                 reverse(&data[offset],&data[offset+mx]);
05484                                 reverse(&data[offset+mx],&data[offset+nx]);
05485                                 reverse(&data[offset],&data[offset+nx]);
05486                         }
05487         }
05488         // y-reverses
05489         if (my != 0) {
05490                 for (int iz = 0; iz < nz; iz++) {
05491                         size_t offset = (size_t)nx*ny*iz;
05492                         colreverse(&data[offset], &data[offset + my*nx], nx);
05493                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05494                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05495                 }
05496         }
05497         if (mz != 0) {
05498                 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05499                 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05500                 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05501         }
05502         image->update();
05503 }
05504 
05505 //-----------------------------------------------------------------------------------------------------------------------
05506 
05507 
05508 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05509 {
05510         if (image->is_complex())
05511                 throw ImageFormatException("Cannot do histogram on Fourier image");
05512         //float hmax, hmin;
05513         float *imageptr=0, *maskptr=0;
05514         int nx=image->get_xsize();
05515         int ny=image->get_ysize();
05516         int nz=image->get_zsize();
05517 
05518         if(mask != NULL){
05519                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05520                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05521                 maskptr =mask->get_data();
05522         }
05523         if( nbins == 0) nbins = nx;
05524         vector <float> freq(2*nbins, 0.0);
05525 
05526         imageptr=image->get_data();
05527         if( hmin == hmax ) {
05528                 if(mask == NULL) {
05529                         hmax = image->get_attr("maximum");
05530                         hmin = image->get_attr("minimum");
05531                 } else {
05532                         bool  First = true;
05533                         for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05534                         if (maskptr[i]>=0.5f) {
05535                                         if(First) {
05536                                                 hmax = imageptr[i];
05537                                                 hmin = imageptr[i];
05538                                                 First = false;
05539                                         } else {
05540                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05541                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05542                                         }
05543                                 }
05544                         }
05545                 }
05546         }
05547         float hdiff = hmax - hmin;
05548         float ff = (nbins-1)/hdiff;
05549         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05550         if(mask == NULL) {
05551                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05552                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05553                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05554                 }
05555         } else {
05556                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05557                         if(maskptr[i] >= 0.5) {
05558                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05559                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05560                         }
05561                 }
05562         }
05563         return freq;
05564 }
05565 
05566 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05567 {
05568         /* Exception Handle */
05569         if (img->is_complex() || ref->is_complex())
05570                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05571 
05572         if(mask != NULL){
05573                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05574                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05575         /* ===================================================== */
05576 
05577         /* Image size calculation */
05578         size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05579         size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05580         /* ===================================================== */
05581 
05582         /* The reference image attributes */
05583         float *ref_ptr = ref->get_data();
05584         float ref_h_min = ref->get_attr("minimum");
05585         float ref_h_max = ref->get_attr("maximum");
05586         float ref_h_avg = ref->get_attr("mean");
05587         float ref_h_sig = ref->get_attr("sigma");
05588         /* ===================================================== */
05589 
05590         /* Input image under mask attributes */
05591         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05592 
05593         vector<float> img_data = Util::infomask(img, mask);
05594         float img_avg = img_data[0];
05595         float img_sig = img_data[1];
05596 
05597         /* The image under mask -- size calculation */
05598         int cnt=0;
05599         for(size_t i=0;i<size_img;++i)
05600                 if (mask_ptr[i]>0.5f)
05601                                 cnt++;
05602         /* ===================================================== */
05603 
05604         /* Histogram of reference image calculation */
05605         float ref_h_diff = ref_h_max - ref_h_min;
05606 
05607         #ifdef _WIN32
05608                 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05609         #else
05610                 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05611         #endif  //_WIN32
05612 
05613         float *ref_freq_bin = new float[3*hist_len];
05614 
05615         //initialize value in each bin to zero
05616         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05617 
05618         for (size_t i = 0;i < size_ref;++i) {
05619                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05620                 ref_freq_bin[L]++;
05621         }
05622         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05623 
05624         //Parameters Calculation (i.e) 'A' x + 'B'
05625         float A = ref_h_sig/img_sig;
05626         float B = ref_h_avg - (A*img_avg);
05627 
05628         vector<float> args;
05629         args.push_back(A);
05630         args.push_back(B);
05631 
05632         vector<float> scale;
05633         scale.push_back(1.e-7f*A);
05634         scale.push_back(-1.e-7f*B);
05635 
05636         vector<float> ref_freq_hist;
05637         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05638 
05639         vector<float> data;
05640         data.push_back(ref_h_diff);
05641         data.push_back(ref_h_min);
05642 
05643         Dict parameter;
05644 
05645         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05646         parameter["args"] = args;
05647         parameter["scale"]= scale;
05648         parameter["data"] = data;
05649         parameter["ref_freq_bin"] = ref_freq_hist;
05650         parameter["size_img"]=(double)size_img;
05651         parameter["hist_len"]=hist_len;
05652         /* ===================================================== */
05653 
05654         return parameter;
05655 }
05656 
05657 
05658 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)
05659 {
05660         float *img_ptr = img->get_data();
05661         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05662 
05663         int *img_freq_bin = new int[3*hist_len];
05664         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05665         for(size_t i = 0;i < size_img;++i) {
05666                 if(mask_ptr[i] > 0.5f) {
05667                         float img_xn = img_ptr[i]*PA + PB;
05668                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05669                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05670                 }
05671         }
05672         int freq_hist = 0;
05673 
05674         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);
05675         freq_hist = (-freq_hist);
05676         return static_cast<float>(freq_hist);
05677 }
05678 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05679 #define    QUADPI                       3.141592653589793238462643383279502884197
05680 #define    DGR_TO_RAD                   QUADPI/180
05681 #define    DM(I)                        DM          [I-1]
05682 #define    SS(I)                        SS          [I-1]
05683 Dict Util::CANG(float PHI,float THETA,float PSI)
05684 {
05685         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05686         vector<float>   DM,SS;
05687 
05688         for(int i =0;i<9;i++) DM.push_back(0);
05689 
05690         for(int i =0;i<6;i++) SS.push_back(0);
05691 
05692         CPHI = cos(double(PHI)*DGR_TO_RAD);
05693         SPHI = sin(double(PHI)*DGR_TO_RAD);
05694         CTHE = cos(double(THETA)*DGR_TO_RAD);
05695         STHE = sin(double(THETA)*DGR_TO_RAD);
05696         CPSI = cos(double(PSI)*DGR_TO_RAD);
05697         SPSI = sin(double(PSI)*DGR_TO_RAD);
05698 
05699         SS(1) = float(CPHI);
05700         SS(2) = float(SPHI);
05701         SS(3) = float(CTHE);
05702         SS(4) = float(STHE);
05703         SS(5) = float(CPSI);
05704         SS(6) = float(SPSI);
05705 
05706         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05707         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05708         DM(3) = float(-STHE*CPSI);
05709         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05710         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05711         DM(6) = float(STHE*SPSI);
05712         DM(7) = float(STHE*CPHI);
05713         DM(8) = float(STHE*SPHI);
05714         DM(9) = float(CTHE);
05715 
05716         Dict DMnSS;
05717         DMnSS["DM"] = DM;
05718         DMnSS["SS"] = SS;
05719 
05720         return(DMnSS);
05721 }
05722 #undef SS
05723 #undef DM
05724 #undef QUADPI
05725 #undef DGR_TO_RAD
05726 //-----------------------------------------------------------------------------------------------------------------------
05727 #define    DM(I)                        DM[I-1]
05728 #define    B(i,j)                       Bptr[i-1+((j-1)*NSAM)]
05729 #define    CUBE(i,j,k)                  CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*(size_t)NX3D]
05730 
05731 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05732 {
05733 
05734         float  *Bptr = B->get_data();
05735         float  *CUBEptr = CUBE->get_data();
05736 
05737         int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05738         float DIPX,DIPY,XB,YB,XBB,YBB;
05739 
05740         Transform * t = B->get_attr("xform.projection");
05741         Dict d = t->get_params("spider");
05742         if(t) {delete t; t=0;}
05743         //  Unsure about sign of shifts, check later PAP 06/28/09
05744         float x_shift = d[ "tx" ];
05745         float y_shift = d[ "ty" ];
05746         x_shift = -x_shift;
05747         y_shift = -y_shift;
05748 
05749         NSAM = B->get_xsize();
05750         NROW = B->get_ysize();
05751         NX3D = CUBE->get_xsize();
05752         NY3D = CUBE->get_ysize();
05753         NZC  = CUBE->get_zsize();
05754 
05755 
05756         LDPX   = NX3D/2 +1;
05757         LDPY   = NY3D/2 +1;
05758         LDPZ   = NZC/2 +1;
05759         LDPNMX = NSAM/2 +1;
05760         LDPNMY = NROW/2 +1;
05761         NZ1    = 1;
05762 
05763         for(int K=1;K<=NZC;K++) {
05764                 KZ=K-1+NZ1;
05765                 for(int J=1;J<=NY3D;J++) {
05766                         XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05767                         YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05768                         for(int I=1;I<=NX3D;I++) {
05769                                 XB  = (I-1)*DM(1)+XBB-x_shift;
05770                                 IQX = int(XB+float(LDPNMX));
05771                                 if (IQX <1 || IQX >= NSAM) continue;
05772                                 YB  = (I-1)*DM(4)+YBB-y_shift;
05773                                 IQY = int(YB+float(LDPNMY));
05774                                 if (IQY<1 || IQY>=NROW)  continue;
05775                                 DIPX = XB+LDPNMX-IQX;
05776                                 DIPY = YB+LDPNMY-IQY;
05777 
05778                                 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05779                         }
05780                 }
05781         }
05782 }
05783 
05784 #undef DM
05785 #undef B
05786 #undef CUBE
05787 
05788 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05789 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05790 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05791 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05792 
05793 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05794 {
05795         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05796         float WW,OX,OY;
05797 
05798         NSAM = PROJ->get_xsize();
05799         NROW = PROJ->get_ysize();
05800         int ntotal = NSAM*NROW;
05801         float q = 2.0f;
05802         float qt = 8.0f/q;
05803         //  Fix for padding 2x
05804         int ipad = 1;
05805         NSAM *= ipad;
05806         NROW *= ipad;
05807         NNNN = NSAM+2-(NSAM%2);
05808         int NX2 = NSAM/2;
05809         NR2  = NROW/2;
05810 
05811         NANG = int(SS.size())/6;
05812 
05813         EMData* W = new EMData();
05814         int Wnx = NNNN/2;
05815         W->set_size(Wnx,NROW,1);
05816         W->to_zero();
05817         float *Wptr = W->get_data();
05818         float *PROJptr = PROJ->get_data();
05819         for (L=1; L<=NANG; L++) {
05820                 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);
05821                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05822                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05823                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05824                 if(OX < 0.0f) {
05825                         OX = -OX;
05826                         OY = -OY;
05827                 }
05828 
05829                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05830                         for(int J=1;J<=NROW;J++) {
05831                                 JY = (J-1);
05832                                 if(JY > NR2) JY -= NROW;
05833 #ifdef _WIN32
05834                                 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05835                                 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05836 #else
05837                                 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05838                                 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05839 #endif  //_WIN32
05840                                 if( xmi <= xma) {
05841                                         for(int I=xmi;I<=xma;I++) {
05842                                                 float Y = fabs(OX*I + OY*JY);
05843                                                 W(I+1,J) += exp(-qt*Y*Y);
05844         //cout << " L   "<<L << " I   "<<I << " JY   "<<JY << " ARG   "<<qt*Y*Y <<endl;
05845                                         }
05846                                 }
05847                         }
05848                 } else {
05849                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05850                 }
05851         }
05852         EMData* proj_in = PROJ;
05853 
05854         PROJ = PROJ->norm_pad( false, ipad);
05855         PROJ->do_fft_inplace();
05856         PROJ->update();
05857         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05858         PROJptr = PROJ->get_data();
05859 
05860         float WNRMinv,temp;
05861         float osnr = 1.0f/SNR;
05862         WNRMinv = 1.0f/W(1,1);
05863         for(int J=1;J<=NROW;J++)  {
05864                 JY = J-1;
05865                 if( JY > NR2)  JY -= NROW;
05866                 float sy = JY;
05867                 sy /= NROW;
05868                 sy *= sy;
05869                 for(int I=1;I<=NNNN;I+=2) {
05870                         KX           = (I+1)/2;
05871                         temp         = W(KX,J)*WNRMinv;
05872                         WW           = temp/(temp*temp + osnr);
05873                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
05874                         float sx = KX-1;
05875                         sx /= NSAM;
05876                         WW *= exp(qt*(sy + sx*sx));
05877                         PROJ(I,J)   *= WW;
05878                         PROJ(I+1,J) *= WW;
05879                 }
05880         }
05881         delete W; W = 0;
05882         PROJ->do_ift_inplace();
05883         PROJ->depad();
05884 
05885         float* data_src = PROJ->get_data();
05886         float* data_dst = proj_in->get_data();
05887 
05888         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05889 
05890         proj_in->update();
05891 
05892         delete PROJ;
05893 }
05894 /*
05895 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05896 {
05897         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05898         float WW,OX,OY,Y;
05899 
05900         NSAM = PROJ->get_xsize();
05901         NROW = PROJ->get_ysize();
05902         //  Fix for padding 2x
05903         int ntotal = NSAM*NROW;
05904         int ipad = 1;
05905         NSAM *= ipad;
05906         NROW *= ipad;
05907         NNNN = NSAM+2-(NSAM%2);
05908         NR2  = NROW/2;
05909 
05910         NANG = int(SS.size())/6;
05911 
05912         EMData* W = new EMData();
05913         int Wnx = NNNN/2;
05914         W->set_size(Wnx,NROW,1);
05915         W->to_zero();
05916         float *Wptr = W->get_data();
05917         float *PROJptr = PROJ->get_data();
05918         for (L=1; L<=NANG; L++) {
05919                 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);
05920                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05921                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05922                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05923         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
05924 
05925                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
05926                         for(int J=1;J<=NROW;J++) {
05927                                 JY = (J-1);
05928                                 if(JY > NR2) JY=JY-NROW;
05929                                 for(int I=1;I<=NNNN/2;I++) {
05930                                         Y =  fabs(OX * (I-1) + OY * JY);
05931                                         if(Y < 2.0f) {
05932                                         W(I,J) += exp(-4*Y*Y);
05933         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
05934                                 }
05935                         }
05936                 } else {
05937                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05938                 }
05939         }
05940         EMData* proj_in = PROJ;
05941 
05942         PROJ = PROJ->norm_pad( false, ipad);
05943         PROJ->do_fft_inplace();
05944         PROJ->update();
05945         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05946         PROJptr = PROJ->get_data();
05947 
05948         float WNRMinv,temp;
05949         float osnr = 1.0f/SNR;
05950         WNRMinv = 1.0f/W(1,1);
05951         for(int J=1;J<=NROW;J++)
05952                 for(int I=1;I<=NNNN;I+=2) {
05953                         KX           = (I+1)/2;
05954                         temp         = W(KX,J)*WNRMinv;
05955                         WW           = temp/(temp*temp + osnr);
05956                         PROJ(I,J)   *= WW;
05957                         PROJ(I+1,J) *= WW;
05958                 }
05959         delete W; W = 0;
05960         PROJ->do_ift_inplace();
05961         PROJ->depad();
05962 
05963         float* data_src = PROJ->get_data();
05964         float* data_dst = proj_in->get_data();
05965 
05966         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05967 
05968         proj_in->update();
05969 
05970         delete PROJ;
05971 }
05972 */
05973 #undef PROJ
05974 #undef W
05975 #undef SS
05976 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05977 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05978 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05979 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05980 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
05981 #define    CC(i)                        CC          [i-1]
05982 #define    CP(i)                        CP          [i-1]
05983 #define    VP(i)                        VP          [i-1]
05984 #define    VV(i)                        VV          [i-1]
05985 #define    AMAX1(i,j)                   i>j?i:j
05986 #define    AMIN1(i,j)                   i<j?i:j
05987 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05988 {
05989         float rad2deg =(180.0f/3.1415926f);
05990         float deg2rad = (3.1415926f/180.0f);
05991 
05992         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05993 
05994         NSAM = PROJ->get_xsize();
05995         NROW = PROJ->get_ysize();
05996         NNNN = NSAM+2-(NSAM%2);
05997         NR2  = NROW/2;
05998         NANG = int(SS.size())/6;
05999 
06000         float RI[9];
06001         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06002         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06003         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06004         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06005         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06006         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06007         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06008         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06009         RI(3,3)=SS(3,NUMP);
06010 
06011         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06012 
06013         EMData* W = new EMData();
06014         int Wnx = NNNN/2;
06015         W->set_size(NNNN/2,NROW,1);
06016         W->to_one();
06017         float *Wptr = W->get_data();
06018 
06019         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06020 
06021         for (L=1; L<=NANG; L++) {
06022                 if (L != NUMP) {
06023                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06024                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06025                         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);
06026 
06027                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
06028                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06029                         ALPHA=rad2deg*float(asin(CCN));
06030                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06031                         if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
06032                         if(ALPHA<1.0E-6) {
06033                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06034                         } else {
06035                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06036                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
06037                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06038                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06039                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06040                                 CP(1)   = 0.0;CP(2) = 0.0;
06041                                 VP(1)   = 0.0;VP(2) = 0.0;
06042 
06043                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06044                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06045                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06046                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06047 
06048                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06049 
06050                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
06051                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
06052                                 float tmpinv = 1.0f/TMP;
06053                                 for(int J=1;J<=NROW;J++) {
06054                                         JY = (J-1);
06055                                         if (JY>NR2)  JY=JY-NROW;
06056                                         for(int I=1;I<=NNNN/2;I++) {
06057                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06058                                                 RT     = 1.0f-FV/FM;
06059                                                 W(I,J) += ((RT>0.0f)*RT);
06060                                         }
06061                                 }
06062                         }
06063                 }
06064         }
06065 
06066         EMData* proj_in = PROJ;
06067 
06068         PROJ = PROJ->norm_pad( false, 1);
06069         PROJ->do_fft_inplace();
06070         PROJ->update();
06071         float *PROJptr = PROJ->get_data();
06072 
06073         int KX;
06074         float WW;
06075         for(int J=1; J<=NROW; J++)
06076                 for(int I=1; I<=NNNN; I+=2) {
06077                         KX          =  (I+1)/2;
06078                         WW          =  1.0f/W(KX,J);
06079                         PROJ(I,J)   = PROJ(I,J)*WW;
06080                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
06081                 }
06082         delete W; W = 0;
06083         PROJ->do_ift_inplace();
06084         PROJ->depad();
06085 
06086         float* data_src = PROJ->get_data();
06087         float* data_dst = proj_in->get_data();
06088 
06089         int ntotal = NSAM*NROW;
06090         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
06091 
06092         proj_in->update();
06093         delete PROJ;
06094 }
06095 #undef   AMAX1
06096 #undef   AMIN1
06097 #undef   RI
06098 #undef   CC
06099 #undef   CP
06100 #undef   VV
06101 #undef   VP
06102 
06103 
06104 #undef   W
06105 #undef   SS
06106 #undef   PROJ
06107 
06108 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06109 {
06110         float cst  = cs*1.0e7f;
06111 
06112         wgh /= 100.0;
06113         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06114         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06115         float ak2 = ak*ak;
06116         float g1 = dzz*1.0e4f*lambda*ak2;
06117         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06118 
06119         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06120         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06121 
06122         return ctfv;
06123 }
06124 
06125 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06126 {
06127         /***********
06128         ***get the size of the image for validation purpose
06129         **************/
06130         int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();  //Aren't  these  implied?  Please check and let me know, PAP.
06131         /********
06132         ***Exception Handle
06133         *************/
06134         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06135                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06136 
06137         size_t i, size = (size_t)nx*ny*nz;
06138 
06139         float* img_ptr = image->get_data();
06140         float* mask_ptr = mask->get_data();
06141 
06142         int ln=0;  //length of the output image = number of points under the mask.
06143         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06144 
06145         EMData* new_image = new EMData();
06146         new_image->set_size(ln,1,1); /* set size of the new image */
06147         float *new_ptr    = new_image->get_data();
06148 
06149         ln=-1;
06150         for(i = 0;i < size;i++){
06151                 if(mask_ptr[i] > 0.5f) {
06152                         ln++;
06153                         new_ptr[ln]=img_ptr[i];
06154                 }
06155         }
06156 
06157         return new_image;
06158 }
06159 
06160 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06161 {
06162         /********
06163         ***Exception Handle
06164         *************/
06165         if(mask == NULL)
06166                 throw ImageDimensionException("The mask cannot be an null image");
06167 
06168         /***********
06169         ***get the size of the mask
06170         **************/
06171         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06172 
06173         size_t i,size = (size_t)nx*ny*nz;                        /* loop counters */
06174         /* new image declaration */
06175         EMData *new_image = new EMData();
06176         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06177         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06178         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06179         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06180         int count = 0;
06181         float sum_under_mask = 0.0 ;
06182         for(i = 0;i < size;i++){
06183                         if(mask_ptr[i] > 0.5f){
06184                                 new_ptr[i] = img_ptr[count];
06185                                 sum_under_mask += img_ptr[count];
06186                                 count++;
06187                                 if( count > image->get_xsize() ) {
06188                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06189                                 }
06190                         }
06191         }
06192 
06193         if( count > image->get_xsize() ) {
06194             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06195         }
06196 
06197         float avg_under_mask = sum_under_mask / count;
06198         for(i = 0;i < size;i++) {
06199                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06200         }
06201         new_image->update();
06202         return new_image;
06203 }
06204 
06205 
06206 
06207 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06208 {
06209         vector<float>new_peak;
06210         int n1=peak1.size()/3;
06211         float p_size2=p_size*p_size;
06212         for (int i=0;i<n1;++i) {
06213                 vector<float>::iterator it2= peak1.begin()+3*i;
06214                 bool push_back1=true;
06215                 int n2=peak2.size()/3;
06216                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06217                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06218                 if(n2 ==0) {
06219                         new_peak.push_back(*it2);
06220                         new_peak.push_back(*(it2+1));
06221                         new_peak.push_back(*(it2+2));
06222                 } else  {
06223                         int j=0;
06224                         while (j< n2-1 ) {
06225                                 vector<float>::iterator it3= peak2.begin()+3*j;
06226                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06227                                 if(d2< p_size2 ) {
06228                                         if( (*it2)<(*it3) ) {
06229                                                 new_peak.push_back(*it3);
06230                                                 new_peak.push_back(*(it3+1));
06231                                                 new_peak.push_back(*(it3+2));
06232                                                 peak2.erase(it3);
06233                                                 peak2.erase(it3);
06234                                                 peak2.erase(it3);
06235                                                 push_back1=false;
06236                                         } else {
06237                                                 peak2.erase(it3);
06238                                                 peak2.erase(it3);
06239                                                 peak2.erase(it3);
06240                                         }
06241                                 } else  j=j+1;
06242                                 n2=peak2.size()/3;
06243                         }
06244                         if(push_back1) {
06245                                 new_peak.push_back(*it2);
06246                                 new_peak.push_back(*(it2+1));
06247                                 new_peak.push_back(*(it2+2));
06248                         }
06249                 }
06250         }
06251         return new_peak;
06252 }
06253 
06254 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06255 {
06256         // n size of the covariance/correlation matrix
06257         // covmat --- covariance/correlation matrix (n by n)
06258         // eigval --- returns eigenvalues
06259         // eigvec --- returns eigenvectors
06260 
06261         ENTERFUNC;
06262 
06263         int i;
06264 
06265         // make a copy of covmat so that it will not be overwritten
06266         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06267 
06268         char NEEDV = 'V';
06269         char UPLO = 'U';
06270         int lwork = -1;
06271         int info = 0;
06272         float *work, wsize;
06273 
06274         //  query to get optimal workspace
06275         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06276         lwork = (int)wsize;
06277 
06278         work = (float *)calloc(lwork, sizeof(float));
06279         //  calculate eigs
06280         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06281         free(work);
06282         EXITFUNC;
06283         return info;
06284 }
06285 
06286 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06287 {
06288 
06289         ENTERFUNC;
06290         int len = covmatpy.size();
06291         float *eigvec;
06292         float *eigval;
06293         float *covmat;
06294         int status = 0;
06295         eigval = (float*)calloc(ncov,sizeof(float));
06296         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06297         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06298 
06299         const float *covmat_ptr;
06300         covmat_ptr = &covmatpy[0];
06301         for(int i=0;i<len;i++){
06302             covmat[i] = covmat_ptr[i];
06303         }
06304 
06305         status = Util::coveig(ncov, covmat, eigval, eigvec);
06306 
06307         vector<float> eigval_py(ncov);
06308         const float *eigval_ptr;
06309         eigval_ptr = &eigval[0];
06310         for(int i=0;i<ncov;i++){
06311             eigval_py[i] = eigval_ptr[i];
06312         }
06313 
06314         vector<float> eigvec_py(ncov*ncov);
06315         const float *eigvec_ptr;
06316         eigvec_ptr = &eigvec[0];
06317         for(int i=0;i<ncov*ncov;i++){
06318             eigvec_py[i] = eigvec_ptr[i];
06319         }
06320 
06321         Dict res;
06322         res["eigval"] = eigval_py;
06323         res["eigvec"] = eigvec_py;
06324 
06325         EXITFUNC;
06326         return res;
06327 }
06328 
06329 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06330 {
06331         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06332 
06333         k=(int)pw.size();
06334         l=0;
06335         m=k;
06336         n2=n+2;
06337         n1=n+1;
06338         klmd=k+l+m;
06339         klm2d= k+l+m+2;
06340         nklmd=k+l+m+n;
06341         n2d=n+2;
06342         /*size has to be increased when N is large*/
06343         n_larg=klmd*2;
06344         klm2d=n_larg+klm2d;
06345         klmd=n_larg+klmd;
06346         nklmd=n_larg+nklmd;
06347         int size_q=klm2d*n2d;
06348         int size_cu=nklmd*2;
06349         static int i__;
06350 
06351          double *q ;
06352          double *x ;
06353          double *res;
06354          double *cu;
06355          float *q2;
06356          float *pw_;
06357          long int *iu;
06358          double *s;
06359          q = (double*)calloc(size_q,sizeof(double));
06360          x = (double*)calloc(n2d,sizeof(double));
06361          res = (double*)calloc(klmd,sizeof(double));
06362          cu =(double*)calloc(size_cu,sizeof(double));
06363          s = (double*)calloc(klmd,sizeof(double));
06364          q2 = (float*)calloc(size_q,sizeof(float));
06365          iu = (long int*)calloc(size_cu,sizeof(long int));
06366          pw_ = (float*)calloc(k,sizeof(float));
06367 
06368         for( i__ =0;i__<k;++i__)
06369                 {
06370                 pw_[i__]=log(pw[i__]); }
06371         long int l_k=k;
06372         long int l_n=n;
06373         long int l_iswi=iswi;
06374         vector<float> cl1_res;
06375         cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06376         free(q);
06377         free(x);
06378         free(res);
06379         free(s);
06380         free(cu);
06381         free(q2);
06382         free(iu);
06383         free(pw_);
06384         return cl1_res;
06385 }
06386 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)
06387 {
06388     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06389     float r__1;
06390     int tmp__i;
06391     long int i__, j;
06392     --s;
06393     --res;
06394     iu -= 3;
06395     cu -= 3;
06396     --x;
06397     long int klm2d;
06398     klm2d= *k+*k+2;
06399     klm2d=klm2d+klm2d;
06400     q_dim1 = klm2d;
06401     q_offset = 1 + q_dim1;
06402     q -= q_offset;
06403     q2_dim1 = klm2d;
06404     q2_offset = 1 + q2_dim1;
06405     q2 -= q2_offset;
06406     i__2=0;
06407     i__1 = *n - 1;
06408     tmp__i=0;
06409     for (j = 1; j <= i__1; ++j) {
06410         i__2 = *k;
06411         tmp__i+=1;
06412         for (i__ = 1; i__ <= i__2; ++i__) {
06413             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06414             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06415         }
06416     }
06417     for  (i__ = 1; i__ <= i__2; ++i__)
06418       { q2[i__ + *n * q2_dim1] = 1.f;
06419             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06420         }
06421    vector<float> fit_res;
06422    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06423    return fit_res;
06424 }
06425 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)
06426 {
06427     /* System generated locals */
06428     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06429 
06430     /* Local variables */
06431     long int i__, j, m, n1, ii, jj;
06432     double tmp;
06433     vector<float> p;
06434     --x;
06435     q_dim1 = *klm2d;
06436     q_offset = 1 + q_dim1;
06437     q -= q_offset;
06438     q1_dim1 = *klm2d;
06439     q1_offset = 1 + q1_dim1;
06440     q1 -= q1_offset;
06441     --s;
06442     --res;
06443     iu -= 3;
06444     cu -= 3;
06445 
06446     /* Function Body */
06447     long int l = 0;
06448 
06449 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06450     m = *ks;
06451     n1 = *n + 1;
06452     if (*iswi == 1) {
06453         i__1 = n1;
06454         for (jj = 1; jj <= i__1; ++jj) {
06455             i__2 = *ks;
06456             for (ii = 1; ii <= i__2; ++ii) {
06457         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06458 
06459                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06460                         ;
06461             }
06462         }
06463     } else if (*iswi == 2) {
06464         i__1 = *ks;
06465         for (ii = 1; ii <= i__1; ++ii) {
06466             i__2 = n1;
06467             for (jj = 1; jj <= i__2; ++jj) {
06468                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06469                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06470             }
06471         }
06472     } else if (*iswi == 3) {
06473         l = 2;
06474         i__1 = n1;
06475         for (jj = 1; jj <= i__1; ++jj) {
06476             i__2 = *ks + 2;
06477             for (ii = 1; ii <= i__2; ++ii) {
06478                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06479             }
06480             i__2 = *ks;
06481             for (ii = 1; ii <= i__2; ++ii) {
06482                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06483             }
06484         }
06485     } else if (*iswi == 4) {
06486         l = 2;
06487         i__1 = n1;
06488         for (jj = 1; jj <= i__1; ++jj) {
06489             i__2 = *ks + 2;
06490             for (ii = 1; ii <= i__2; ++ii) {
06491                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06492             }
06493             i__2 = *ks;
06494             for (ii = 1; ii <= i__2; ++ii) {
06495                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06496             }
06497         }
06498     } else if (*iswi == 5) {
06499         l = 1;
06500         i__1 = n1;
06501         for (jj = 1; jj <= i__1; ++jj) {
06502             i__2 = *ks + 1;
06503             for (ii = 1; ii <= i__2; ++ii) {
06504                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06505             }
06506             i__2 = *ks;
06507             for (ii = 1; ii <= i__2; ++ii) {
06508                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06509             }
06510         }
06511     } else if (*iswi == 6) {
06512         l = 1;
06513         i__1 = n1;
06514         for (jj = 1; jj <= i__1; ++jj) {
06515             i__2 = *ks + 1;
06516             for (ii = 1; ii <= i__2; ++ii) {
06517                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06518             }
06519             i__2 = *ks;
06520             for (ii = 1; ii <= i__2; ++ii) {
06521                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06522             }
06523         }
06524     } else if (*iswi == 7) {
06525         l = 3;
06526         i__1 = n1;
06527         for (jj = 1; jj <= i__1; ++jj) {
06528             i__2 = *ks + 3;
06529             for (ii = 1; ii <= i__2; ++ii) {
06530                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06531             }
06532             i__2 = *ks;
06533             for (ii = 1; ii <= i__2; ++ii) {
06534                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06535             }
06536         }
06537     } else if (*iswi == 8) {
06538         l = 4;
06539         i__1 = n1;
06540         for (jj = 1; jj <= i__1; ++jj) {
06541             i__2 = *ks + 4;
06542             for (ii = 1; ii <= i__2; ++ii) {
06543                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06544             }
06545             i__2 = *ks;
06546             for (ii = 1; ii <= i__2; ++ii) {
06547                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06548             }
06549         }
06550     }
06551 
06552     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06553     i__1 = *ks;
06554     int tmp__j=0;
06555     for (i__ = 1; i__ <= i__1; ++i__) {
06556         tmp = 0.f;
06557         i__2 = *n - 1;
06558         for (j = 1; j <= i__2; ++j) {
06559         tmp__j=j;
06560             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06561         }
06562         tmp += x[*n];
06563         p.push_back(static_cast<float>(exp(tmp)));
06564         p.push_back(q1[i__ + q1_dim1]);
06565     }
06566     i__2=*n;
06567     for (i__=1;i__<=i__2;++i__)
06568         { p.push_back(static_cast<float>(x[i__]));}
06569     return p;
06570 }
06571 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06572         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06573 {
06574 
06575     long int q_dim1, q_offset, i__1, i__2;
06576     double d__1;
06577 
06578     static long int i__, j;
06579     static double z__;
06580     static long int n1, n2, ia, ii, kk, in, nk, js;
06581     static double sn, zu, zv;
06582     static long int nk1, klm, nkl, jmn, jpn;
06583     static double cuv;
06584     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06585     static float xmin;
06586     static double xmax;
06587     static long int iout;
06588     static double xsum;
06589     static long int iineg, maxit;
06590     static double toler;
06591     static float error;
06592     static double pivot;
06593     static long int kforce, iphase;
06594     static double tpivot;
06595 
06596     --s;
06597     --res;
06598     iu -= 3;
06599     cu -= 3;
06600     --x;
06601     q_dim1 = *klm2d;
06602     q_offset = 1 + q_dim1;
06603     q -= q_offset;
06604 
06605     /* Function Body */
06606     maxit = 500;
06607     kode = 0;
06608     toler = 1e-4f;
06609     iter = 0;
06610     n1 = *n + 1;
06611     n2 = *n + 2;
06612     nk = *n + *k;
06613     nk1 = nk + 1;
06614     nkl = nk + *l;
06615     nkl1 = nkl + 1;
06616     klm = *k + *l + *m;
06617     klm1 = klm + 1;
06618     klm2 = klm + 2;
06619     nklm = *n + klm;
06620     kforce = 1;
06621     iter = 0;
06622     js = 1;
06623     ia = 0;
06624 /* SET UP LABELS IN Q. */
06625     i__1 = *n;
06626     for (j = 1; j <= i__1; ++j) {
06627         q[klm2 + j * q_dim1] = (double) j;
06628 /* L10: */
06629     }
06630     i__1 = klm;
06631     for (i__ = 1; i__ <= i__1; ++i__) {
06632         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06633         if (q[i__ + n1 * q_dim1] >= 0.f) {
06634             goto L30;
06635         }
06636         i__2 = n2;
06637         for (j = 1; j <= i__2; ++j) {
06638             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06639 /* L20: */
06640         }
06641 L30:
06642         ;
06643     }
06644 /* SET UP PHASE 1 COSTS. */
06645     iphase = 2;
06646     i__1 = nklm;
06647     for (j = 1; j <= i__1; ++j) {
06648         cu[(j << 1) + 1] = 0.f;
06649         cu[(j << 1) + 2] = 0.f;
06650         iu[(j << 1) + 1] = 0;
06651         iu[(j << 1) + 2] = 0;
06652 /* L40: */
06653     }
06654     if (*l == 0) {
06655         goto L60;
06656     }
06657     i__1 = nkl;
06658     for (j = nk1; j <= i__1; ++j) {
06659         cu[(j << 1) + 1] = 1.f;
06660         cu[(j << 1) + 2] = 1.f;
06661         iu[(j << 1) + 1] = 1;
06662         iu[(j << 1) + 2] = 1;
06663 /* L50: */
06664     }
06665     iphase = 1;
06666 L60:
06667     if (*m == 0) {
06668         goto L80;
06669     }
06670     i__1 = nklm;
06671     for (j = nkl1; j <= i__1; ++j) {
06672         cu[(j << 1) + 2] = 1.f;
06673         iu[(j << 1) + 2] = 1;
06674         jmn = j - *n;
06675         if (q[jmn + n2 * q_dim1] < 0.f) {
06676             iphase = 1;
06677         }
06678 /* L70: */
06679     }
06680 L80:
06681     if (kode == 0) {
06682         goto L150;
06683     }
06684     i__1 = *n;
06685     for (j = 1; j <= i__1; ++j) {
06686         if ((d__1 = x[j]) < 0.) {
06687             goto L90;
06688         } else if (d__1 == 0) {
06689             goto L110;
06690         } else {
06691             goto L100;
06692         }
06693 L90:
06694         cu[(j << 1) + 1] = 1.f;
06695         iu[(j << 1) + 1] = 1;
06696         goto L110;
06697 L100:
06698         cu[(j << 1) + 2] = 1.f;
06699         iu[(j << 1) + 2] = 1;
06700 L110:
06701         ;
06702     }
06703     i__1 = *k;
06704     for (j = 1; j <= i__1; ++j) {
06705         jpn = j + *n;
06706         if ((d__1 = res[j]) < 0.) {
06707             goto L120;
06708         } else if (d__1 == 0) {
06709             goto L140;
06710         } else {
06711             goto L130;
06712         }
06713 L120:
06714         cu[(jpn << 1) + 1] = 1.f;
06715         iu[(jpn << 1) + 1] = 1;
06716         if (q[j + n2 * q_dim1] > 0.f) {
06717             iphase = 1;
06718         }
06719         goto L140;
06720 L130:
06721         cu[(jpn << 1) + 2] = 1.f;
06722         iu[(jpn << 1) + 2] = 1;
06723         if (q[j + n2 * q_dim1] < 0.f) {
06724             iphase = 1;
06725         }
06726 L140:
06727         ;
06728     }
06729 L150:
06730     if (iphase == 2) {
06731         goto L500;
06732     }
06733 /* COMPUTE THE MARGINAL COSTS. */
06734 L160:
06735     i__1 = n1;
06736     for (j = js; j <= i__1; ++j) {
06737         xsum = 0.;
06738         i__2 = klm;
06739         for (i__ = 1; i__ <= i__2; ++i__) {
06740             ii = (long int) q[i__ + n2 * q_dim1];
06741             if (ii < 0) {
06742                 goto L170;
06743             }
06744             z__ = cu[(ii << 1) + 1];
06745             goto L180;
06746 L170:
06747             iineg = -ii;
06748             z__ = cu[(iineg << 1) + 2];
06749 L180:
06750             xsum += q[i__ + j * q_dim1] * z__;
06751 /*  180       XSUM = XSUM + Q(I,J)*Z */
06752 /* L190: */
06753         }
06754         q[klm1 + j * q_dim1] = xsum;
06755 /* L200: */
06756     }
06757     i__1 = *n;
06758     for (j = js; j <= i__1; ++j) {
06759         ii = (long int) q[klm2 + j * q_dim1];
06760         if (ii < 0) {
06761             goto L210;
06762         }
06763         z__ = cu[(ii << 1) + 1];
06764         goto L220;
06765 L210:
06766         iineg = -ii;
06767         z__ = cu[(iineg << 1) + 2];
06768 L220:
06769         q[klm1 + j * q_dim1] -= z__;
06770 /* L230: */
06771     }
06772 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06773 L240:
06774     xmax = 0.f;
06775     if (js > *n) {
06776         goto L490;
06777     }
06778     i__1 = *n;
06779     for (j = js; j <= i__1; ++j) {
06780         zu = q[klm1 + j * q_dim1];
06781         ii = (long int) q[klm2 + j * q_dim1];
06782         if (ii > 0) {
06783             goto L250;
06784         }
06785         ii = -ii;
06786         zv = zu;
06787         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06788         goto L260;
06789 L250:
06790         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06791 L260:
06792         if (kforce == 1 && ii > *n) {
06793             goto L280;
06794         }
06795         if (iu[(ii << 1) + 1] == 1) {
06796             goto L270;
06797         }
06798         if (zu <= xmax) {
06799             goto L270;
06800         }
06801         xmax = zu;
06802         in = j;
06803 L270:
06804         if (iu[(ii << 1) + 2] == 1) {
06805             goto L280;
06806         }
06807         if (zv <= xmax) {
06808             goto L280;
06809         }
06810         xmax = zv;
06811         in = j;
06812 L280:
06813         ;
06814     }
06815     if (xmax <= toler) {
06816         goto L490;
06817     }
06818     if (q[klm1 + in * q_dim1] == xmax) {
06819         goto L300;
06820     }
06821     i__1 = klm2;
06822     for (i__ = 1; i__ <= i__1; ++i__) {
06823         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06824 /* L290: */
06825     }
06826     q[klm1 + in * q_dim1] = xmax;
06827 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06828 L300:
06829     if (iphase == 1 || ia == 0) {
06830         goto L330;
06831     }
06832     xmax = 0.f;
06833     i__1 = ia;
06834     for (i__ = 1; i__ <= i__1; ++i__) {
06835         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06836         if (z__ <= xmax) {
06837             goto L310;
06838         }
06839         xmax = z__;
06840         iout = i__;
06841 L310:
06842         ;
06843     }
06844     if (xmax <= toler) {
06845         goto L330;
06846     }
06847     i__1 = n2;
06848     for (j = 1; j <= i__1; ++j) {
06849         z__ = q[ia + j * q_dim1];
06850         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06851         q[iout + j * q_dim1] = z__;
06852 /* L320: */
06853     }
06854     iout = ia;
06855     --ia;
06856     pivot = q[iout + in * q_dim1];
06857     goto L420;
06858 L330:
06859     kk = 0;
06860     i__1 = klm;
06861     for (i__ = 1; i__ <= i__1; ++i__) {
06862         z__ = q[i__ + in * q_dim1];
06863         if (z__ <= toler) {
06864             goto L340;
06865         }
06866         ++kk;
06867         res[kk] = q[i__ + n1 * q_dim1] / z__;
06868         s[kk] = (double) i__;
06869 L340:
06870         ;
06871     }
06872 L350:
06873     if (kk > 0) {
06874         goto L360;
06875     }
06876     kode = 2;
06877     goto L590;
06878 L360:
06879     xmin = static_cast<float>( res[1] );
06880     iout = (long int) s[1];
06881     j = 1;
06882     if (kk == 1) {
06883         goto L380;
06884     }
06885     i__1 = kk;
06886     for (i__ = 2; i__ <= i__1; ++i__) {
06887         if (res[i__] >= xmin) {
06888             goto L370;
06889         }
06890         j = i__;
06891         xmin = static_cast<float>( res[i__] );
06892         iout = (long int) s[i__];
06893 L370:
06894         ;
06895     }
06896     res[j] = res[kk];
06897     s[j] = s[kk];
06898 L380:
06899     --kk;
06900     pivot = q[iout + in * q_dim1];
06901     ii = (long int) q[iout + n2 * q_dim1];
06902     if (iphase == 1) {
06903         goto L400;
06904     }
06905     if (ii < 0) {
06906         goto L390;
06907     }
06908     if (iu[(ii << 1) + 2] == 1) {
06909         goto L420;
06910     }
06911     goto L400;
06912 L390:
06913     iineg = -ii;
06914     if (iu[(iineg << 1) + 1] == 1) {
06915         goto L420;
06916     }
06917 /* 400 II = IABS(II) */
06918 L400:
06919     ii = abs(ii);
06920     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06921     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06922         goto L420;
06923     }
06924 /* BYPASS INTERMEDIATE VERTICES. */
06925     i__1 = n1;
06926     for (j = js; j <= i__1; ++j) {
06927         z__ = q[iout + j * q_dim1];
06928         q[klm1 + j * q_dim1] -= z__ * cuv;
06929         q[iout + j * q_dim1] = -z__;
06930 /* L410: */
06931     }
06932     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06933     goto L350;
06934 /* GAUSS-JORDAN ELIMINATION. */
06935 L420:
06936     if (iter < maxit) {
06937         goto L430;
06938     }
06939     kode = 3;
06940     goto L590;
06941 L430:
06942     ++iter;
06943     i__1 = n1;
06944     for (j = js; j <= i__1; ++j) {
06945         if (j != in) {
06946             q[iout + j * q_dim1] /= pivot;
06947         }
06948 /* L440: */
06949     }
06950 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
06951 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
06952 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
06953 /*     DO 460 J=JS,N1 */
06954 /*        IF(J .EQ. IN) GO TO 460 */
06955 /*        Z = -Q(IOUT,J) */
06956 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
06957 /* 460 CONTINUE */
06958     i__1 = n1;
06959     for (j = js; j <= i__1; ++j) {
06960         if (j == in) {
06961             goto L460;
06962         }
06963         z__ = -q[iout + j * q_dim1];
06964         i__2 = klm1;
06965         for (i__ = 1; i__ <= i__2; ++i__) {
06966             if (i__ != iout) {
06967                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06968             }
06969 /* L450: */
06970         }
06971 L460:
06972         ;
06973     }
06974     tpivot = -pivot;
06975     i__1 = klm1;
06976     for (i__ = 1; i__ <= i__1; ++i__) {
06977         if (i__ != iout) {
06978             q[i__ + in * q_dim1] /= tpivot;
06979         }
06980 /* L470: */
06981     }
06982     q[iout + in * q_dim1] = 1.f / pivot;
06983     z__ = q[iout + n2 * q_dim1];
06984     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06985     q[klm2 + in * q_dim1] = z__;
06986     ii = (long int) abs(z__);
06987     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06988         goto L240;
06989     }
06990     i__1 = klm2;
06991     for (i__ = 1; i__ <= i__1; ++i__) {
06992         z__ = q[i__ + in * q_dim1];
06993         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06994         q[i__ + js * q_dim1] = z__;
06995 /* L480: */
06996     }
06997     ++js;
06998     goto L240;
06999 /* TEST FOR OPTIMALITY. */
07000 L490:
07001     if (kforce == 0) {
07002         goto L580;
07003     }
07004     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07005         goto L500;
07006     }
07007     kforce = 0;
07008     goto L240;
07009 /* SET UP PHASE 2 COSTS. */
07010 L500:
07011     iphase = 2;
07012     i__1 = nklm;
07013     for (j = 1; j <= i__1; ++j) {
07014         cu[(j << 1) + 1] = 0.f;
07015         cu[(j << 1) + 2] = 0.f;
07016 /* L510: */
07017     }
07018     i__1 = nk;
07019     for (j = n1; j <= i__1; ++j) {
07020         cu[(j << 1) + 1] = 1.f;
07021         cu[(j << 1) + 2] = 1.f;
07022 /* L520: */
07023     }
07024     i__1 = klm;
07025     for (i__ = 1; i__ <= i__1; ++i__) {
07026         ii = (long int) q[i__ + n2 * q_dim1];
07027         if (ii > 0) {
07028             goto L530;
07029         }
07030         ii = -ii;
07031         if (iu[(ii << 1) + 2] == 0) {
07032             goto L560;
07033         }
07034         cu[(ii << 1) + 2] = 0.f;
07035         goto L540;
07036 L530:
07037         if (iu[(ii << 1) + 1] == 0) {
07038             goto L560;
07039         }
07040         cu[(ii << 1) + 1] = 0.f;
07041 L540:
07042         ++ia;
07043         i__2 = n2;
07044         for (j = 1; j <= i__2; ++j) {
07045             z__ = q[ia + j * q_dim1];
07046             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07047             q[i__ + j * q_dim1] = z__;
07048 /* L550: */
07049         }
07050 L560:
07051         ;
07052     }
07053     goto L160;
07054 L570:
07055     if (q[klm1 + n1 * q_dim1] <= toler) {
07056         goto L500;
07057     }
07058     kode = 1;
07059     goto L590;
07060 L580:
07061     if (iphase == 1) {
07062         goto L570;
07063     }
07064 /* PREPARE OUTPUT. */
07065     kode = 0;
07066 L590:
07067     xsum = 0.;
07068     i__1 = *n;
07069     for (j = 1; j <= i__1; ++j) {
07070         x[j] = 0.f;
07071 /* L600: */
07072     }
07073     i__1 = klm;
07074     for (i__ = 1; i__ <= i__1; ++i__) {
07075         res[i__] = 0.f;
07076 /* L610: */
07077     }
07078     i__1 = klm;
07079     for (i__ = 1; i__ <= i__1; ++i__) {
07080         ii = (long int) q[i__ + n2 * q_dim1];
07081         sn = 1.f;
07082         if (ii > 0) {
07083             goto L620;
07084         }
07085         ii = -ii;
07086         sn = -1.f;
07087 L620:
07088         if (ii > *n) {
07089             goto L630;
07090         }
07091         x[ii] = sn * q[i__ + n1 * q_dim1];
07092         goto L640;
07093 L630:
07094         iimn = ii - *n;
07095         res[iimn] = sn * q[i__ + n1 * q_dim1];
07096         if (ii >= n1 && ii <= nk) {
07097             xsum += q[i__ + n1 * q_dim1];
07098         }
07099 L640:
07100         ;
07101     }
07102     error = (float)xsum;
07103     return;
07104 }
07105 
07106 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07107 {
07108         int j,d;
07109         EMData * e = new EMData();
07110         float *eptr, *imgptr;
07111         imgptr = img->get_data();
07112         float SSE = 0.f;
07113         for (j = 0 ; j < N ; j++) {
07114                 e->read_image(images,S[j]);
07115                 eptr = e->get_data();
07116                 for (d = 0; d < size; d++) {
07117                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07118                 }
07119         delete e;
07120         return SSE;
07121 }
07122 
07123 
07124 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07125 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07126 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07127 
07128 
07129 #define         quadpi                  3.141592653589793238462643383279502884197
07130 #define         dgr_to_rad              quadpi/180
07131 #define         deg_to_rad              quadpi/180
07132 #define         rad_to_deg              180/quadpi
07133 #define         rad_to_dgr              180/quadpi
07134 #define         TRUE                    1
07135 #define         FALSE                   0
07136 
07137 
07138 #define theta(i)                theta   [i-1]
07139 #define phi(i)                  phi     [i-1]
07140 #define weight(i)               weight  [i-1]
07141 #define lband(i)                lband   [i-1]
07142 #define ts(i)                   ts      [i-1]
07143 #define thetast(i)              thetast [i-1]
07144 #define key(i)                  key     [i-1]
07145 
07146 
07147 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07148 {
07149 
07150         ENTERFUNC;
07151 
07152         if ( th.size() != ph.size() ) {
07153                 LOGERR("images not same size");
07154                 throw ImageFormatException( "images not same size");
07155         }
07156 
07157         // rand_seed
07158         srand(10);
07159 
07160         int i,*key;
07161         int len = th.size();
07162         double *theta,*phi,*weight;
07163         theta   =       (double*) calloc(len,sizeof(double));
07164         phi     =       (double*) calloc(len,sizeof(double));
07165         weight  =       (double*) calloc(len,sizeof(double));
07166         key     =       (int*) calloc(len,sizeof(int));
07167         const float *thptr, *phptr;
07168 
07169         thptr = &th[0];
07170         phptr = &ph[0];
07171         for(i=1;i<=len;i++){
07172                 key(i) = i;
07173                 weight(i) = 0.0;
07174         }
07175 
07176         for(i = 0;i<len;i++){
07177                 theta[i] = thptr[i];
07178                 phi[i]   = phptr[i];
07179         }
07180 
07181         //  sort by theta
07182         Util::hsortd(theta, phi, key, len, 1);
07183 
07184         //Util::voronoidiag(theta,phi, weight, len);
07185         Util::voronoi(phi, theta, weight, len);
07186 
07187         //sort by key
07188         Util::hsortd(weight, weight, key, len, 2);
07189 
07190         free(theta);
07191         free(phi);
07192         free(key);
07193         vector<double> wt;
07194         double count = 0;
07195         for(i=1; i<= len; i++)
07196         {
07197                 wt.push_back(weight(i));
07198                 count += weight(i);
07199         }
07200 
07201         //if( abs(count-6.28) > 0.1 )
07202         //{
07203         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07204         //}
07205 
07206         free(weight);
07207 
07208         EXITFUNC;
07209         return wt;
07210 
07211 }
07212 
07213 struct  tmpstruct{
07214         double theta1,phi1;
07215         int key1;
07216         };
07217 
07218 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07219 {
07220         ENTERFUNC;
07221         vector<tmpstruct> tmp(len);
07222         int i;
07223         for(i = 1;i<=len;i++)
07224         {
07225                 tmp[i-1].theta1 = theta(i);
07226                 tmp[i-1].phi1 = phi(i);
07227                 tmp[i-1].key1 = key(i);
07228         }
07229 
07230         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07231         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07232 
07233         for(i = 1;i<=len;i++)
07234         {
07235                 theta(i) = tmp[i-1].theta1;
07236                 phi(i)   = tmp[i-1].phi1;
07237                 key(i)   = tmp[i-1].key1;
07238         }
07239         EXITFUNC;
07240 }
07241 
07242 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07243 {
07244         return(tmp1.theta1 < tmp2.theta1);
07245 }
07246 
07247 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07248 {
07249         return(tmp1.key1 < tmp2.key1);
07250 }
07251 
07252 /******************  VORONOI DIAGRAM **********************************/
07253 /*
07254 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07255 {
07256         ENTERFUNC;
07257 
07258         int     *lband;
07259         double  aat=0.0f,*ts;
07260         double  aa,acum,area;
07261         int     last;
07262         int numth       =       1;
07263         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07264 
07265         int i,it,l,k;
07266         int nband,lb,low,medium,lhigh,lbw,lenw;
07267 
07268 
07269         lband   =       (int*)calloc(nbt,sizeof(int));
07270         ts      =       (double*)calloc(nbt,sizeof(double));
07271 
07272         if(lband == NULL || ts == NULL ){
07273                 fprintf(stderr,"memory allocation failure!\n");
07274                 exit(1);
07275         }
07276 
07277         nband=nbt;
07278 
07279         while(nband>0){
07280                 Util::angstep(ts,nband);
07281 
07282                 l=1;
07283                 for(i=1;i<=n;i++){
07284                         if(theta(i)>ts(l)){
07285                                 lband(l)=i;
07286                                 l=l+1;
07287                                 if(l>nband)  exit(1);
07288                         }
07289                 }
07290 
07291                 l=1;
07292                 for(i=1;i<=n;i++){
07293                         if(theta(i)>ts(l)){
07294                                 lband(l)=i;
07295                                 l=l+1;
07296                                 if(l>nband)  exit(1);
07297                         }
07298                 }
07299 
07300                 lband(l)=n+1;
07301                 acum=0.0;
07302                 for(it=l;it>=1;it-=numth){
07303                         for(i=it;i>=mymax(1,it-numth+1);i--){
07304                         if(i==l) last   =        TRUE;
07305                         else     last   =        FALSE;
07306 
07307                         if(l==1){
07308                                 lb=1;
07309                                 low=1;
07310                                 medium=n+1;
07311                                 lhigh=n-lb+1;
07312                                 lbw=1;
07313                         }
07314                         else if(i==1){
07315                                 lb=1;
07316                                 low=1;
07317                                 medium=lband(1);
07318                                 lhigh=lband(2)-1;
07319                                 lbw=1;
07320                         }
07321                         else if(i==l){
07322                                 if(l==2)        lb=1;
07323                                 else            lb=lband(l-2);
07324                                 low=lband(l-1)-lb+1;
07325                                 medium=lband(l)-lb+1;
07326                                 lhigh=n-lb+1;
07327                                 lbw=lband(i-1);
07328                         }
07329                         else{
07330                                 if(i==2)        lb=1;
07331                                 else            lb=lband(i-2);
07332                                 low=lband(i-1)-lb+1;
07333                                 medium=lband(i)-lb+1;
07334                                 lhigh=lband(i+1)-1-lb+1;
07335                                 lbw=lband(i-1);
07336                         }
07337                         lenw=medium-low;
07338 
07339 
07340                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07341 
07342 
07343                         if(nband>1){
07344                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07345                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07346 
07347                                 aa = 0.0;
07348                                 for(k = lbw;k<=lbw+lenw-1;k++)
07349                                         aa = aa+weight(k);
07350 
07351                                 acum=acum+aa;
07352                                 aat=aa/area;
07353                                 }
07354 
07355                         }
07356                         for(i=it;mymax(1,it-numth+1);i--){
07357                         if(fabs(aat-1.0)>0.02){
07358                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07359                                 goto  label2;
07360                                 }
07361                         }
07362                 acum=acum/quadpi/2.0;
07363                 exit(1);
07364 label2:
07365 
07366                 continue;
07367                 }
07368 
07369         free(ts);
07370         free(lband);
07371 
07372         }
07373 
07374         EXITFUNC;
07375 }
07376 
07377 
07378 void Util::angstep(double* thetast,int len){
07379 
07380         ENTERFUNC;
07381 
07382         double t1,t2,tmp;
07383         int i;
07384         if(len>1){
07385                 t1=0;
07386                 for(i=1;i<=len-1;i++){
07387                         tmp=cos(t1)-1.0/((float)len);
07388                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07389                         thetast(i)=t2 * rad_to_deg;
07390                         t1=t2;
07391                 }
07392         }
07393         thetast(len)=90.0;
07394 
07395         EXITFUNC;
07396 }
07397 */
07398 /*
07399 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07400 {
07401 
07402         ENTERFUNC;
07403         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07404         int nt6, n, ier,nout,lnew,mdup,nd;
07405         int i,k,mt,status;
07406 
07407 
07408         double *ds, *x, *y, *z;
07409         double tol=1.0e-8;
07410         double a;
07411 
07412         if(last){
07413                 if(medium>nt)  n = nt+nt;
07414                 else           n = nt+nt-medium+1;
07415         }
07416         else{
07417                 n=nt;
07418         }
07419 
07420         nt6 = n*6;
07421 
07422         list = (int*)calloc(nt6,sizeof(int));
07423         lptr = (int*)calloc(nt6,sizeof(int));
07424         lend = (int*)calloc(n  ,sizeof(int));
07425         iwk  = (int*)calloc(n  ,sizeof(int));
07426         good = (int*)calloc(n  ,sizeof(int));
07427         key  = (int*)calloc(n  ,sizeof(int));
07428         indx = (int*)calloc(n  ,sizeof(int));
07429         lcnt = (int*)calloc(n  ,sizeof(int));
07430 
07431         ds      =       (double*) calloc(n,sizeof(double));
07432         x       =       (double*) calloc(n,sizeof(double));
07433         y       =       (double*) calloc(n,sizeof(double));
07434         z       =       (double*) calloc(n,sizeof(double));
07435 
07436         if (list == NULL ||
07437         lptr == NULL ||
07438         lend == NULL ||
07439         iwk  == NULL ||
07440         good == NULL ||
07441         key  == NULL ||
07442         indx == NULL ||
07443         lcnt == NULL ||
07444         x    == NULL ||
07445         y    == NULL ||
07446         z    == NULL ||
07447         ds   == NULL) {
07448                 printf("memory allocation failure!\n");
07449                 exit(1);
07450         }
07451 
07452 
07453 
07454         for(i = 1;i<=nt;i++){
07455                 x[i-1] = theta(i);
07456                 y[i-1] = phi(i);
07457         }
07458 
07459 
07460 
07461         if (last) {
07462                 for(i=nt+1;i<=n;i++){
07463                         x[i-1]=180.0-x[2*nt-i];
07464                         y[i-1]=180.0+y[2*nt-i];
07465                 }
07466         }
07467 
07468 
07469         Util::disorder2(x,y,key,n);
07470 
07471         Util::ang_to_xyz(x,y,z,n);
07472 
07473 
07474         //  Make sure that first three are no colinear
07475         label1:
07476         for(k=0; k<2; k++){
07477                 for(i=k+1; i<3; i++){
07478                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07479                                 Util::flip23(x, y, z, key, k, n);
07480                                 goto label1;
07481                         }
07482                 }
07483         }
07484 
07485 
07486         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07487 
07488 
07489         if (status != 0) {
07490                 printf(" error in trmsh3 \n");
07491                 exit(1);
07492         }
07493 
07494 
07495         mdup=n-nout;
07496         if (ier == -2) {
07497                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07498                 exit(1);
07499         }
07500         else if (ier > 0) {
07501                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07502                 exit(1);
07503         }
07504 
07505         nd=0;
07506         for (k=1;k<=n;k++){
07507                 if (indx[k-1]>0){
07508                         nd++;
07509                         good[nd-1]=k;
07510                 }
07511         }
07512 
07513 
07514         for(i = 1;i<=nout;i++) {
07515                 k=good[i-1];
07516                 if (key[k-1] >= low && key[k-1]<medium){
07517                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07518                         if (ier != 0){
07519                                 weight[key[k-1]-low] =-1.0;
07520                         }
07521                         else {
07522                                 weight[key[k-1]-low]=a/lcnt[i-1];
07523                         }
07524                 }
07525         }
07526 
07527 // Fill out the duplicated weights
07528         for(i = 1;i<=n;i++){
07529                 mt=-indx[i-1];
07530                 if (mt>0){
07531                         k=good[mt-1];
07532 //  This is a duplicated entry, get the already calculated
07533 //   weight and assign it.
07534                         if (key[i-1]>=low && key[i-1]<medium){
07535 //  Is it already calculated weight??
07536                                 if(key[k-1]>=low && key[k-1]<medium){
07537                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07538                                 }
07539                                 else{
07540 //  No, the weight is from the outside of valid region, calculate it anyway
07541                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07542                                         if (ier != 0){
07543                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07544                                                 weight[key[i-1]-low] =-1.0;
07545                                         }
07546                                         else {
07547                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07548                                         }
07549                                 }
07550                         }
07551                 }
07552         }
07553 
07554 
07555         free(list);
07556         free(lend);
07557         free(iwk);
07558         free(good);
07559         free(key);
07560 
07561         free(indx);
07562         free(lcnt);
07563         free(ds);
07564         free(x);
07565         free(y);
07566         free(z);
07567         EXITFUNC;
07568 }
07569 */
07570 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07571 {
07572 
07573         ENTERFUNC;
07574 
07575         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07576         int nt6, n, ier, nout, lnew, mdup, nd;
07577         int i,k,mt,status;
07578 
07579 
07580         double *ds, *x, *y, *z;
07581         double tol  = 1.0e-8;
07582         double dtol = 15;
07583         double a;
07584 
07585         /*if(last){
07586                 if(medium>nt)  n = nt+nt;
07587                 else           n = nt+nt-medium+1;
07588         }
07589         else{
07590                 n=nt;
07591         }*/
07592 
07593         n = nt + nt;
07594 
07595         nt6 = n*6;
07596 
07597         list = (int*)calloc(nt6,sizeof(int));
07598         lptr = (int*)calloc(nt6,sizeof(int));
07599         lend = (int*)calloc(n  ,sizeof(int));
07600         iwk  = (int*)calloc(n  ,sizeof(int));
07601         good = (int*)calloc(n  ,sizeof(int));
07602         key  = (int*)calloc(n  ,sizeof(int));
07603         indx = (int*)calloc(n  ,sizeof(int));
07604         lcnt = (int*)calloc(n  ,sizeof(int));
07605 
07606         ds      =       (double*) calloc(n,sizeof(double));
07607         x       =       (double*) calloc(n,sizeof(double));
07608         y       =       (double*) calloc(n,sizeof(double));
07609         z       =       (double*) calloc(n,sizeof(double));
07610 
07611         if (list == NULL ||
07612         lptr == NULL ||
07613         lend == NULL ||
07614         iwk  == NULL ||
07615         good == NULL ||
07616         key  == NULL ||
07617         indx == NULL ||
07618         lcnt == NULL ||
07619         x    == NULL ||
07620         y    == NULL ||
07621         z    == NULL ||
07622         ds   == NULL) {
07623                 printf("memory allocation failure!\n");
07624                 exit(1);
07625         }
07626 
07627         bool colinear=true;
07628         while(colinear)
07629         {
07630 
07631         L1:
07632             for(i = 0; i<nt; i++){
07633                 x[i] = theta[i];
07634                 y[i] = phi[i];
07635                 x[nt+i] = 180.0 - x[i];
07636                 y[nt+i] = 180.0 + y[i];
07637             }
07638 
07639             Util::disorder2(x, y, key, n);
07640 
07641             // check if the first three angles are not close, else shuffle
07642             double val;
07643             for(k=0; k<2; k++){
07644                 for(i=k+1; i<3; i++){
07645                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07646                     if( val  < dtol) {
07647                         goto L1;
07648                     }
07649                 }
07650             }
07651 
07652             Util::ang_to_xyz(x, y, z, n);
07653 
07654             //  Make sure that first three has no duplication
07655             bool dupnode=true;
07656             dupnode=true;
07657             while(dupnode)
07658             {
07659                 for(k=0; k<2; k++){
07660                     for(i=k+1; i<3; i++){
07661                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07662                                 Util::flip23(x, y, z, key, k, n);
07663                                 continue;
07664                         }
07665                     }
07666                 }
07667                 dupnode = false;
07668             }
07669 
07670 
07671             ier = 0;
07672 
07673             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07674 
07675             if (status != 0) {
07676                 printf(" error in trmsh3 \n");
07677                 exit(1);
07678             }
07679 
07680             if (ier > 0) {
07681                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07682                 exit(1);
07683             }
07684 
07685             mdup=n-nout;
07686             if (ier == -2) {
07687                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07688             }
07689             else
07690             {
07691                 colinear=false;
07692             }
07693         }
07694 
07695 
07696         Assert( ier != -2 );
07697 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07698 //  INDX contains node numbers from the squeezed list
07699         nd=0;
07700         for (k=1; k<=n; k++){
07701                 if (indx[k-1]>0) {
07702                         nd++;
07703                         good[nd-1]=k;
07704                 }
07705         }
07706 
07707 //
07708 // *** Compute the Voronoi region areas.
07709 //
07710         for(i = 1; i<=nout; i++) {
07711                 k=good[i-1];
07712                 //  We only need n weights from hemisphere
07713                 if (key[k-1] <= nt) {
07714 //  CALCULATE THE AREA
07715                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07716                         if (ier != 0){
07717 //  We set the weight to -1, this will signal the error in the calling
07718 //   program, as the area will turn out incorrect
07719                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07720                                 weight[key[k-1]-1] =-1.0;
07721                         } else {
07722 //  Assign the weight
07723                                 weight[key[k-1]-1]=a/lcnt[i-1];
07724                         }
07725                 }
07726         }
07727 
07728 
07729 // Fill out the duplicated weights
07730         for(i = 1; i<=n; i++){
07731                 mt =- indx[i-1];
07732                 if (mt>0){
07733                         k = good[mt-1];
07734 //  This is a duplicated entry, get the already calculated
07735 //   weight and assign it.
07736                 //  We only need n weights from hemisphere
07737                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07738                         }
07739         }
07740 
07741         free(list);
07742         free(lend);
07743         free(iwk);
07744         free(good);
07745         free(key);
07746         free(lptr);
07747         free(indx);
07748         free(lcnt);
07749         free(ds);
07750         free(x);
07751         free(y);
07752         free(z);
07753 
07754 
07755         EXITFUNC;
07756 }
07757 
07758 void Util::disorder2(double *x,double *y, int *key, int len)
07759 {
07760         ENTERFUNC;
07761         int k, i;
07762         for(i=0; i<len; i++) key[i]=i+1;
07763 
07764         for(i = 0; i<len;i++){
07765                 k = rand()%len;
07766                 std::swap(key[k], key[i]);
07767                 std::swap(x[k], x[i]);
07768                 std::swap(y[k], y[i]);
07769         }
07770         EXITFUNC;
07771 }
07772 
07773 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07774 {
07775         ENTERFUNC;
07776         double costheta,sintheta,cosphi,sinphi;
07777         for(int i = 0;  i<len;  i++)
07778         {
07779                 cosphi = cos(y[i]*dgr_to_rad);
07780                 sinphi = sin(y[i]*dgr_to_rad);
07781                 if(fabs(x[i]-90.0)< 1.0e-5){
07782                         x[i] = cosphi;
07783                         y[i] = sinphi;
07784                         z[i] = 0.0;
07785                 }
07786                 else{
07787                         costheta = cos(x[i]*dgr_to_rad);
07788                         sintheta = sin(x[i]*dgr_to_rad);
07789                         x[i] = cosphi*sintheta;
07790                         y[i] = sinphi*sintheta;
07791                         z[i] = costheta;
07792                 }
07793         }
07794         EXITFUNC;
07795 }
07796 
07797 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07798 {
07799         ENTERFUNC;
07800         int i = k;
07801         while( i == k )  i = rand()%len;
07802         std::swap(key[i], key[k]);
07803         std::swap(x[i], x[k]);
07804         std::swap(y[i], y[k]);
07805         std::swap(z[i], z[k]);
07806         EXITFUNC;
07807 }
07808 
07809 
07810 #undef  mymax
07811 #undef  mymin
07812 #undef  sign
07813 #undef  quadpi
07814 #undef  dgr_to_rad
07815 #undef  deg_to_rad
07816 #undef  rad_to_deg
07817 #undef  rad_to_dgr
07818 #undef  TRUE
07819 #undef  FALSE
07820 #undef  theta
07821 #undef  phi
07822 #undef  weight
07823 #undef  lband
07824 #undef  ts
07825 #undef  thetast
07826 #undef  key
07827 
07828 
07829 /*################################################################################################
07830 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07831 ######   You must link the resulting object file with the libraries: #############################
07832 ####################    -lf2c -lm   (in that order)   ############################################
07833 ################################################################################################*/
07834 
07835 /* Common Block Declarations */
07836 
07837 
07838 #define TRUE_ (1)
07839 #define FALSE_ (0)
07840 #define abs(x) ((x) >= 0 ? (x) : -(x))
07841 
07842 struct stcom_{
07843     double y;
07844 };
07845 stcom_ stcom_1;
07846 #ifdef KR_headers
07847 double floor();
07848 int i_dnnt(x) double *x;
07849 #else
07850 int i_dnnt(double *x)
07851 #endif
07852 {
07853         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07854 }
07855 
07856 
07857 
07858 
07859 /* ____________________STRID______________________________________ */
07860 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07861         double *y, double *z__, int *n, int *list, int *
07862         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07863         int *near__, int *next, double *dist, int *ier)
07864 {
07865     /* System generated locals */
07866     int i__1, i__2;
07867 
07868     /* Local variables */
07869     static double d__;
07870     static int i__, j;
07871     static double d1, d2, d3;
07872     static int i0, lp, kt, ku, lpl, nku;
07873     static int nexti;
07874 
07875 
07876 /* *********************************************************** */
07877 
07878 /*                                              From STRIPACK */
07879 /*                                            Robert J. Renka */
07880 /*                                  Dept. of Computer Science */
07881 /*                                       Univ. of North Texas */
07882 /*                                           renka@cs.unt.edu */
07883 /*                                                   01/20/03 */
07884 
07885 /*   This is an alternative to TRMESH with the inclusion of */
07886 /* an efficient means of removing duplicate or nearly dupli- */
07887 /* cate nodes. */
07888 
07889 /*   This subroutine creates a Delaunay triangulation of a */
07890 /* set of N arbitrarily distributed points, referred to as */
07891 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07892 /* routine TRMESH for definitions and a list of additional */
07893 /* subroutines.  This routine is an alternative to TRMESH */
07894 /* with the inclusion of an efficient means of removing dup- */
07895 /* licate or nearly duplicate nodes. */
07896 
07897 /*   The algorithm has expected time complexity O(N*log(N)) */
07898 /* for random nodal distributions. */
07899 
07900 
07901 /* On input: */
07902 
07903 /*       N0 = Number of nodes, possibly including duplicates. */
07904 /*            N0 .GE. 3. */
07905 
07906 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07907 /*             bound on the deviation from 1 of the cosine of */
07908 /*             the angle between the nodes.  Note that */
07909 /*             |1-cos(A)| is approximately A*A/2. */
07910 
07911 /* The above parameters are not altered by this routine. */
07912 
07913 /*       X,Y,Z = Arrays of length at least N0 containing the */
07914 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07915 /*               Z(K)) is referred to as node K, and K is re- */
07916 /*               ferred to as a nodal index.  It is required */
07917 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
07918 /*               K.  The first three nodes must not be col- */
07919 /*               linear (lie on a common great circle). */
07920 
07921 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
07922 
07923 /*       LEND = Array of length at least N0. */
07924 
07925 /*       INDX = Array of length at least N0. */
07926 
07927 /*       LCNT = Array of length at least N0 (length N is */
07928 /*              sufficient). */
07929 
07930 /*       NEAR,NEXT,DIST = Work space arrays of length at */
07931 /*                        least N0.  The space is used to */
07932 /*                        efficiently determine the nearest */
07933 /*                        triangulation node to each un- */
07934 /*                        processed node for use by ADDNOD. */
07935 
07936 /* On output: */
07937 
07938 /*       N = Number of nodes in the triangulation.  3 .LE. N */
07939 /*           .LE. N0, or N = 0 if IER < 0. */
07940 
07941 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
07942 /*               of the triangulation nodes in the first N */
07943 /*               locations.  The original array elements are */
07944 /*               shifted down as necessary to eliminate dup- */
07945 /*               licate nodes. */
07946 
07947 /*       LIST = Set of nodal indexes which, along with LPTR, */
07948 /*              LEND, and LNEW, define the triangulation as a */
07949 /*              set of N adjacency lists -- counterclockwise- */
07950 /*              ordered sequences of neighboring nodes such */
07951 /*              that the first and last neighbors of a bound- */
07952 /*              ary node are boundary nodes (the first neigh- */
07953 /*              bor of an interior node is arbitrary).  In */
07954 /*              order to distinguish between interior and */
07955 /*              boundary nodes, the last neighbor of each */
07956 /*              boundary node is represented by the negative */
07957 /*              of its index. */
07958 
07959 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
07960 /*              correspondence with the elements of LIST. */
07961 /*              LIST(LPTR(I)) indexes the node which follows */
07962 /*              LIST(I) in cyclical counterclockwise order */
07963 /*              (the first neighbor follows the last neigh- */
07964 /*              bor). */
07965 
07966 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
07967 /*              points to the last neighbor of node K for */
07968 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
07969 /*              only if K is a boundary node. */
07970 
07971 /*       LNEW = Pointer to the first empty location in LIST */
07972 /*              and LPTR (list length plus one).  LIST, LPTR, */
07973 /*              LEND, and LNEW are not altered if IER < 0, */
07974 /*              and are incomplete if IER > 0. */
07975 
07976 /*       INDX = Array of output (triangulation) nodal indexes */
07977 /*              associated with input nodes.  For I = 1 to */
07978 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
07979 /*              the triangulation node with the same (or */
07980 /*              nearly the same) coordinates as input node I. */
07981 
07982 /*       LCNT = Array of int weights (counts) associated */
07983 /*              with the triangulation nodes.  For I = 1 to */
07984 /*              N, LCNT(I) is the number of occurrences of */
07985 /*              node I in the input node set, and thus the */
07986 /*              number of duplicates is LCNT(I)-1. */
07987 
07988 /*       NEAR,NEXT,DIST = Garbage. */
07989 
07990 /*       IER = Error indicator: */
07991 /*             IER =  0 if no errors were encountered. */
07992 /*             IER = -1 if N0 < 3 on input. */
07993 /*             IER = -2 if the first three nodes are */
07994 /*                      collinear. */
07995 /*             IER = -3 if Subroutine ADDNOD returns an error */
07996 /*                      flag.  This should not occur. */
07997 
07998 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
07999 /*                                INSERT, INTADD, JRAND, */
08000 /*                                LEFT, LSTPTR, STORE, SWAP, */
08001 /*                                SWPTST, TRFIND */
08002 
08003 /* Intrinsic function called by TRMSH3:  ABS */
08004 
08005 /* *********************************************************** */
08006 
08007 
08008 /* Local parameters: */
08009 
08010 /* D =        (Negative cosine of) distance from node KT to */
08011 /*              node I */
08012 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
08013 /*              respectively */
08014 /* I,J =      Nodal indexes */
08015 /* I0 =       Index of the node preceding I in a sequence of */
08016 /*              unprocessed nodes:  I = NEXT(I0) */
08017 /* KT =       Index of a triangulation node */
08018 /* KU =       Index of an unprocessed node and DO-loop index */
08019 /* LP =       LIST index (pointer) of a neighbor of KT */
08020 /* LPL =      Pointer to the last neighbor of KT */
08021 /* NEXTI =    NEXT(I) */
08022 /* NKU =      NEAR(KU) */
08023 
08024     /* Parameter adjustments */
08025     --dist;
08026     --next;
08027     --near__;
08028     --indx;
08029     --lend;
08030     --z__;
08031     --y;
08032     --x;
08033     --list;
08034     --lptr;
08035     --lcnt;
08036 
08037     /* Function Body */
08038     if (*n0 < 3) {
08039         *n = 0;
08040         *ier = -1;
08041         return 0;
08042     }
08043 
08044 /* Store the first triangle in the linked list. */
08045 
08046     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08047             z__[3])) {
08048 
08049 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
08050 
08051         list[1] = 3;
08052         lptr[1] = 2;
08053         list[2] = -2;
08054         lptr[2] = 1;
08055         lend[1] = 2;
08056 
08057         list[3] = 1;
08058         lptr[3] = 4;
08059         list[4] = -3;
08060         lptr[4] = 3;
08061         lend[2] = 4;
08062 
08063         list[5] = 2;
08064         lptr[5] = 6;
08065         list[6] = -1;
08066         lptr[6] = 5;
08067         lend[3] = 6;
08068 
08069     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08070             y[3], &z__[3])) {
08071 
08072 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
08073 /*     i.e., node 3 lies in the left hemisphere defined by */
08074 /*     arc 1->2. */
08075 
08076         list[1] = 2;
08077         lptr[1] = 2;
08078         list[2] = -3;
08079         lptr[2] = 1;
08080         lend[1] = 2;
08081 
08082         list[3] = 3;
08083         lptr[3] = 4;
08084         list[4] = -1;
08085         lptr[4] = 3;
08086         lend[2] = 4;
08087 
08088         list[5] = 1;
08089         lptr[5] = 6;
08090         list[6] = -2;
08091         lptr[6] = 5;
08092         lend[3] = 6;
08093 
08094 
08095     } else {
08096 
08097 /*   The first three nodes are collinear. */
08098 
08099         *n = 0;
08100         *ier = -2;
08101         return 0;
08102     }
08103 
08104     //printf("pass check colinear\n");
08105 
08106 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08107 
08108     *lnew = 7;
08109     indx[1] = 1;
08110     indx[2] = 2;
08111     indx[3] = 3;
08112     lcnt[1] = 1;
08113     lcnt[2] = 1;
08114     lcnt[3] = 1;
08115     if (*n0 == 3) {
08116         *n = 3;
08117         *ier = 0;
08118         return 0;
08119     }
08120 
08121 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08122 /*   used to obtain an expected-time (N*log(N)) incremental */
08123 /*   algorithm by enabling constant search time for locating */
08124 /*   each new node in the triangulation. */
08125 
08126 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08127 /*   triangulation node closest to KU (used as the starting */
08128 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08129 /*   is an increasing function of the arc length (angular */
08130 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08131 /*   arc length a. */
08132 
08133 /* Since it is necessary to efficiently find the subset of */
08134 /*   unprocessed nodes associated with each triangulation */
08135 /*   node J (those that have J as their NEAR entries), the */
08136 /*   subsets are stored in NEAR and NEXT as follows:  for */
08137 /*   each node J in the triangulation, I = NEAR(J) is the */
08138 /*   first unprocessed node in J's set (with I = 0 if the */
08139 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08140 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08141 /*   set are initially ordered by increasing indexes (which */
08142 /*   maximizes efficiency) but that ordering is not main- */
08143 /*   tained as the data structure is updated. */
08144 
08145 /* Initialize the data structure for the single triangle. */
08146 
08147     near__[1] = 0;
08148     near__[2] = 0;
08149     near__[3] = 0;
08150     for (ku = *n0; ku >= 4; --ku) {
08151         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08152         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08153         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08154         if (d1 <= d2 && d1 <= d3) {
08155             near__[ku] = 1;
08156             dist[ku] = d1;
08157             next[ku] = near__[1];
08158             near__[1] = ku;
08159         } else if (d2 <= d1 && d2 <= d3) {
08160             near__[ku] = 2;
08161             dist[ku] = d2;
08162             next[ku] = near__[2];
08163             near__[2] = ku;
08164         } else {
08165             near__[ku] = 3;
08166             dist[ku] = d3;
08167             next[ku] = near__[3];
08168             near__[3] = ku;
08169         }
08170 /* L1: */
08171     }
08172 
08173 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08174 /*   in the triangulation, and NKU = NEAR(KU). */
08175 
08176     kt = 3;
08177     i__1 = *n0;
08178     for (ku = 4; ku <= i__1; ++ku) {
08179         nku = near__[ku];
08180 
08181 /* Remove KU from the set of unprocessed nodes associated */
08182 /*   with NEAR(KU). */
08183         i__ = nku;
08184         if (near__[i__] == ku) {
08185             near__[i__] = next[ku];
08186         } else {
08187             i__ = near__[i__];
08188 L2:
08189             i0 = i__;
08190             i__ = next[i0];
08191             if (i__ != ku) {
08192                 goto L2;
08193             }
08194             next[i0] = next[ku];
08195         }
08196         near__[ku] = 0;
08197 
08198 /* Bypass duplicate nodes. */
08199 
08200         if (dist[ku] <= *tol - 1.) {
08201             indx[ku] = -nku;
08202             ++lcnt[nku];
08203             goto L6;
08204         }
08205 
08206 
08207 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08208         ++kt;
08209         x[kt] = x[ku];
08210         y[kt] = y[ku];
08211         z__[kt] = z__[ku];
08212         indx[ku] = kt;
08213         lcnt[kt] = 1;
08214         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08215                 , lnew, ier);
08216         if (*ier != 0) {
08217             *n = 0;
08218             *ier = -3;
08219             return 0;
08220         }
08221 
08222 /* Loop on neighbors J of node KT. */
08223 
08224         lpl = lend[kt];
08225         lp = lpl;
08226 L3:
08227         lp = lptr[lp];
08228         j = (i__2 = list[lp], abs(i__2));
08229 
08230 /* Loop on elements I in the sequence of unprocessed nodes */
08231 /*   associated with J:  KT is a candidate for replacing J */
08232 /*   as the nearest triangulation node to I.  The next value */
08233 /*   of I in the sequence, NEXT(I), must be saved before I */
08234 /*   is moved because it is altered by adding I to KT's set. */
08235 
08236         i__ = near__[j];
08237 L4:
08238         if (i__ == 0) {
08239             goto L5;
08240         }
08241         nexti = next[i__];
08242 
08243 /* Test for the distance from I to KT less than the distance */
08244 /*   from I to J. */
08245 
08246         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08247         if (d__ < dist[i__]) {
08248 
08249 /* Replace J by KT as the nearest triangulation node to I: */
08250 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08251 /*   of unprocessed nodes and add it to KT's set. */
08252 
08253             near__[i__] = kt;
08254             dist[i__] = d__;
08255             if (i__ == near__[j]) {
08256                 near__[j] = nexti;
08257             } else {
08258                 next[i0] = nexti;
08259             }
08260             next[i__] = near__[kt];
08261             near__[kt] = i__;
08262         } else {
08263             i0 = i__;
08264         }
08265 
08266 /* Bottom of loop on I. */
08267 
08268         i__ = nexti;
08269         goto L4;
08270 
08271 /* Bottom of loop on neighbors J. */
08272 
08273 L5:
08274         if (lp != lpl) {
08275             goto L3;
08276         }
08277 L6:
08278         ;
08279     }
08280     *n = kt;
08281     *ier = 0;
08282     return 0;
08283 } /* trmsh3_ */
08284 
08285 /* stripack.dbl sent by Robert on 06/03/03 */
08286 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08287         double *y, double *z__, int *list, int *lptr, int
08288         *lend, int *lnew, int *ier)
08289 {
08290     /* Initialized data */
08291 
08292     static double tol = 0.;
08293 
08294     /* System generated locals */
08295     int i__1;
08296 
08297     /* Local variables */
08298     static int l;
08299     static double p[3], b1, b2, b3;
08300     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08301     /* Subroutine */ int swap_(int *, int *, int *,
08302             int *, int *, int *, int *, int *);
08303     static int lpo1s;
08304     /* Subroutine */ int bdyadd_(int *, int *, int *,
08305             int *, int *, int *, int *), intadd_(int *,
08306             int *, int *, int *, int *, int *, int *,
08307             int *), trfind_(int *, double *, int *,
08308             double *, double *, double *, int *, int *,
08309             int *, double *, double *, double *, int *,
08310             int *, int *), covsph_(int *, int *, int *,
08311             int *, int *, int *);
08312     int lstptr_(int *, int *, int *, int *);
08313     long int swptst_(int *, int *, int *, int *,
08314             double *, double *, double *);
08315 
08316 
08317 /* *********************************************************** */
08318 
08319 /*                                              From STRIPACK */
08320 /*                                            Robert J. Renka */
08321 /*                                  Dept. of Computer Science */
08322 /*                                       Univ. of North Texas */
08323 /*                                           renka@cs.unt.edu */
08324 /*                                                   01/08/03 */
08325 
08326 /*   This subroutine adds node K to a triangulation of the */
08327 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08328 /* of the convex hull of nodes 1,...,K. */
08329 
08330 /*   The algorithm consists of the following steps:  node K */
08331 /* is located relative to the triangulation (TRFIND), its */
08332 /* index is added to the data structure (INTADD or BDYADD), */
08333 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08334 /* the arcs opposite K so that all arcs incident on node K */
08335 /* and opposite node K are locally optimal (satisfy the cir- */
08336 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08337 /* input, a Delaunay triangulation will result. */
08338 
08339 
08340 /* On input: */
08341 
08342 /*       NST = Index of a node at which TRFIND begins its */
08343 /*             search.  Search time depends on the proximity */
08344 /*             of this node to K.  If NST < 1, the search is */
08345 /*             begun at node K-1. */
08346 
08347 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08348 /*           new node to be added.  K .GE. 4. */
08349 
08350 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08351 /*               tesian coordinates of the nodes. */
08352 /*               (X(I),Y(I),Z(I)) defines node I for */
08353 /*               I = 1,...,K. */
08354 
08355 /* The above parameters are not altered by this routine. */
08356 
08357 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08358 /*                             the triangulation of nodes 1 */
08359 /*                             to K-1.  The array lengths are */
08360 /*                             assumed to be large enough to */
08361 /*                             add node K.  Refer to Subrou- */
08362 /*                             tine TRMESH. */
08363 
08364 /* On output: */
08365 
08366 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08367 /*                             the addition of node K as the */
08368 /*                             last entry unless IER .NE. 0 */
08369 /*                             and IER .NE. -3, in which case */
08370 /*                             the arrays are not altered. */
08371 
08372 /*       IER = Error indicator: */
08373 /*             IER =  0 if no errors were encountered. */
08374 /*             IER = -1 if K is outside its valid range */
08375 /*                      on input. */
08376 /*             IER = -2 if all nodes (including K) are col- */
08377 /*                      linear (lie on a common geodesic). */
08378 /*             IER =  L if nodes L and K coincide for some */
08379 /*                      L < K.  Refer to TOL below. */
08380 
08381 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08382 /*                                INTADD, JRAND, LSTPTR, */
08383 /*                                STORE, SWAP, SWPTST, */
08384 /*                                TRFIND */
08385 
08386 /* Intrinsic function called by ADDNOD:  ABS */
08387 
08388 /* *********************************************************** */
08389 
08390 
08391 /* Local parameters: */
08392 
08393 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08394 /*              by TRFIND. */
08395 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08396 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08397 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08398 /*              counterclockwise order. */
08399 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08400 /*              be tested for a swap */
08401 /* IST =      Index of node at which TRFIND begins its search */
08402 /* KK =       Local copy of K */
08403 /* KM1 =      K-1 */
08404 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08405 /*              if node K coincides with a vertex */
08406 /* LP =       LIST pointer */
08407 /* LPF =      LIST pointer to the first neighbor of K */
08408 /* LPO1 =     LIST pointer to IO1 */
08409 /* LPO1S =    Saved value of LPO1 */
08410 /* P =        Cartesian coordinates of node K */
08411 /* TOL =      Tolerance defining coincident nodes:  bound on */
08412 /*              the deviation from 1 of the cosine of the */
08413 /*              angle between the nodes. */
08414 /*              Note that |1-cos(A)| is approximately A*A/2. */
08415 
08416     /* Parameter adjustments */
08417     --lend;
08418     --z__;
08419     --y;
08420     --x;
08421     --list;
08422     --lptr;
08423 
08424     /* Function Body */
08425 
08426     kk = *k;
08427     if (kk < 4) {
08428         goto L3;
08429     }
08430 
08431 /* Initialization: */
08432     km1 = kk - 1;
08433     ist = *nst;
08434     if (ist < 1) {
08435         ist = km1;
08436     }
08437     p[0] = x[kk];
08438     p[1] = y[kk];
08439     p[2] = z__[kk];
08440 
08441 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08442 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08443 /*   from node K. */
08444     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08445             , &b1, &b2, &b3, &i1, &i2, &i3);
08446 
08447 /*   Test for collinear or (nearly) duplicate nodes. */
08448 
08449     if (i1 == 0) {
08450         goto L4;
08451     }
08452     l = i1;
08453     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08454         goto L5;
08455     }
08456     l = i2;
08457     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08458         goto L5;
08459     }
08460     if (i3 != 0) {
08461         l = i3;
08462         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08463             goto L5;
08464         }
08465         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08466     } else {
08467         if (i1 != i2) {
08468             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08469         } else {
08470             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08471         }
08472     }
08473     *ier = 0;
08474 
08475 /* Initialize variables for optimization of the */
08476 /*   triangulation. */
08477     lp = lend[kk];
08478     lpf = lptr[lp];
08479     io2 = list[lpf];
08480     lpo1 = lptr[lpf];
08481     io1 = (i__1 = list[lpo1], abs(i__1));
08482 
08483 /* Begin loop:  find the node opposite K. */
08484 
08485 L1:
08486     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08487     if (list[lp] < 0) {
08488         goto L2;
08489     }
08490     lp = lptr[lp];
08491     in1 = (i__1 = list[lp], abs(i__1));
08492 
08493 /* Swap test:  if a swap occurs, two new arcs are */
08494 /*             opposite K and must be tested. */
08495 
08496     lpo1s = lpo1;
08497     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08498         goto L2;
08499     }
08500     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08501     if (lpo1 == 0) {
08502 
08503 /*   A swap is not possible because KK and IN1 are already */
08504 /*     adjacent.  This error in SWPTST only occurs in the */
08505 /*     neutral case and when there are nearly duplicate */
08506 /*     nodes. */
08507 
08508         lpo1 = lpo1s;
08509         goto L2;
08510     }
08511     io1 = in1;
08512     goto L1;
08513 
08514 /* No swap occurred.  Test for termination and reset */
08515 /*   IO2 and IO1. */
08516 
08517 L2:
08518     if (lpo1 == lpf || list[lpo1] < 0) {
08519         return 0;
08520     }
08521     io2 = io1;
08522     lpo1 = lptr[lpo1];
08523     io1 = (i__1 = list[lpo1], abs(i__1));
08524     goto L1;
08525 
08526 /* KK < 4. */
08527 
08528 L3:
08529     *ier = -1;
08530     return 0;
08531 
08532 /* All nodes are collinear. */
08533 
08534 L4:
08535     *ier = -2;
08536     return 0;
08537 
08538 /* Nodes L and K coincide. */
08539 
08540 L5:
08541     *ier = l;
08542     return 0;
08543 } /* addnod_ */
08544 
08545 double angle_(double *v1, double *v2, double *v3)
08546 {
08547     /* System generated locals */
08548     double ret_val;
08549 
08550     /* Builtin functions */
08551     //double sqrt(double), acos(double);
08552 
08553     /* Local variables */
08554     static double a;
08555     static int i__;
08556     static double ca, s21, s23, u21[3], u23[3];
08557 
08558 
08559 /* *********************************************************** */
08560 
08561 /*                                              From STRIPACK */
08562 /*                                            Robert J. Renka */
08563 /*                                  Dept. of Computer Science */
08564 /*                                       Univ. of North Texas */
08565 /*                                           renka@cs.unt.edu */
08566 /*                                                   06/03/03 */
08567 
08568 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08569 /* face of the unit sphere, this function returns the */
08570 /* interior angle at V2 -- the dihedral angle between the */
08571 /* plane defined by V2 and V3 (and the origin) and the plane */
08572 /* defined by V2 and V1 or, equivalently, the angle between */
08573 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08574 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08575 /* wise.  The surface area of a spherical polygon with CCW- */
08576 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08577 /* Asum is the sum of the m interior angles computed from the */
08578 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08579 /* (Vm-1,Vm,V1). */
08580 
08581 
08582 /* On input: */
08583 
08584 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08585 /*                  sian coordinates of unit vectors.  These */
08586 /*                  vectors, if nonzero, are implicitly */
08587 /*                  scaled to have length 1. */
08588 
08589 /* Input parameters are not altered by this function. */
08590 
08591 /* On output: */
08592 
08593 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08594 /*               V2 X V3 = 0. */
08595 
08596 /* Module required by ANGLE:  LEFT */
08597 
08598 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08599 
08600 /* *********************************************************** */
08601 
08602 
08603 /* Local parameters: */
08604 
08605 /* A =       Interior angle at V2 */
08606 /* CA =      cos(A) */
08607 /* I =       DO-loop index and index for U21 and U23 */
08608 /* S21,S23 = Sum of squared components of U21 and U23 */
08609 /* U21,U23 = Unit normal vectors to the planes defined by */
08610 /*             pairs of triangle vertices */
08611 
08612 
08613 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08614 
08615     /* Parameter adjustments */
08616     --v3;
08617     --v2;
08618     --v1;
08619 
08620     /* Function Body */
08621     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08622     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08623     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08624 
08625     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08626     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08627     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08628 
08629 /* Normalize U21 and U23 to unit vectors. */
08630 
08631     s21 = 0.;
08632     s23 = 0.;
08633     for (i__ = 1; i__ <= 3; ++i__) {
08634         s21 += u21[i__ - 1] * u21[i__ - 1];
08635         s23 += u23[i__ - 1] * u23[i__ - 1];
08636 /* L1: */
08637     }
08638 
08639 /* Test for a degenerate triangle associated with collinear */
08640 /*   vertices. */
08641 
08642     if (s21 == 0. || s23 == 0.) {
08643         ret_val = 0.;
08644         return ret_val;
08645     }
08646     s21 = sqrt(s21);
08647     s23 = sqrt(s23);
08648     for (i__ = 1; i__ <= 3; ++i__) {
08649         u21[i__ - 1] /= s21;
08650         u23[i__ - 1] /= s23;
08651 /* L2: */
08652     }
08653 
08654 /* Compute the angle A between normals: */
08655 
08656 /*   CA = cos(A) = <U21,U23> */
08657 
08658     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08659     if (ca < -1.) {
08660         ca = -1.;
08661     }
08662     if (ca > 1.) {
08663         ca = 1.;
08664     }
08665     a = acos(ca);
08666 
08667 /* Adjust A to the interior angle:  A > Pi iff */
08668 /*   V3 Right V1->V2. */
08669 
08670     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08671             , &v3[3])) {
08672         a = acos(-1.) * 2. - a;
08673     }
08674     ret_val = a;
08675     return ret_val;
08676 } /* angle_ */
08677 
08678 double areas_(double *v1, double *v2, double *v3)
08679 {
08680     /* System generated locals */
08681     double ret_val;
08682 
08683     /* Builtin functions */
08684     //double sqrt(double), acos(double);
08685 
08686     /* Local variables */
08687     static int i__;
08688     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08689             ca2, ca3;
08690 
08691 
08692 /* *********************************************************** */
08693 
08694 /*                                              From STRIPACK */
08695 /*                                            Robert J. Renka */
08696 /*                                  Dept. of Computer Science */
08697 /*                                       Univ. of North Texas */
08698 /*                                           renka@cs.unt.edu */
08699 /*                                                   06/22/98 */
08700 
08701 /*   This function returns the area of a spherical triangle */
08702 /* on the unit sphere. */
08703 
08704 
08705 /* On input: */
08706 
08707 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08708 /*                  sian coordinates of unit vectors (the */
08709 /*                  three triangle vertices in any order). */
08710 /*                  These vectors, if nonzero, are implicitly */
08711 /*                  scaled to have length 1. */
08712 
08713 /* Input parameters are not altered by this function. */
08714 
08715 /* On output: */
08716 
08717 /*       AREAS = Area of the spherical triangle defined by */
08718 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08719 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08720 /*               if and only if V1, V2, and V3 lie in (or */
08721 /*               close to) a plane containing the origin. */
08722 
08723 /* Modules required by AREAS:  None */
08724 
08725 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08726 
08727 /* *********************************************************** */
08728 
08729 
08730 /* Local parameters: */
08731 
08732 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08733 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08734 /* I =           DO-loop index and index for Uij */
08735 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08736 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08737 /*                 pairs of triangle vertices */
08738 
08739 
08740 /* Compute cross products Uij = Vi X Vj. */
08741 
08742     /* Parameter adjustments */
08743     --v3;
08744     --v2;
08745     --v1;
08746 
08747     /* Function Body */
08748     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08749     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08750     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08751 
08752     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08753     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08754     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08755 
08756     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08757     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08758     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08759 
08760 /* Normalize Uij to unit vectors. */
08761 
08762     s12 = 0.;
08763     s23 = 0.;
08764     s31 = 0.;
08765     for (i__ = 1; i__ <= 3; ++i__) {
08766         s12 += u12[i__ - 1] * u12[i__ - 1];
08767         s23 += u23[i__ - 1] * u23[i__ - 1];
08768         s31 += u31[i__ - 1] * u31[i__ - 1];
08769 /* L2: */
08770     }
08771 
08772 /* Test for a degenerate triangle associated with collinear */
08773 /*   vertices. */
08774 
08775     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08776         ret_val = 0.;
08777         return ret_val;
08778     }
08779     s12 = sqrt(s12);
08780     s23 = sqrt(s23);
08781     s31 = sqrt(s31);
08782     for (i__ = 1; i__ <= 3; ++i__) {
08783         u12[i__ - 1] /= s12;
08784         u23[i__ - 1] /= s23;
08785         u31[i__ - 1] /= s31;
08786 /* L3: */
08787     }
08788 
08789 /* Compute interior angles Ai as the dihedral angles between */
08790 /*   planes: */
08791 /*           CA1 = cos(A1) = -<U12,U31> */
08792 /*           CA2 = cos(A2) = -<U23,U12> */
08793 /*           CA3 = cos(A3) = -<U31,U23> */
08794 
08795     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08796     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08797     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08798     if (ca1 < -1.) {
08799         ca1 = -1.;
08800     }
08801     if (ca1 > 1.) {
08802         ca1 = 1.;
08803     }
08804     if (ca2 < -1.) {
08805         ca2 = -1.;
08806     }
08807     if (ca2 > 1.) {
08808         ca2 = 1.;
08809     }
08810     if (ca3 < -1.) {
08811         ca3 = -1.;
08812     }
08813     if (ca3 > 1.) {
08814         ca3 = 1.;
08815     }
08816     a1 = acos(ca1);
08817     a2 = acos(ca2);
08818     a3 = acos(ca3);
08819 
08820 /* Compute AREAS = A1 + A2 + A3 - PI. */
08821 
08822     ret_val = a1 + a2 + a3 - acos(-1.);
08823     if (ret_val < 0.) {
08824         ret_val = 0.;
08825     }
08826     return ret_val;
08827 } /* areas_ */
08828 
08829 //double areas_(double *, double *, double *);
08830 
08831 double Util::areav_(int *k, int *n, double *x, double *y,
08832         double *z__, int *list, int *lptr, int *lend, int
08833         *ier)
08834 {
08835     /* Initialized data */
08836 
08837     static double amax = 6.28;
08838 
08839     /* System generated locals */
08840     double ret_val;
08841 
08842     /* Local variables */
08843     static double a, c0[3], c2[3], c3[3];
08844     static int n1, n2, n3;
08845     static double v1[3], v2[3], v3[3];
08846     static int lp, lpl, ierr;
08847     static double asum;
08848     static long int first;
08849 
08850 
08851 /* *********************************************************** */
08852 
08853 /*                                            Robert J. Renka */
08854 /*                                  Dept. of Computer Science */
08855 /*                                       Univ. of North Texas */
08856 /*                                           renka@cs.unt.edu */
08857 /*                                                   10/25/02 */
08858 
08859 /*   Given a Delaunay triangulation and the index K of an */
08860 /* interior node, this subroutine returns the (surface) area */
08861 /* of the Voronoi region associated with node K.  The Voronoi */
08862 /* region is the polygon whose vertices are the circumcenters */
08863 /* of the triangles that contain node K, where a triangle */
08864 /* circumcenter is the point (unit vector) lying at the same */
08865 /* angular distance from the three vertices and contained in */
08866 /* the same hemisphere as the vertices. */
08867 
08868 
08869 /* On input: */
08870 
08871 /*       K = Nodal index in the range 1 to N. */
08872 
08873 /*       N = Number of nodes in the triangulation.  N > 3. */
08874 
08875 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08876 /*               coordinates of the nodes (unit vectors). */
08877 
08878 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08879 /*                        gulation.  Refer to Subroutine */
08880 /*                        TRMESH. */
08881 
08882 /* Input parameters are not altered by this function. */
08883 
08884 /* On output: */
08885 
08886 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08887 /*               in which case AREAV = 0. */
08888 
08889 /*       IER = Error indicator: */
08890 /*             IER = 0 if no errors were encountered. */
08891 /*             IER = 1 if K or N is outside its valid range */
08892 /*                     on input. */
08893 /*             IER = 2 if K indexes a boundary node. */
08894 /*             IER = 3 if an error flag is returned by CIRCUM */
08895 /*                     (null triangle). */
08896 /*             IER = 4 if AREAS returns a value greater than */
08897 /*                     AMAX (defined below). */
08898 
08899 /* Modules required by AREAV:  AREAS, CIRCUM */
08900 
08901 /* *********************************************************** */
08902 
08903 
08904 /* Maximum valid triangle area is less than 2*Pi: */
08905 
08906     /* Parameter adjustments */
08907     --lend;
08908     --z__;
08909     --y;
08910     --x;
08911     --list;
08912     --lptr;
08913 
08914     /* Function Body */
08915 
08916 /* Test for invalid input. */
08917 
08918     if (*k < 1 || *k > *n || *n <= 3) {
08919         goto L11;
08920     }
08921 
08922 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08923 /*   FIRST = TRUE only for the first triangle. */
08924 /*   The Voronoi region area is accumulated in ASUM. */
08925 
08926     n1 = *k;
08927     v1[0] = x[n1];
08928     v1[1] = y[n1];
08929     v1[2] = z__[n1];
08930     lpl = lend[n1];
08931     n3 = list[lpl];
08932     if (n3 < 0) {
08933         goto L12;
08934     }
08935     lp = lpl;
08936     first = TRUE_;
08937     asum = 0.;
08938 
08939 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08940 
08941 L1:
08942     n2 = n3;
08943     lp = lptr[lp];
08944     n3 = list[lp];
08945     v2[0] = x[n2];
08946     v2[1] = y[n2];
08947     v2[2] = z__[n2];
08948     v3[0] = x[n3];
08949     v3[1] = y[n3];
08950     v3[2] = z__[n3];
08951     if (first) {
08952 
08953 /* First triangle:  compute the circumcenter C3 and save a */
08954 /*   copy in C0. */
08955 
08956         circum_(v1, v2, v3, c3, &ierr);
08957         if (ierr != 0) {
08958             goto L13;
08959         }
08960         c0[0] = c3[0];
08961         c0[1] = c3[1];
08962         c0[2] = c3[2];
08963         first = FALSE_;
08964     } else {
08965 
08966 /* Set C2 to C3, compute the new circumcenter C3, and compute */
08967 /*   the area A of triangle (V1,C2,C3). */
08968 
08969         c2[0] = c3[0];
08970         c2[1] = c3[1];
08971         c2[2] = c3[2];
08972         circum_(v1, v2, v3, c3, &ierr);
08973         if (ierr != 0) {
08974             goto L13;
08975         }
08976         a = areas_(v1, c2, c3);
08977         if (a > amax) {
08978             goto L14;
08979         }
08980         asum += a;
08981     }
08982 
08983 /* Bottom on loop on neighbors of K. */
08984 
08985     if (lp != lpl) {
08986         goto L1;
08987     }
08988 
08989 /* Compute the area of triangle (V1,C3,C0). */
08990 
08991     a = areas_(v1, c3, c0);
08992     if (a > amax) {
08993         goto L14;
08994     }
08995     asum += a;
08996 
08997 /* No error encountered. */
08998 
08999     *ier = 0;
09000     ret_val = asum;
09001     return ret_val;
09002 
09003 /* Invalid input. */
09004 
09005 L11:
09006     *ier = 1;
09007     ret_val = 0.;
09008     return ret_val;
09009 
09010 /* K indexes a boundary node. */
09011 
09012 L12:
09013     *ier = 2;
09014     ret_val = 0.;
09015     return ret_val;
09016 
09017 /* Error in CIRCUM. */
09018 
09019 L13:
09020     *ier = 3;
09021     ret_val = 0.;
09022     return ret_val;
09023 
09024 /* AREAS value larger than AMAX. */
09025 
09026 L14:
09027     *ier = 4;
09028     ret_val = 0.;
09029     return ret_val;
09030 } /* areav_ */
09031 
09032 double areav_new__(int *k, int *n, double *x, double *y,
09033         double *z__, int *list, int *lptr, int *lend, int
09034         *ier)
09035 {
09036     /* System generated locals */
09037     double ret_val = 0;
09038 
09039     /* Builtin functions */
09040     //double acos(double);
09041 
09042     /* Local variables */
09043     static int m;
09044     static double c1[3], c2[3], c3[3];
09045     static int n1, n2, n3;
09046     static double v1[3], v2[3], v3[3];
09047     static int lp;
09048     static double c1s[3], c2s[3];
09049     static int lpl, ierr;
09050     static double asum;
09051     double angle_(double *, double *, double *);
09052     static float areav;
09053 
09054 
09055 /* *********************************************************** */
09056 
09057 /*                                            Robert J. Renka */
09058 /*                                  Dept. of Computer Science */
09059 /*                                       Univ. of North Texas */
09060 /*                                           renka@cs.unt.edu */
09061 /*                                                   06/03/03 */
09062 
09063 /*   Given a Delaunay triangulation and the index K of an */
09064 /* interior node, this subroutine returns the (surface) area */
09065 /* of the Voronoi region associated with node K.  The Voronoi */
09066 /* region is the polygon whose vertices are the circumcenters */
09067 /* of the triangles that contain node K, where a triangle */
09068 /* circumcenter is the point (unit vector) lying at the same */
09069 /* angular distance from the three vertices and contained in */
09070 /* the same hemisphere as the vertices.  The Voronoi region */
09071 /* area is computed as Asum-(m-2)*Pi, where m is the number */
09072 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
09073 /* of interior angles at the vertices. */
09074 
09075 
09076 /* On input: */
09077 
09078 /*       K = Nodal index in the range 1 to N. */
09079 
09080 /*       N = Number of nodes in the triangulation.  N > 3. */
09081 
09082 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09083 /*               coordinates of the nodes (unit vectors). */
09084 
09085 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09086 /*                        gulation.  Refer to Subroutine */
09087 /*                        TRMESH. */
09088 
09089 /* Input parameters are not altered by this function. */
09090 
09091 /* On output: */
09092 
09093 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09094 /*               in which case AREAV = 0. */
09095 
09096 /*       IER = Error indicator: */
09097 /*             IER = 0 if no errors were encountered. */
09098 /*             IER = 1 if K or N is outside its valid range */
09099 /*                     on input. */
09100 /*             IER = 2 if K indexes a boundary node. */
09101 /*             IER = 3 if an error flag is returned by CIRCUM */
09102 /*                     (null triangle). */
09103 
09104 /* Modules required by AREAV:  ANGLE, CIRCUM */
09105 
09106 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09107 
09108 /* *********************************************************** */
09109 
09110 
09111 /* Test for invalid input. */
09112 
09113     /* Parameter adjustments */
09114     --lend;
09115     --z__;
09116     --y;
09117     --x;
09118     --list;
09119     --lptr;
09120 
09121     /* Function Body */
09122     if (*k < 1 || *k > *n || *n <= 3) {
09123         goto L11;
09124     }
09125 
09126 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09127 /*   The number of neighbors and the sum of interior angles */
09128 /*   are accumulated in M and ASUM, respectively. */
09129 
09130     n1 = *k;
09131     v1[0] = x[n1];
09132     v1[1] = y[n1];
09133     v1[2] = z__[n1];
09134     lpl = lend[n1];
09135     n3 = list[lpl];
09136     if (n3 < 0) {
09137         goto L12;
09138     }
09139     lp = lpl;
09140     m = 0;
09141     asum = 0.;
09142 
09143 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09144 
09145 L1:
09146     ++m;
09147     n2 = n3;
09148     lp = lptr[lp];
09149     n3 = list[lp];
09150     v2[0] = x[n2];
09151     v2[1] = y[n2];
09152     v2[2] = z__[n2];
09153     v3[0] = x[n3];
09154     v3[1] = y[n3];
09155     v3[2] = z__[n3];
09156     if (m == 1) {
09157 
09158 /* First triangle:  compute the circumcenter C2 and save a */
09159 /*   copy in C1S. */
09160 
09161         circum_(v1, v2, v3, c2, &ierr);
09162         if (ierr != 0) {
09163             goto L13;
09164         }
09165         c1s[0] = c2[0];
09166         c1s[1] = c2[1];
09167         c1s[2] = c2[2];
09168     } else if (m == 2) {
09169 
09170 /* Second triangle:  compute the circumcenter C3 and save a */
09171 /*   copy in C2S. */
09172 
09173         circum_(v1, v2, v3, c3, &ierr);
09174         if (ierr != 0) {
09175             goto L13;
09176         }
09177         c2s[0] = c3[0];
09178         c2s[1] = c3[1];
09179         c2s[2] = c3[2];
09180     } else {
09181 
09182 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09183 /*   C3, and compute the interior angle at C2 from the */
09184 /*   sequence of vertices (C1,C2,C3). */
09185 
09186         c1[0] = c2[0];
09187         c1[1] = c2[1];
09188         c1[2] = c2[2];
09189         c2[0] = c3[0];
09190         c2[1] = c3[1];
09191         c2[2] = c3[2];
09192         circum_(v1, v2, v3, c3, &ierr);
09193         if (ierr != 0) {
09194             goto L13;
09195         }
09196         asum += angle_(c1, c2, c3);
09197     }
09198 
09199 /* Bottom on loop on neighbors of K. */
09200 
09201     if (lp != lpl) {
09202         goto L1;
09203     }
09204 
09205 /* C3 is the last vertex.  Compute its interior angle from */
09206 /*   the sequence (C2,C3,C1S). */
09207 
09208     asum += angle_(c2, c3, c1s);
09209 
09210 /* Compute the interior angle at C1S from */
09211 /*   the sequence (C3,C1S,C2S). */
09212 
09213     asum += angle_(c3, c1s, c2s);
09214 
09215 /* No error encountered. */
09216 
09217     *ier = 0;
09218     ret_val = asum - (double) (m - 2) * acos(-1.);
09219     return ret_val;
09220 
09221 /* Invalid input. */
09222 
09223 L11:
09224     *ier = 1;
09225     areav = 0.f;
09226     return ret_val;
09227 
09228 /* K indexes a boundary node. */
09229 
09230 L12:
09231     *ier = 2;
09232     areav = 0.f;
09233     return ret_val;
09234 
09235 /* Error in CIRCUM. */
09236 
09237 L13:
09238     *ier = 3;
09239     areav = 0.f;
09240     return ret_val;
09241 } /* areav_new__ */
09242 
09243 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09244         list, int *lptr, int *lend, int *lnew)
09245 {
09246     static int k, n1, n2, lp, lsav, nsav, next;
09247     /* Subroutine */ int insert_(int *, int *, int *,
09248             int *, int *);
09249 
09250 
09251 /* *********************************************************** */
09252 
09253 /*                                              From STRIPACK */
09254 /*                                            Robert J. Renka */
09255 /*                                  Dept. of Computer Science */
09256 /*                                       Univ. of North Texas */
09257 /*                                           renka@cs.unt.edu */
09258 /*                                                   07/11/96 */
09259 
09260 /*   This subroutine adds a boundary node to a triangulation */
09261 /* of a set of KK-1 points on the unit sphere.  The data */
09262 /* structure is updated with the insertion of node KK, but no */
09263 /* optimization is performed. */
09264 
09265 /*   This routine is identical to the similarly named routine */
09266 /* in TRIPACK. */
09267 
09268 
09269 /* On input: */
09270 
09271 /*       KK = Index of a node to be connected to the sequence */
09272 /*            of all visible boundary nodes.  KK .GE. 1 and */
09273 /*            KK must not be equal to I1 or I2. */
09274 
09275 /*       I1 = First (rightmost as viewed from KK) boundary */
09276 /*            node in the triangulation that is visible from */
09277 /*            node KK (the line segment KK-I1 intersects no */
09278 /*            arcs. */
09279 
09280 /*       I2 = Last (leftmost) boundary node that is visible */
09281 /*            from node KK.  I1 and I2 may be determined by */
09282 /*            Subroutine TRFIND. */
09283 
09284 /* The above parameters are not altered by this routine. */
09285 
09286 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09287 /*                             created by Subroutine TRMESH. */
09288 /*                             Nodes I1 and I2 must be in- */
09289 /*                             cluded in the triangulation. */
09290 
09291 /* On output: */
09292 
09293 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09294 /*                             the addition of node KK.  Node */
09295 /*                             KK is connected to I1, I2, and */
09296 /*                             all boundary nodes in between. */
09297 
09298 /* Module required by BDYADD:  INSERT */
09299 
09300 /* *********************************************************** */
09301 
09302 
09303 /* Local parameters: */
09304 
09305 /* K =     Local copy of KK */
09306 /* LP =    LIST pointer */
09307 /* LSAV =  LIST pointer */
09308 /* N1,N2 = Local copies of I1 and I2, respectively */
09309 /* NEXT =  Boundary node visible from K */
09310 /* NSAV =  Boundary node visible from K */
09311 
09312     /* Parameter adjustments */
09313     --lend;
09314     --lptr;
09315     --list;
09316 
09317     /* Function Body */
09318     k = *kk;
09319     n1 = *i1;
09320     n2 = *i2;
09321 
09322 /* Add K as the last neighbor of N1. */
09323 
09324     lp = lend[n1];
09325     lsav = lptr[lp];
09326     lptr[lp] = *lnew;
09327     list[*lnew] = -k;
09328     lptr[*lnew] = lsav;
09329     lend[n1] = *lnew;
09330     ++(*lnew);
09331     next = -list[lp];
09332     list[lp] = next;
09333     nsav = next;
09334 
09335 /* Loop on the remaining boundary nodes between N1 and N2, */
09336 /*   adding K as the first neighbor. */
09337 
09338 L1:
09339     lp = lend[next];
09340     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09341     if (next == n2) {
09342         goto L2;
09343     }
09344     next = -list[lp];
09345     list[lp] = next;
09346     goto L1;
09347 
09348 /* Add the boundary nodes between N1 and N2 as neighbors */
09349 /*   of node K. */
09350 
09351 L2:
09352     lsav = *lnew;
09353     list[*lnew] = n1;
09354     lptr[*lnew] = *lnew + 1;
09355     ++(*lnew);
09356     next = nsav;
09357 
09358 L3:
09359     if (next == n2) {
09360         goto L4;
09361     }
09362     list[*lnew] = next;
09363     lptr[*lnew] = *lnew + 1;
09364     ++(*lnew);
09365     lp = lend[next];
09366     next = list[lp];
09367     goto L3;
09368 
09369 L4:
09370     list[*lnew] = -n2;
09371     lptr[*lnew] = lsav;
09372     lend[k] = *lnew;
09373     ++(*lnew);
09374     return 0;
09375 } /* bdyadd_ */
09376 
09377 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09378         int *lend, int *nodes, int *nb, int *na, int *nt)
09379 {
09380     /* System generated locals */
09381     int i__1;
09382 
09383     /* Local variables */
09384     static int k, n0, lp, nn, nst;
09385 
09386 
09387 /* *********************************************************** */
09388 
09389 /*                                              From STRIPACK */
09390 /*                                            Robert J. Renka */
09391 /*                                  Dept. of Computer Science */
09392 /*                                       Univ. of North Texas */
09393 /*                                           renka@cs.unt.edu */
09394 /*                                                   06/26/96 */
09395 
09396 /*   Given a triangulation of N nodes on the unit sphere */
09397 /* created by Subroutine TRMESH, this subroutine returns an */
09398 /* array containing the indexes (if any) of the counterclock- */
09399 /* wise-ordered sequence of boundary nodes -- the nodes on */
09400 /* the boundary of the convex hull of the set of nodes.  (The */
09401 /* boundary is empty if the nodes do not lie in a single */
09402 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09403 /* triangles are also returned. */
09404 
09405 
09406 /* On input: */
09407 
09408 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09409 
09410 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09411 /*                        gulation.  Refer to Subroutine */
09412 /*                        TRMESH. */
09413 
09414 /* The above parameters are not altered by this routine. */
09415 
09416 /*       NODES = int array of length at least NB */
09417 /*               (NB .LE. N). */
09418 
09419 /* On output: */
09420 
09421 /*       NODES = Ordered sequence of boundary node indexes */
09422 /*               in the range 1 to N (in the first NB loca- */
09423 /*               tions). */
09424 
09425 /*       NB = Number of boundary nodes. */
09426 
09427 /*       NA,NT = Number of arcs and triangles, respectively, */
09428 /*               in the triangulation. */
09429 
09430 /* Modules required by BNODES:  None */
09431 
09432 /* *********************************************************** */
09433 
09434 
09435 /* Local parameters: */
09436 
09437 /* K =   NODES index */
09438 /* LP =  LIST pointer */
09439 /* N0 =  Boundary node to be added to NODES */
09440 /* NN =  Local copy of N */
09441 /* NST = First element of nodes (arbitrarily chosen to be */
09442 /*         the one with smallest index) */
09443 
09444     /* Parameter adjustments */
09445     --lend;
09446     --list;
09447     --lptr;
09448     --nodes;
09449 
09450     /* Function Body */
09451     nn = *n;
09452 
09453 /* Search for a boundary node. */
09454 
09455     i__1 = nn;
09456     for (nst = 1; nst <= i__1; ++nst) {
09457         lp = lend[nst];
09458         if (list[lp] < 0) {
09459             goto L2;
09460         }
09461 /* L1: */
09462     }
09463 
09464 /* The triangulation contains no boundary nodes. */
09465 
09466     *nb = 0;
09467     *na = (nn - 2) * 3;
09468     *nt = nn - (2<<1);
09469     return 0;
09470 
09471 /* NST is the first boundary node encountered.  Initialize */
09472 /*   for traversal of the boundary. */
09473 
09474 L2:
09475     nodes[1] = nst;
09476     k = 1;
09477     n0 = nst;
09478 
09479 /* Traverse the boundary in counterclockwise order. */
09480 
09481 L3:
09482     lp = lend[n0];
09483     lp = lptr[lp];
09484     n0 = list[lp];
09485     if (n0 == nst) {
09486         goto L4;
09487     }
09488     ++k;
09489     nodes[k] = n0;
09490     goto L3;
09491 
09492 /* Store the counts. */
09493 
09494 L4:
09495     *nb = k;
09496     *nt = (*n << 1) - *nb - 2;
09497     *na = *nt + *n - 1;
09498     return 0;
09499 } /* bnodes_ */
09500 
09501 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09502         int *ier)
09503 {
09504     /* System generated locals */
09505     int i__1;
09506 
09507     /* Builtin functions */
09508     //double atan(double), cos(double), sin(double);
09509 
09510     /* Local variables */
09511     static double a, c__;
09512     static int i__;
09513     static double s;
09514     static int k2, k3;
09515     static double x0, y0;
09516     static int kk, np1;
09517 
09518 
09519 /* *********************************************************** */
09520 
09521 /*                                              From STRIPACK */
09522 /*                                            Robert J. Renka */
09523 /*                                  Dept. of Computer Science */
09524 /*                                       Univ. of North Texas */
09525 /*                                           renka@cs.unt.edu */
09526 /*                                                   04/06/90 */
09527 
09528 /*   This subroutine computes the coordinates of a sequence */
09529 /* of N equally spaced points on the unit circle centered at */
09530 /* (0,0).  An N-sided polygonal approximation to the circle */
09531 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09532 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09533 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09534 /* is 2*PI*R, where R is the radius of the circle in device */
09535 /* coordinates. */
09536 
09537 
09538 /* On input: */
09539 
09540 /*       K = Number of points in each quadrant, defining N as */
09541 /*           4K.  K .GE. 1. */
09542 
09543 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09544 
09545 /* K is not altered by this routine. */
09546 
09547 /* On output: */
09548 
09549 /*       XC,YC = Cartesian coordinates of the points on the */
09550 /*               unit circle in the first N+1 locations. */
09551 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09552 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09553 /*               and YC(N+1) = YC(1). */
09554 
09555 /*       IER = Error indicator: */
09556 /*             IER = 0 if no errors were encountered. */
09557 /*             IER = 1 if K < 1 on input. */
09558 
09559 /* Modules required by CIRCLE:  None */
09560 
09561 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09562 /*                                          SIN */
09563 
09564 /* *********************************************************** */
09565 
09566 
09567 /* Local parameters: */
09568 
09569 /* I =     DO-loop index and index for XC and YC */
09570 /* KK =    Local copy of K */
09571 /* K2 =    K*2 */
09572 /* K3 =    K*3 */
09573 /* NP1 =   N+1 = 4*K + 1 */
09574 /* A =     Angular separation between adjacent points */
09575 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09576 /*           rotation through angle A */
09577 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09578 /*           circle in the first quadrant */
09579 
09580     /* Parameter adjustments */
09581     --yc;
09582     --xc;
09583 
09584     /* Function Body */
09585     kk = *k;
09586     k2 = kk << 1;
09587     k3 = kk * 3;
09588     np1 = (kk << 2) + 1;
09589 
09590 /* Test for invalid input, compute A, C, and S, and */
09591 /*   initialize (X0,Y0) to (1,0). */
09592 
09593     if (kk < 1) {
09594         goto L2;
09595     }
09596     a = atan(1.) * 2. / (double) kk;
09597     c__ = cos(a);
09598     s = sin(a);
09599     x0 = 1.;
09600     y0 = 0.;
09601 
09602 /* Loop on points (X0,Y0) in the first quadrant, storing */
09603 /*   the point and its reflections about the x axis, the */
09604 /*   y axis, and the line y = -x. */
09605 
09606     i__1 = kk;
09607     for (i__ = 1; i__ <= i__1; ++i__) {
09608         xc[i__] = x0;
09609         yc[i__] = y0;
09610         xc[i__ + kk] = -y0;
09611         yc[i__ + kk] = x0;
09612         xc[i__ + k2] = -x0;
09613         yc[i__ + k2] = -y0;
09614         xc[i__ + k3] = y0;
09615         yc[i__ + k3] = -x0;
09616 
09617 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09618 
09619         x0 = c__ * x0 - s * y0;
09620         y0 = s * x0 + c__ * y0;
09621 /* L1: */
09622     }
09623 
09624 /* Store the coordinates of the first point as the last */
09625 /*   point. */
09626 
09627     xc[np1] = xc[1];
09628     yc[np1] = yc[1];
09629     *ier = 0;
09630     return 0;
09631 
09632 /* K < 1. */
09633 
09634 L2:
09635     *ier = 1;
09636     return 0;
09637 } /* circle_ */
09638 
09639 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09640         double *c__, int *ier)
09641 {
09642     /* Builtin functions */
09643     //double sqrt(double);
09644 
09645     /* Local variables */
09646     static int i__;
09647     static double e1[3], e2[3], cu[3], cnorm;
09648 
09649 
09650 /* *********************************************************** */
09651 
09652 /*                                              From STRIPACK */
09653 /*                                            Robert J. Renka */
09654 /*                                  Dept. of Computer Science */
09655 /*                                       Univ. of North Texas */
09656 /*                                           renka@cs.unt.edu */
09657 /*                                                   10/27/02 */
09658 
09659 /*   This subroutine returns the circumcenter of a spherical */
09660 /* triangle on the unit sphere:  the point on the sphere sur- */
09661 /* face that is equally distant from the three triangle */
09662 /* vertices and lies in the same hemisphere, where distance */
09663 /* is taken to be arc-length on the sphere surface. */
09664 
09665 
09666 /* On input: */
09667 
09668 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09669 /*                  sian coordinates of the three triangle */
09670 /*                  vertices (unit vectors) in CCW order. */
09671 
09672 /* The above parameters are not altered by this routine. */
09673 
09674 /*       C = Array of length 3. */
09675 
09676 /* On output: */
09677 
09678 /*       C = Cartesian coordinates of the circumcenter unless */
09679 /*           IER > 0, in which case C is not defined.  C = */
09680 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09681 
09682 /*       IER = Error indicator: */
09683 /*             IER = 0 if no errors were encountered. */
09684 /*             IER = 1 if V1, V2, and V3 lie on a common */
09685 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09686 /*             (The vertices are not tested for validity.) */
09687 
09688 /* Modules required by CIRCUM:  None */
09689 
09690 /* Intrinsic function called by CIRCUM:  SQRT */
09691 
09692 /* *********************************************************** */
09693 
09694 
09695 /* Local parameters: */
09696 
09697 /* CNORM = Norm of CU:  used to compute C */
09698 /* CU =    Scalar multiple of C:  E1 X E2 */
09699 /* E1,E2 = Edges of the underlying planar triangle: */
09700 /*           V2-V1 and V3-V1, respectively */
09701 /* I =     DO-loop index */
09702 
09703     /* Parameter adjustments */
09704     --c__;
09705     --v3;
09706     --v2;
09707     --v1;
09708 
09709     /* Function Body */
09710     for (i__ = 1; i__ <= 3; ++i__) {
09711         e1[i__ - 1] = v2[i__] - v1[i__];
09712         e2[i__ - 1] = v3[i__] - v1[i__];
09713 /* L1: */
09714     }
09715 
09716 /* Compute CU = E1 X E2 and CNORM**2. */
09717 
09718     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09719     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09720     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09721     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09722 
09723 /* The vertices lie on a common line if and only if CU is */
09724 /*   the zero vector. */
09725 
09726     if (cnorm != 0.) {
09727 
09728 /*   No error:  compute C. */
09729 
09730         cnorm = sqrt(cnorm);
09731         for (i__ = 1; i__ <= 3; ++i__) {
09732             c__[i__] = cu[i__ - 1] / cnorm;
09733 /* L2: */
09734         }
09735 
09736 /* If the vertices are nearly identical, the problem is */
09737 /*   ill-conditioned and it is possible for the computed */
09738 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09739 /*   when it should be positive. */
09740 
09741         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09742             c__[1] = -c__[1];
09743             c__[2] = -c__[2];
09744             c__[3] = -c__[3];
09745         }
09746         *ier = 0;
09747     } else {
09748 
09749 /*   CU = 0. */
09750 
09751         *ier = 1;
09752     }
09753     return 0;
09754 } /* circum_ */
09755 
09756 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09757         *lptr, int *lend, int *lnew)
09758 {
09759     static int k, lp, nst, lsav, next;
09760     /* Subroutine */ int insert_(int *, int *, int *,
09761             int *, int *);
09762 
09763 
09764 /* *********************************************************** */
09765 
09766 /*                                              From STRIPACK */
09767 /*                                            Robert J. Renka */
09768 /*                                  Dept. of Computer Science */
09769 /*                                       Univ. of North Texas */
09770 /*                                           renka@cs.unt.edu */
09771 /*                                                   07/17/96 */
09772 
09773 /*   This subroutine connects an exterior node KK to all */
09774 /* boundary nodes of a triangulation of KK-1 points on the */
09775 /* unit sphere, producing a triangulation that covers the */
09776 /* sphere.  The data structure is updated with the addition */
09777 /* of node KK, but no optimization is performed.  All boun- */
09778 /* dary nodes must be visible from node KK. */
09779 
09780 
09781 /* On input: */
09782 
09783 /*       KK = Index of the node to be connected to the set of */
09784 /*            all boundary nodes.  KK .GE. 4. */
09785 
09786 /*       N0 = Index of a boundary node (in the range 1 to */
09787 /*            KK-1).  N0 may be determined by Subroutine */
09788 /*            TRFIND. */
09789 
09790 /* The above parameters are not altered by this routine. */
09791 
09792 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09793 /*                             created by Subroutine TRMESH. */
09794 /*                             Node N0 must be included in */
09795 /*                             the triangulation. */
09796 
09797 /* On output: */
09798 
09799 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09800 /*                             the addition of node KK as the */
09801 /*                             last entry.  The updated */
09802 /*                             triangulation contains no */
09803 /*                             boundary nodes. */
09804 
09805 /* Module required by COVSPH:  INSERT */
09806 
09807 /* *********************************************************** */
09808 
09809 
09810 /* Local parameters: */
09811 
09812 /* K =     Local copy of KK */
09813 /* LP =    LIST pointer */
09814 /* LSAV =  LIST pointer */
09815 /* NEXT =  Boundary node visible from K */
09816 /* NST =   Local copy of N0 */
09817 
09818     /* Parameter adjustments */
09819     --lend;
09820     --lptr;
09821     --list;
09822 
09823     /* Function Body */
09824     k = *kk;
09825     nst = *n0;
09826 
09827 /* Traverse the boundary in clockwise order, inserting K as */
09828 /*   the first neighbor of each boundary node, and converting */
09829 /*   the boundary node to an interior node. */
09830 
09831     next = nst;
09832 L1:
09833     lp = lend[next];
09834     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09835     next = -list[lp];
09836     list[lp] = next;
09837     if (next != nst) {
09838         goto L1;
09839     }
09840 
09841 /* Traverse the boundary again, adding each node to K's */
09842 /*   adjacency list. */
09843 
09844     lsav = *lnew;
09845 L2:
09846     lp = lend[next];
09847     list[*lnew] = next;
09848     lptr[*lnew] = *lnew + 1;
09849     ++(*lnew);
09850     next = list[lp];
09851     if (next != nst) {
09852         goto L2;
09853     }
09854 
09855     lptr[*lnew - 1] = lsav;
09856     lend[k] = *lnew - 1;
09857     return 0;
09858 } /* covsph_ */
09859 
09860 
09861 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09862         double *y, double *z__, int *list, int *lend, int
09863         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09864         double *xc, double *yc, double *zc, double *rc,
09865         int *ier)
09866 {
09867     /* System generated locals */
09868     int i__1, i__2;
09869 
09870     /* Builtin functions */
09871     //double acos(double);
09872 
09873     /* Local variables */
09874     static double c__[3], t;
09875     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09876     static double v1[3], v2[3], v3[3];
09877     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09878              lpn;
09879     static long int swp;
09880     static int ierr;
09881     int lstptr_(int *, int *, int *, int *);
09882     long int swptst_(int *, int *, int *, int *,
09883             double *, double *, double *);
09884 
09885 
09886 /* *********************************************************** */
09887 
09888 /*                                              From STRIPACK */
09889 /*                                            Robert J. Renka */
09890 /*                                  Dept. of Computer Science */
09891 /*                                       Univ. of North Texas */
09892 /*                                           renka@cs.unt.edu */
09893 /*                                                   03/05/03 */
09894 
09895 /*   Given a Delaunay triangulation of nodes on the surface */
09896 /* of the unit sphere, this subroutine returns the set of */
09897 /* triangle circumcenters corresponding to Voronoi vertices, */
09898 /* along with the circumradii and a list of triangle indexes */
09899 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09900 /* entries. */
09901 
09902 /*   A triangle circumcenter is the point (unit vector) lying */
09903 /* at the same angular distance from the three vertices and */
09904 /* contained in the same hemisphere as the vertices.  (Note */
09905 /* that the negative of a circumcenter is also equidistant */
09906 /* from the vertices.)  If the triangulation covers the sur- */
09907 /* face, the Voronoi vertices are the circumcenters of the */
09908 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09909 /* LNEW are not altered in this case. */
09910 
09911 /*   On the other hand, if the nodes are contained in a sin- */
09912 /* gle hemisphere, the triangulation is implicitly extended */
09913 /* to the entire surface by adding pseudo-arcs (of length */
09914 /* greater than 180 degrees) between boundary nodes forming */
09915 /* pseudo-triangles whose 'circumcenters' are included in the */
09916 /* list.  This extension to the triangulation actually con- */
09917 /* sists of a triangulation of the set of boundary nodes in */
09918 /* which the swap test is reversed (a non-empty circumcircle */
09919 /* test).  The negative circumcenters are stored as the */
09920 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
09921 /* LNEW contain a data structure corresponding to the ex- */
09922 /* tended triangulation (Voronoi diagram), but LIST is not */
09923 /* altered in this case.  Thus, if it is necessary to retain */
09924 /* the original (unextended) triangulation data structure, */
09925 /* copies of LPTR and LNEW must be saved before calling this */
09926 /* routine. */
09927 
09928 
09929 /* On input: */
09930 
09931 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09932 /*           Note that, if N = 3, there are only two Voronoi */
09933 /*           vertices separated by 180 degrees, and the */
09934 /*           Voronoi regions are not well defined. */
09935 
09936 /*       NCOL = Number of columns reserved for LTRI.  This */
09937 /*              must be at least NB-2, where NB is the number */
09938 /*              of boundary nodes. */
09939 
09940 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09941 /*               coordinates of the nodes (unit vectors). */
09942 
09943 /*       LIST = int array containing the set of adjacency */
09944 /*              lists.  Refer to Subroutine TRMESH. */
09945 
09946 /*       LEND = Set of pointers to ends of adjacency lists. */
09947 /*              Refer to Subroutine TRMESH. */
09948 
09949 /* The above parameters are not altered by this routine. */
09950 
09951 /*       LPTR = Array of pointers associated with LIST.  Re- */
09952 /*              fer to Subroutine TRMESH. */
09953 
09954 /*       LNEW = Pointer to the first empty location in LIST */
09955 /*              and LPTR (list length plus one). */
09956 
09957 /*       LTRI = int work space array dimensioned 6 by */
09958 /*              NCOL, or unused dummy parameter if NB = 0. */
09959 
09960 /*       LISTC = int array of length at least 3*NT, where */
09961 /*               NT = 2*N-4 is the number of triangles in the */
09962 /*               triangulation (after extending it to cover */
09963 /*               the entire surface if necessary). */
09964 
09965 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
09966 
09967 /* On output: */
09968 
09969 /*       LPTR = Array of pointers associated with LISTC: */
09970 /*              updated for the addition of pseudo-triangles */
09971 /*              if the original triangulation contains */
09972 /*              boundary nodes (NB > 0). */
09973 
09974 /*       LNEW = Pointer to the first empty location in LISTC */
09975 /*              and LPTR (list length plus one).  LNEW is not */
09976 /*              altered if NB = 0. */
09977 
09978 /*       LTRI = Triangle list whose first NB-2 columns con- */
09979 /*              tain the indexes of a clockwise-ordered */
09980 /*              sequence of vertices (first three rows) */
09981 /*              followed by the LTRI column indexes of the */
09982 /*              triangles opposite the vertices (or 0 */
09983 /*              denoting the exterior region) in the last */
09984 /*              three rows.  This array is not generally of */
09985 /*              any use. */
09986 
09987 /*       LISTC = Array containing triangle indexes (indexes */
09988 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
09989 /*               pondence with LIST/LPTR entries (or entries */
09990 /*               that would be stored in LIST for the */
09991 /*               extended triangulation):  the index of tri- */
09992 /*               angle (N1,N2,N3) is stored in LISTC(K), */
09993 /*               LISTC(L), and LISTC(M), where LIST(K), */
09994 /*               LIST(L), and LIST(M) are the indexes of N2 */
09995 /*               as a neighbor of N1, N3 as a neighbor of N2, */
09996 /*               and N1 as a neighbor of N3.  The Voronoi */
09997 /*               region associated with a node is defined by */
09998 /*               the CCW-ordered sequence of circumcenters in */
09999 /*               one-to-one correspondence with its adjacency */
10000 /*               list (in the extended triangulation). */
10001 
10002 /*       NB = Number of boundary nodes unless IER = 1. */
10003 
10004 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
10005 /*                  nates of the triangle circumcenters */
10006 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
10007 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
10008 /*                  correspond to pseudo-triangles if NB > 0. */
10009 
10010 /*       RC = Array containing circumradii (the arc lengths */
10011 /*            or angles between the circumcenters and associ- */
10012 /*            ated triangle vertices) in 1-1 correspondence */
10013 /*            with circumcenters. */
10014 
10015 /*       IER = Error indicator: */
10016 /*             IER = 0 if no errors were encountered. */
10017 /*             IER = 1 if N < 3. */
10018 /*             IER = 2 if NCOL < NB-2. */
10019 /*             IER = 3 if a triangle is degenerate (has ver- */
10020 /*                     tices lying on a common geodesic). */
10021 
10022 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
10023 
10024 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
10025 
10026 /* *********************************************************** */
10027 
10028 
10029 /* Local parameters: */
10030 
10031 /* C =         Circumcenter returned by Subroutine CIRCUM */
10032 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
10033 /* I4 =        LTRI row index in the range 1 to 3 */
10034 /* IERR =      Error flag for calls to CIRCUM */
10035 /* KT =        Triangle index */
10036 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
10037 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
10038 /*               and N2 as vertices of KT1 */
10039 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
10040 /*               and N2 as vertices of KT2 */
10041 /* LP,LPN =    LIST pointers */
10042 /* LPL =       LIST pointer of the last neighbor of N1 */
10043 /* N0 =        Index of the first boundary node (initial */
10044 /*               value of N1) in the loop on boundary nodes */
10045 /*               used to store the pseudo-triangle indexes */
10046 /*               in LISTC */
10047 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
10048 /*               or pseudo-triangle (clockwise order) */
10049 /* N4 =        Index of the node opposite N2 -> N1 */
10050 /* NM2 =       N-2 */
10051 /* NN =        Local copy of N */
10052 /* NT =        Number of pseudo-triangles:  NB-2 */
10053 /* SWP =       long int variable set to TRUE in each optimiza- */
10054 /*               tion loop (loop on pseudo-arcs) iff a swap */
10055 /*               is performed */
10056 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
10057 /*               Subroutine CIRCUM */
10058 
10059     /* Parameter adjustments */
10060     --lend;
10061     --z__;
10062     --y;
10063     --x;
10064     ltri -= 7;
10065     --list;
10066     --lptr;
10067     --listc;
10068     --xc;
10069     --yc;
10070     --zc;
10071     --rc;
10072 
10073     /* Function Body */
10074     nn = *n;
10075     *nb = 0;
10076     nt = 0;
10077     if (nn < 3) {
10078         goto L21;
10079     }
10080 
10081 /* Search for a boundary node N1. */
10082 
10083     i__1 = nn;
10084     for (n1 = 1; n1 <= i__1; ++n1) {
10085         lp = lend[n1];
10086         if (list[lp] < 0) {
10087             goto L2;
10088         }
10089 /* L1: */
10090     }
10091 
10092 /* The triangulation already covers the sphere. */
10093 
10094     goto L9;
10095 
10096 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
10097 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
10098 /*   boundary nodes to which it is not already adjacent. */
10099 
10100 /*   Set N3 and N2 to the first and last neighbors, */
10101 /*     respectively, of N1. */
10102 
10103 L2:
10104     n2 = -list[lp];
10105     lp = lptr[lp];
10106     n3 = list[lp];
10107 
10108 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
10109 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
10110 /*     along with the indexes of the triangles opposite */
10111 /*     the vertices. */
10112 
10113 L3:
10114     ++nt;
10115     if (nt <= *ncol) {
10116         ltri[nt * 6 + 1] = n1;
10117         ltri[nt * 6 + 2] = n2;
10118         ltri[nt * 6 + 3] = n3;
10119         ltri[nt * 6 + 4] = nt + 1;
10120         ltri[nt * 6 + 5] = nt - 1;
10121         ltri[nt * 6 + 6] = 0;
10122     }
10123     n1 = n2;
10124     lp = lend[n1];
10125     n2 = -list[lp];
10126     if (n2 != n3) {
10127         goto L3;
10128     }
10129 
10130     *nb = nt + 2;
10131     if (*ncol < nt) {
10132         goto L22;
10133     }
10134     ltri[nt * 6 + 4] = 0;
10135     if (nt == 1) {
10136         goto L7;
10137     }
10138 
10139 /* Optimize the exterior triangulation (set of pseudo- */
10140 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
10141 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
10142 /*   The loop on pseudo-arcs is repeated until no swaps are */
10143 /*   performed. */
10144 
10145 L4:
10146     swp = FALSE_;
10147     i__1 = nt - 1;
10148     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10149         for (i3 = 1; i3 <= 3; ++i3) {
10150             kt2 = ltri[i3 + 3 + kt1 * 6];
10151             if (kt2 <= kt1) {
10152                 goto L5;
10153             }
10154 
10155 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10156 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10157 
10158             if (i3 == 1) {
10159                 i1 = 2;
10160                 i2 = 3;
10161             } else if (i3 == 2) {
10162                 i1 = 3;
10163                 i2 = 1;
10164             } else {
10165                 i1 = 1;
10166                 i2 = 2;
10167             }
10168             n1 = ltri[i1 + kt1 * 6];
10169             n2 = ltri[i2 + kt1 * 6];
10170             n3 = ltri[i3 + kt1 * 6];
10171 
10172 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10173 /*     LTRI(I+3,KT2) = KT1. */
10174 
10175             if (ltri[kt2 * 6 + 4] == kt1) {
10176                 i4 = 1;
10177             } else if (ltri[kt2 * 6 + 5] == kt1) {
10178                 i4 = 2;
10179             } else {
10180                 i4 = 3;
10181             }
10182             n4 = ltri[i4 + kt2 * 6];
10183 
10184 /*   The empty circumcircle test is reversed for the pseudo- */
10185 /*     triangles.  The reversal is implicit in the clockwise */
10186 /*     ordering of the vertices. */
10187 
10188             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10189                 goto L5;
10190             }
10191 
10192 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10193 /*     Nj as a vertex of KTi. */
10194 
10195             swp = TRUE_;
10196             kt11 = ltri[i1 + 3 + kt1 * 6];
10197             kt12 = ltri[i2 + 3 + kt1 * 6];
10198             if (i4 == 1) {
10199                 i2 = 2;
10200                 i1 = 3;
10201             } else if (i4 == 2) {
10202                 i2 = 3;
10203                 i1 = 1;
10204             } else {
10205                 i2 = 1;
10206                 i1 = 2;
10207             }
10208             kt21 = ltri[i1 + 3 + kt2 * 6];
10209             kt22 = ltri[i2 + 3 + kt2 * 6];
10210             ltri[kt1 * 6 + 1] = n4;
10211             ltri[kt1 * 6 + 2] = n3;
10212             ltri[kt1 * 6 + 3] = n1;
10213             ltri[kt1 * 6 + 4] = kt12;
10214             ltri[kt1 * 6 + 5] = kt22;
10215             ltri[kt1 * 6 + 6] = kt2;
10216             ltri[kt2 * 6 + 1] = n3;
10217             ltri[kt2 * 6 + 2] = n4;
10218             ltri[kt2 * 6 + 3] = n2;
10219             ltri[kt2 * 6 + 4] = kt21;
10220             ltri[kt2 * 6 + 5] = kt11;
10221             ltri[kt2 * 6 + 6] = kt1;
10222 
10223 /*   Correct the KT11 and KT22 entries that changed. */
10224 
10225             if (kt11 != 0) {
10226                 i4 = 4;
10227                 if (ltri[kt11 * 6 + 4] != kt1) {
10228                     i4 = 5;
10229                     if (ltri[kt11 * 6 + 5] != kt1) {
10230                         i4 = 6;
10231                     }
10232                 }
10233                 ltri[i4 + kt11 * 6] = kt2;
10234             }
10235             if (kt22 != 0) {
10236                 i4 = 4;
10237                 if (ltri[kt22 * 6 + 4] != kt2) {
10238                     i4 = 5;
10239                     if (ltri[kt22 * 6 + 5] != kt2) {
10240                         i4 = 6;
10241                     }
10242                 }
10243                 ltri[i4 + kt22 * 6] = kt1;
10244             }
10245 L5:
10246             ;
10247         }
10248 /* L6: */
10249     }
10250     if (swp) {
10251         goto L4;
10252     }
10253 
10254 /* Compute and store the negative circumcenters and radii of */
10255 /*   the pseudo-triangles in the first NT positions. */
10256 
10257 L7:
10258     i__1 = nt;
10259     for (kt = 1; kt <= i__1; ++kt) {
10260         n1 = ltri[kt * 6 + 1];
10261         n2 = ltri[kt * 6 + 2];
10262         n3 = ltri[kt * 6 + 3];
10263         v1[0] = x[n1];
10264         v1[1] = y[n1];
10265         v1[2] = z__[n1];
10266         v2[0] = x[n2];
10267         v2[1] = y[n2];
10268         v2[2] = z__[n2];
10269         v3[0] = x[n3];
10270         v3[1] = y[n3];
10271         v3[2] = z__[n3];
10272         circum_(v2, v1, v3, c__, &ierr);
10273         if (ierr != 0) {
10274             goto L23;
10275         }
10276 
10277 /*   Store the negative circumcenter and radius (computed */
10278 /*     from <V1,C>). */
10279 
10280         xc[kt] = -c__[0];
10281         yc[kt] = -c__[1];
10282         zc[kt] = -c__[2];
10283         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10284         if (t < -1.) {
10285             t = -1.;
10286         }
10287         if (t > 1.) {
10288             t = 1.;
10289         }
10290         rc[kt] = acos(t);
10291 /* L8: */
10292     }
10293 
10294 /* Compute and store the circumcenters and radii of the */
10295 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10296 /*   Also, store the triangle indexes KT in the appropriate */
10297 /*   LISTC positions. */
10298 
10299 L9:
10300     kt = nt;
10301 
10302 /*   Loop on nodes N1. */
10303 
10304     nm2 = nn - 2;
10305     i__1 = nm2;
10306     for (n1 = 1; n1 <= i__1; ++n1) {
10307         lpl = lend[n1];
10308         lp = lpl;
10309         n3 = list[lp];
10310 
10311 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10312 /*     and N3 > N1. */
10313 
10314 L10:
10315         lp = lptr[lp];
10316         n2 = n3;
10317         n3 = (i__2 = list[lp], abs(i__2));
10318         if (n2 <= n1 || n3 <= n1) {
10319             goto L11;
10320         }
10321         ++kt;
10322 
10323 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10324 
10325         v1[0] = x[n1];
10326         v1[1] = y[n1];
10327         v1[2] = z__[n1];
10328         v2[0] = x[n2];
10329         v2[1] = y[n2];
10330         v2[2] = z__[n2];
10331         v3[0] = x[n3];
10332         v3[1] = y[n3];
10333         v3[2] = z__[n3];
10334         circum_(v1, v2, v3, c__, &ierr);
10335         if (ierr != 0) {
10336             goto L23;
10337         }
10338 
10339 /*   Store the circumcenter, radius and triangle index. */
10340 
10341         xc[kt] = c__[0];
10342         yc[kt] = c__[1];
10343         zc[kt] = c__[2];
10344         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10345         if (t < -1.) {
10346             t = -1.;
10347         }
10348         if (t > 1.) {
10349             t = 1.;
10350         }
10351         rc[kt] = acos(t);
10352 
10353 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10354 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10355 /*     of N2, and N1 as a neighbor of N3. */
10356 
10357         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10358         listc[lpn] = kt;
10359         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10360         listc[lpn] = kt;
10361         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10362         listc[lpn] = kt;
10363 L11:
10364         if (lp != lpl) {
10365             goto L10;
10366         }
10367 /* L12: */
10368     }
10369     if (nt == 0) {
10370         goto L20;
10371     }
10372 
10373 /* Store the first NT triangle indexes in LISTC. */
10374 
10375 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10376 /*     boundary arc opposite N3. */
10377 
10378     kt1 = 0;
10379 L13:
10380     ++kt1;
10381     if (ltri[kt1 * 6 + 4] == 0) {
10382         i1 = 2;
10383         i2 = 3;
10384         i3 = 1;
10385         goto L14;
10386     } else if (ltri[kt1 * 6 + 5] == 0) {
10387         i1 = 3;
10388         i2 = 1;
10389         i3 = 2;
10390         goto L14;
10391     } else if (ltri[kt1 * 6 + 6] == 0) {
10392         i1 = 1;
10393         i2 = 2;
10394         i3 = 3;
10395         goto L14;
10396     }
10397     goto L13;
10398 L14:
10399     n1 = ltri[i1 + kt1 * 6];
10400     n0 = n1;
10401 
10402 /*   Loop on boundary nodes N1 in CCW order, storing the */
10403 /*     indexes of the clockwise-ordered sequence of triangles */
10404 /*     that contain N1.  The first triangle overwrites the */
10405 /*     last neighbor position, and the remaining triangles, */
10406 /*     if any, are appended to N1's adjacency list. */
10407 
10408 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10409 
10410 L15:
10411     lp = lend[n1];
10412     lpn = lptr[lp];
10413     listc[lp] = kt1;
10414 
10415 /*   Loop on triangles KT2 containing N1. */
10416 
10417 L16:
10418     kt2 = ltri[i2 + 3 + kt1 * 6];
10419     if (kt2 != 0) {
10420 
10421 /*   Append KT2 to N1's triangle list. */
10422 
10423         lptr[lp] = *lnew;
10424         lp = *lnew;
10425         listc[lp] = kt2;
10426         ++(*lnew);
10427 
10428 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10429 /*     LTRI(I1,KT1) = N1. */
10430 
10431         kt1 = kt2;
10432         if (ltri[kt1 * 6 + 1] == n1) {
10433             i1 = 1;
10434             i2 = 2;
10435             i3 = 3;
10436         } else if (ltri[kt1 * 6 + 2] == n1) {
10437             i1 = 2;
10438             i2 = 3;
10439             i3 = 1;
10440         } else {
10441             i1 = 3;
10442             i2 = 1;
10443             i3 = 2;
10444         }
10445         goto L16;
10446     }
10447 
10448 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10449 /*     N1 to the next boundary node, test for termination, */
10450 /*     and permute the indexes:  the last triangle containing */
10451 /*     a boundary node is the first triangle containing the */
10452 /*     next boundary node. */
10453 
10454     lptr[lp] = lpn;
10455     n1 = ltri[i3 + kt1 * 6];
10456     if (n1 != n0) {
10457         i4 = i3;
10458         i3 = i2;
10459         i2 = i1;
10460         i1 = i4;
10461         goto L15;
10462     }
10463 
10464 /* No errors encountered. */
10465 
10466 L20:
10467     *ier = 0;
10468     return 0;
10469 
10470 /* N < 3. */
10471 
10472 L21:
10473     *ier = 1;
10474     return 0;
10475 
10476 /* Insufficient space reserved for LTRI. */
10477 
10478 L22:
10479     *ier = 2;
10480     return 0;
10481 
10482 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10483 
10484 L23:
10485     *ier = 3;
10486     return 0;
10487 } /* crlist_ */
10488 
10489 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10490         list, int *lptr, int *lend, int *lnew, int *ier)
10491 {
10492     /* System generated locals */
10493     int i__1;
10494 
10495     /* Local variables */
10496     static int n1, n2, n3, lp, lph, lpl;
10497     /* Subroutine */ int delnb_(int *, int *, int *,
10498             int *, int *, int *, int *, int *);
10499     int lstptr_(int *, int *, int *, int *);
10500 
10501 
10502 /* *********************************************************** */
10503 
10504 /*                                              From STRIPACK */
10505 /*                                            Robert J. Renka */
10506 /*                                  Dept. of Computer Science */
10507 /*                                       Univ. of North Texas */
10508 /*                                           renka@cs.unt.edu */
10509 /*                                                   07/17/96 */
10510 
10511 /*   This subroutine deletes a boundary arc from a triangula- */
10512 /* tion.  It may be used to remove a null triangle from the */
10513 /* convex hull boundary.  Note, however, that if the union of */
10514 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10515 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10516 /* NEARND should not be called following an arc deletion. */
10517 
10518 /*   This routine is identical to the similarly named routine */
10519 /* in TRIPACK. */
10520 
10521 
10522 /* On input: */
10523 
10524 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10525 
10526 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10527 /*                 adjacent boundary nodes defining the arc */
10528 /*                 to be removed. */
10529 
10530 /* The above parameters are not altered by this routine. */
10531 
10532 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10533 /*                             created by Subroutine TRMESH. */
10534 
10535 /* On output: */
10536 
10537 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10538 /*                             the removal of arc IO1-IO2 */
10539 /*                             unless IER > 0. */
10540 
10541 /*       IER = Error indicator: */
10542 /*             IER = 0 if no errors were encountered. */
10543 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10544 /*                     range, or IO1 = IO2. */
10545 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10546 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10547 /*                     ready a boundary node, and thus IO1 */
10548 /*                     or IO2 has only two neighbors or a */
10549 /*                     deletion would result in two triangu- */
10550 /*                     lations sharing a single node. */
10551 /*             IER = 4 if one of the nodes is a neighbor of */
10552 /*                     the other, but not vice versa, imply- */
10553 /*                     ing an invalid triangulation data */
10554 /*                     structure. */
10555 
10556 /* Module required by DELARC:  DELNB, LSTPTR */
10557 
10558 /* Intrinsic function called by DELARC:  ABS */
10559 
10560 /* *********************************************************** */
10561 
10562 
10563 /* Local parameters: */
10564 
10565 /* LP =       LIST pointer */
10566 /* LPH =      LIST pointer or flag returned by DELNB */
10567 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10568 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10569 /*              is the directed boundary edge associated */
10570 /*              with IO1-IO2 */
10571 
10572     /* Parameter adjustments */
10573     --lend;
10574     --list;
10575     --lptr;
10576 
10577     /* Function Body */
10578     n1 = *io1;
10579     n2 = *io2;
10580 
10581 /* Test for errors, and set N1->N2 to the directed boundary */
10582 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10583 /*   for some N3. */
10584 
10585     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10586         *ier = 1;
10587         return 0;
10588     }
10589 
10590     lpl = lend[n2];
10591     if (-list[lpl] != n1) {
10592         n1 = n2;
10593         n2 = *io1;
10594         lpl = lend[n2];
10595         if (-list[lpl] != n1) {
10596             *ier = 2;
10597             return 0;
10598         }
10599     }
10600 
10601 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10602 /*   of N1), and test for error 3 (N3 already a boundary */
10603 /*   node). */
10604 
10605     lpl = lend[n1];
10606     lp = lptr[lpl];
10607     lp = lptr[lp];
10608     n3 = (i__1 = list[lp], abs(i__1));
10609     lpl = lend[n3];
10610     if (list[lpl] <= 0) {
10611         *ier = 3;
10612         return 0;
10613     }
10614 
10615 /* Delete N2 as a neighbor of N1, making N3 the first */
10616 /*   neighbor, and test for error 4 (N2 not a neighbor */
10617 /*   of N1).  Note that previously computed pointers may */
10618 /*   no longer be valid following the call to DELNB. */
10619 
10620     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10621     if (lph < 0) {
10622         *ier = 4;
10623         return 0;
10624     }
10625 
10626 /* Delete N1 as a neighbor of N2, making N3 the new last */
10627 /*   neighbor. */
10628 
10629     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10630 
10631 /* Make N3 a boundary node with first neighbor N2 and last */
10632 /*   neighbor N1. */
10633 
10634     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10635     lend[n3] = lp;
10636     list[lp] = -n1;
10637 
10638 /* No errors encountered. */
10639 
10640     *ier = 0;
10641     return 0;
10642 } /* delarc_ */
10643 
10644 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10645         list, int *lptr, int *lend, int *lnew, int *lph)
10646 {
10647     /* System generated locals */
10648     int i__1;
10649 
10650     /* Local variables */
10651     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10652 
10653 
10654 /* *********************************************************** */
10655 
10656 /*                                              From STRIPACK */
10657 /*                                            Robert J. Renka */
10658 /*                                  Dept. of Computer Science */
10659 /*                                       Univ. of North Texas */
10660 /*                                           renka@cs.unt.edu */
10661 /*                                                   07/29/98 */
10662 
10663 /*   This subroutine deletes a neighbor NB from the adjacency */
10664 /* list of node N0 (but N0 is not deleted from the adjacency */
10665 /* list of NB) and, if NB is a boundary node, makes N0 a */
10666 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10667 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10668 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10669 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10670 /* ted.  This requires a search of LEND and LPTR entailing an */
10671 /* expected operation count of O(N). */
10672 
10673 /*   This routine is identical to the similarly named routine */
10674 /* in TRIPACK. */
10675 
10676 
10677 /* On input: */
10678 
10679 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10680 /*               nodes such that NB is a neighbor of N0. */
10681 /*               (N0 need not be a neighbor of NB.) */
10682 
10683 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10684 
10685 /* The above parameters are not altered by this routine. */
10686 
10687 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10688 /*                             triangulation. */
10689 
10690 /* On output: */
10691 
10692 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10693 /*                             the removal of NB from the ad- */
10694 /*                             jacency list of N0 unless */
10695 /*                             LPH < 0. */
10696 
10697 /*       LPH = List pointer to the hole (NB as a neighbor of */
10698 /*             N0) filled in by the values at LNEW-1 or error */
10699 /*             indicator: */
10700 /*             LPH > 0 if no errors were encountered. */
10701 /*             LPH = -1 if N0, NB, or N is outside its valid */
10702 /*                      range. */
10703 /*             LPH = -2 if NB is not a neighbor of N0. */
10704 
10705 /* Modules required by DELNB:  None */
10706 
10707 /* Intrinsic function called by DELNB:  ABS */
10708 
10709 /* *********************************************************** */
10710 
10711 
10712 /* Local parameters: */
10713 
10714 /* I =   DO-loop index */
10715 /* LNW = LNEW-1 (output value of LNEW) */
10716 /* LP =  LIST pointer of the last neighbor of NB */
10717 /* LPB = Pointer to NB as a neighbor of N0 */
10718 /* LPL = Pointer to the last neighbor of N0 */
10719 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10720 /* NN =  Local copy of N */
10721 
10722     /* Parameter adjustments */
10723     --lend;
10724     --list;
10725     --lptr;
10726 
10727     /* Function Body */
10728     nn = *n;
10729 
10730 /* Test for error 1. */
10731 
10732     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10733         *lph = -1;
10734         return 0;
10735     }
10736 
10737 /*   Find pointers to neighbors of N0: */
10738 
10739 /*     LPL points to the last neighbor, */
10740 /*     LPP points to the neighbor NP preceding NB, and */
10741 /*     LPB points to NB. */
10742 
10743     lpl = lend[*n0];
10744     lpp = lpl;
10745     lpb = lptr[lpp];
10746 L1:
10747     if (list[lpb] == *nb) {
10748         goto L2;
10749     }
10750     lpp = lpb;
10751     lpb = lptr[lpp];
10752     if (lpb != lpl) {
10753         goto L1;
10754     }
10755 
10756 /*   Test for error 2 (NB not found). */
10757 
10758     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10759         *lph = -2;
10760         return 0;
10761     }
10762 
10763 /*   NB is the last neighbor of N0.  Make NP the new last */
10764 /*     neighbor and, if NB is a boundary node, then make N0 */
10765 /*     a boundary node. */
10766 
10767     lend[*n0] = lpp;
10768     lp = lend[*nb];
10769     if (list[lp] < 0) {
10770         list[lpp] = -list[lpp];
10771     }
10772     goto L3;
10773 
10774 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10775 /*     node and N0 is not, then make N0 a boundary node with */
10776 /*     last neighbor NP. */
10777 
10778 L2:
10779     lp = lend[*nb];
10780     if (list[lp] < 0 && list[lpl] > 0) {
10781         lend[*n0] = lpp;
10782         list[lpp] = -list[lpp];
10783     }
10784 
10785 /*   Update LPTR so that the neighbor following NB now fol- */
10786 /*     lows NP, and fill in the hole at location LPB. */
10787 
10788 L3:
10789     lptr[lpp] = lptr[lpb];
10790     lnw = *lnew - 1;
10791     list[lpb] = list[lnw];
10792     lptr[lpb] = lptr[lnw];
10793     for (i__ = nn; i__ >= 1; --i__) {
10794         if (lend[i__] == lnw) {
10795             lend[i__] = lpb;
10796             goto L5;
10797         }
10798 /* L4: */
10799     }
10800 
10801 L5:
10802     i__1 = lnw - 1;
10803     for (i__ = 1; i__ <= i__1; ++i__) {
10804         if (lptr[i__] == lnw) {
10805             lptr[i__] = lpb;
10806         }
10807 /* L6: */
10808     }
10809 
10810 /* No errors encountered. */
10811 
10812     *lnew = lnw;
10813     *lph = lpb;
10814     return 0;
10815 } /* delnb_ */
10816 
10817 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10818         double *y, double *z__, int *list, int *lptr, int
10819         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10820 {
10821     /* System generated locals */
10822     int i__1;
10823 
10824     /* Local variables */
10825     static int i__, j, n1, n2;
10826     static double x1, x2, y1, y2, z1, z2;
10827     static int nl, lp, nn, nr;
10828     static double xl, yl, zl, xr, yr, zr;
10829     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10830     static long int bdry;
10831     static int ierr, lwkl;
10832     /* Subroutine */ int swap_(int *, int *, int *,
10833             int *, int *, int *, int *, int *), delnb_(
10834             int *, int *, int *, int *, int *, int *,
10835             int *, int *);
10836     int nbcnt_(int *, int *);
10837     /* Subroutine */ int optim_(double *, double *, double
10838             *, int *, int *, int *, int *, int *, int
10839             *, int *);
10840     static int nfrst;
10841     int lstptr_(int *, int *, int *, int *);
10842 
10843 
10844 /* *********************************************************** */
10845 
10846 /*                                              From STRIPACK */
10847 /*                                            Robert J. Renka */
10848 /*                                  Dept. of Computer Science */
10849 /*                                       Univ. of North Texas */
10850 /*                                           renka@cs.unt.edu */
10851 /*                                                   11/30/99 */
10852 
10853 /*   This subroutine deletes node K (along with all arcs */
10854 /* incident on node K) from a triangulation of N nodes on the */
10855 /* unit sphere, and inserts arcs as necessary to produce a */
10856 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10857 /* triangulation is input, a Delaunay triangulation will */
10858 /* result, and thus, DELNOD reverses the effect of a call to */
10859 /* Subroutine ADDNOD. */
10860 
10861 
10862 /* On input: */
10863 
10864 /*       K = Index (for X, Y, and Z) of the node to be */
10865 /*           deleted.  1 .LE. K .LE. N. */
10866 
10867 /* K is not altered by this routine. */
10868 
10869 /*       N = Number of nodes in the triangulation on input. */
10870 /*           N .GE. 4.  Note that N will be decremented */
10871 /*           following the deletion. */
10872 
10873 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10874 /*               coordinates of the nodes in the triangula- */
10875 /*               tion. */
10876 
10877 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10878 /*                             triangulation.  Refer to Sub- */
10879 /*                             routine TRMESH. */
10880 
10881 /*       LWK = Number of columns reserved for IWK.  LWK must */
10882 /*             be at least NNB-3, where NNB is the number of */
10883 /*             neighbors of node K, including an extra */
10884 /*             pseudo-node if K is a boundary node. */
10885 
10886 /*       IWK = int work array dimensioned 2 by LWK (or */
10887 /*             array of length .GE. 2*LWK). */
10888 
10889 /* On output: */
10890 
10891 /*       N = Number of nodes in the triangulation on output. */
10892 /*           The input value is decremented unless 1 .LE. IER */
10893 /*           .LE. 4. */
10894 
10895 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10896 /*               (with elements K+1,...,N+1 shifted up one */
10897 /*               position, thus overwriting element K) unless */
10898 /*               1 .LE. IER .LE. 4. */
10899 
10900 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10901 /*                             structure reflecting the dele- */
10902 /*                             tion unless 1 .LE. IER .LE. 4. */
10903 /*                             Note that the data structure */
10904 /*                             may have been altered if IER > */
10905 /*                             3. */
10906 
10907 /*       LWK = Number of IWK columns required unless IER = 1 */
10908 /*             or IER = 3. */
10909 
10910 /*       IWK = Indexes of the endpoints of the new arcs added */
10911 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10912 /*             are associated with columns, or pairs of */
10913 /*             adjacent elements if IWK is declared as a */
10914 /*             singly-subscripted array.) */
10915 
10916 /*       IER = Error indicator: */
10917 /*             IER = 0 if no errors were encountered. */
10918 /*             IER = 1 if K or N is outside its valid range */
10919 /*                     or LWK < 0 on input. */
10920 /*             IER = 2 if more space is required in IWK. */
10921 /*                     Refer to LWK. */
10922 /*             IER = 3 if the triangulation data structure is */
10923 /*                     invalid on input. */
10924 /*             IER = 4 if K indexes an interior node with */
10925 /*                     four or more neighbors, none of which */
10926 /*                     can be swapped out due to collineari- */
10927 /*                     ty, and K cannot therefore be deleted. */
10928 /*             IER = 5 if an error flag (other than IER = 1) */
10929 /*                     was returned by OPTIM.  An error */
10930 /*                     message is written to the standard */
10931 /*                     output unit in this case. */
10932 /*             IER = 6 if error flag 1 was returned by OPTIM. */
10933 /*                     This is not necessarily an error, but */
10934 /*                     the arcs may not be optimal. */
10935 
10936 /*   Note that the deletion may result in all remaining nodes */
10937 /* being collinear.  This situation is not flagged. */
10938 
10939 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
10940 /*                                OPTIM, SWAP, SWPTST */
10941 
10942 /* Intrinsic function called by DELNOD:  ABS */
10943 
10944 /* *********************************************************** */
10945 
10946 
10947 /* Local parameters: */
10948 
10949 /* BDRY =    long int variable with value TRUE iff N1 is a */
10950 /*             boundary node */
10951 /* I,J =     DO-loop indexes */
10952 /* IERR =    Error flag returned by OPTIM */
10953 /* IWL =     Number of IWK columns containing arcs */
10954 /* LNW =     Local copy of LNEW */
10955 /* LP =      LIST pointer */
10956 /* LP21 =    LIST pointer returned by SWAP */
10957 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
10958 /* LPH =     Pointer (or flag) returned by DELNB */
10959 /* LPL2 =    Pointer to the last neighbor of N2 */
10960 /* LPN =     Pointer to a neighbor of N1 */
10961 /* LWKL =    Input value of LWK */
10962 /* N1 =      Local copy of K */
10963 /* N2 =      Neighbor of N1 */
10964 /* NFRST =   First neighbor of N1:  LIST(LPF) */
10965 /* NIT =     Number of iterations in OPTIM */
10966 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
10967 /*             following (to the left of) N2, respectively */
10968 /* NN =      Number of nodes in the triangulation */
10969 /* NNB =     Number of neighbors of N1 (including a pseudo- */
10970 /*             node representing the boundary if N1 is a */
10971 /*             boundary node) */
10972 /* X1,Y1,Z1 = Coordinates of N1 */
10973 /* X2,Y2,Z2 = Coordinates of N2 */
10974 /* XL,YL,ZL = Coordinates of NL */
10975 /* XR,YR,ZR = Coordinates of NR */
10976 
10977 
10978 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
10979 /*   one if N1 is a boundary node), and test for errors.  LPF */
10980 /*   and LPL are LIST indexes of the first and last neighbors */
10981 /*   of N1, IWL is the number of IWK columns containing arcs, */
10982 /*   and BDRY is TRUE iff N1 is a boundary node. */
10983 
10984     /* Parameter adjustments */
10985     iwk -= 3;
10986     --lend;
10987     --lptr;
10988     --list;
10989     --z__;
10990     --y;
10991     --x;
10992 
10993     /* Function Body */
10994     n1 = *k;
10995     nn = *n;
10996     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10997         goto L21;
10998     }
10999     lpl = lend[n1];
11000     lpf = lptr[lpl];
11001     nnb = nbcnt_(&lpl, &lptr[1]);
11002     bdry = list[lpl] < 0;
11003     if (bdry) {
11004         ++nnb;
11005     }
11006     if (nnb < 3) {
11007         goto L23;
11008     }
11009     lwkl = *lwk;
11010     *lwk = nnb - 3;
11011     if (lwkl < *lwk) {
11012         goto L22;
11013     }
11014     iwl = 0;
11015     if (nnb == 3) {
11016         goto L3;
11017     }
11018 
11019 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
11020 /*   beginning with the second neighbor.  NR and NL are the */
11021 /*   neighbors preceding and following N2, respectively, and */
11022 /*   LP indexes NL.  The loop is exited when all possible */
11023 /*   swaps have been applied to arcs incident on N1. */
11024 
11025     x1 = x[n1];
11026     y1 = y[n1];
11027     z1 = z__[n1];
11028     nfrst = list[lpf];
11029     nr = nfrst;
11030     xr = x[nr];
11031     yr = y[nr];
11032     zr = z__[nr];
11033     lp = lptr[lpf];
11034     n2 = list[lp];
11035     x2 = x[n2];
11036     y2 = y[n2];
11037     z2 = z__[n2];
11038     lp = lptr[lp];
11039 
11040 /* Top of loop:  set NL to the neighbor following N2. */
11041 
11042 L1:
11043     nl = (i__1 = list[lp], abs(i__1));
11044     if (nl == nfrst && bdry) {
11045         goto L3;
11046     }
11047     xl = x[nl];
11048     yl = y[nl];
11049     zl = z__[nl];
11050 
11051 /*   Test for a convex quadrilateral.  To avoid an incorrect */
11052 /*     test caused by collinearity, use the fact that if N1 */
11053 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
11054 /*     a boundary node, then N2 LEFT NL->NR. */
11055 
11056     lpl2 = lend[n2];
11057     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
11058             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
11059             z2)))) {
11060 
11061 /*   Nonconvex quadrilateral -- no swap is possible. */
11062 
11063         nr = n2;
11064         xr = x2;
11065         yr = y2;
11066         zr = z2;
11067         goto L2;
11068     }
11069 
11070 /*   The quadrilateral defined by adjacent triangles */
11071 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
11072 /*     NL-NR and store it in IWK unless NL and NR are */
11073 /*     already adjacent, in which case the swap is not */
11074 /*     possible.  Indexes larger than N1 must be decremented */
11075 /*     since N1 will be deleted from X, Y, and Z. */
11076 
11077     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11078     if (lp21 == 0) {
11079         nr = n2;
11080         xr = x2;
11081         yr = y2;
11082         zr = z2;
11083         goto L2;
11084     }
11085     ++iwl;
11086     if (nl <= n1) {
11087         iwk[(iwl << 1) + 1] = nl;
11088     } else {
11089         iwk[(iwl << 1) + 1] = nl - 1;
11090     }
11091     if (nr <= n1) {
11092         iwk[(iwl << 1) + 2] = nr;
11093     } else {
11094         iwk[(iwl << 1) + 2] = nr - 1;
11095     }
11096 
11097 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
11098 
11099     lpl = lend[n1];
11100     --nnb;
11101     if (nnb == 3) {
11102         goto L3;
11103     }
11104     lpf = lptr[lpl];
11105     nfrst = list[lpf];
11106     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11107     if (nr == nfrst) {
11108         goto L2;
11109     }
11110 
11111 /*   NR is not the first neighbor of N1. */
11112 /*     Back up and test N1-NR for a swap again:  Set N2 to */
11113 /*     NR and NR to the previous neighbor of N1 -- the */
11114 /*     neighbor of NR which follows N1.  LP21 points to NL */
11115 /*     as a neighbor of NR. */
11116 
11117     n2 = nr;
11118     x2 = xr;
11119     y2 = yr;
11120     z2 = zr;
11121     lp21 = lptr[lp21];
11122     lp21 = lptr[lp21];
11123     nr = (i__1 = list[lp21], abs(i__1));
11124     xr = x[nr];
11125     yr = y[nr];
11126     zr = z__[nr];
11127     goto L1;
11128 
11129 /*   Bottom of loop -- test for termination of loop. */
11130 
11131 L2:
11132     if (n2 == nfrst) {
11133         goto L3;
11134     }
11135     n2 = nl;
11136     x2 = xl;
11137     y2 = yl;
11138     z2 = zl;
11139     lp = lptr[lp];
11140     goto L1;
11141 
11142 /* Delete N1 and all its incident arcs.  If N1 is an interior */
11143 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
11144 /*   then N1 must be separated from its neighbors by a plane */
11145 /*   containing the origin -- its removal reverses the effect */
11146 /*   of a call to COVSPH, and all its neighbors become */
11147 /*   boundary nodes.  This is achieved by treating it as if */
11148 /*   it were a boundary node (setting BDRY to TRUE, changing */
11149 /*   a sign in LIST, and incrementing NNB). */
11150 
11151 L3:
11152     if (! bdry) {
11153         if (nnb > 3) {
11154             bdry = TRUE_;
11155         } else {
11156             lpf = lptr[lpl];
11157             nr = list[lpf];
11158             lp = lptr[lpf];
11159             n2 = list[lp];
11160             nl = list[lpl];
11161             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11162                     x[n2], &y[n2], &z__[n2]);
11163         }
11164         if (bdry) {
11165 
11166 /*   IF a boundary node already exists, then N1 and its */
11167 /*     neighbors cannot be converted to boundary nodes. */
11168 /*     (They must be collinear.)  This is a problem if */
11169 /*     NNB > 3. */
11170 
11171             i__1 = nn;
11172             for (i__ = 1; i__ <= i__1; ++i__) {
11173                 if (list[lend[i__]] < 0) {
11174                     bdry = FALSE_;
11175                     goto L5;
11176                 }
11177 /* L4: */
11178             }
11179             list[lpl] = -list[lpl];
11180             ++nnb;
11181         }
11182     }
11183 L5:
11184     if (! bdry && nnb > 3) {
11185         goto L24;
11186     }
11187 
11188 /* Initialize for loop on neighbors.  LPL points to the last */
11189 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11190 
11191     lp = lpl;
11192     lnw = *lnew;
11193 
11194 /* Loop on neighbors N2 of N1, beginning with the first. */
11195 
11196 L6:
11197     lp = lptr[lp];
11198     n2 = (i__1 = list[lp], abs(i__1));
11199     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11200     if (lph < 0) {
11201         goto L23;
11202     }
11203 
11204 /*   LP and LPL may require alteration. */
11205 
11206     if (lpl == lnw) {
11207         lpl = lph;
11208     }
11209     if (lp == lnw) {
11210         lp = lph;
11211     }
11212     if (lp != lpl) {
11213         goto L6;
11214     }
11215 
11216 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11217 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11218 /*   which are larger than N1 must be decremented. */
11219 
11220     --nn;
11221     if (n1 > nn) {
11222         goto L9;
11223     }
11224     i__1 = nn;
11225     for (i__ = n1; i__ <= i__1; ++i__) {
11226         x[i__] = x[i__ + 1];
11227         y[i__] = y[i__ + 1];
11228         z__[i__] = z__[i__ + 1];
11229         lend[i__] = lend[i__ + 1];
11230 /* L7: */
11231     }
11232 
11233     i__1 = lnw - 1;
11234     for (i__ = 1; i__ <= i__1; ++i__) {
11235         if (list[i__] > n1) {
11236             --list[i__];
11237         }
11238         if (list[i__] < -n1) {
11239             ++list[i__];
11240         }
11241 /* L8: */
11242     }
11243 
11244 /*   For LPN = first to last neighbors of N1, delete the */
11245 /*     preceding neighbor (indexed by LP). */
11246 
11247 /*   Each empty LIST,LPTR location LP is filled in with the */
11248 /*     values at LNW-1, and LNW is decremented.  All pointers */
11249 /*     (including those in LPTR and LEND) with value LNW-1 */
11250 /*     must be changed to LP. */
11251 
11252 /*  LPL points to the last neighbor of N1. */
11253 
11254 L9:
11255     if (bdry) {
11256         --nnb;
11257     }
11258     lpn = lpl;
11259     i__1 = nnb;
11260     for (j = 1; j <= i__1; ++j) {
11261         --lnw;
11262         lp = lpn;
11263         lpn = lptr[lp];
11264         list[lp] = list[lnw];
11265         lptr[lp] = lptr[lnw];
11266         if (lptr[lpn] == lnw) {
11267             lptr[lpn] = lp;
11268         }
11269         if (lpn == lnw) {
11270             lpn = lp;
11271         }
11272         for (i__ = nn; i__ >= 1; --i__) {
11273             if (lend[i__] == lnw) {
11274                 lend[i__] = lp;
11275                 goto L11;
11276             }
11277 /* L10: */
11278         }
11279 
11280 L11:
11281         for (i__ = lnw - 1; i__ >= 1; --i__) {
11282             if (lptr[i__] == lnw) {
11283                 lptr[i__] = lp;
11284             }
11285 /* L12: */
11286         }
11287 /* L13: */
11288     }
11289 
11290 /* Update N and LNEW, and optimize the patch of triangles */
11291 /*   containing K (on input) by applying swaps to the arcs */
11292 /*   in IWK. */
11293 
11294     *n = nn;
11295     *lnew = lnw;
11296     if (iwl > 0) {
11297         nit = iwl << 2;
11298         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11299                 nit, &iwk[3], &ierr);
11300         if (ierr != 0 && ierr != 1) {
11301             goto L25;
11302         }
11303         if (ierr == 1) {
11304             goto L26;
11305         }
11306     }
11307 
11308 /* Successful termination. */
11309 
11310     *ier = 0;
11311     return 0;
11312 
11313 /* Invalid input parameter. */
11314 
11315 L21:
11316     *ier = 1;
11317     return 0;
11318 
11319 /* Insufficient space reserved for IWK. */
11320 
11321 L22:
11322     *ier = 2;
11323     return 0;
11324 
11325 /* Invalid triangulation data structure.  NNB < 3 on input or */
11326 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11327 
11328 L23:
11329     *ier = 3;
11330     return 0;
11331 
11332 /* N1 is interior but NNB could not be reduced to 3. */
11333 
11334 L24:
11335     *ier = 4;
11336     return 0;
11337 
11338 /* Error flag (other than 1) returned by OPTIM. */
11339 
11340 L25:
11341     *ier = 5;
11342 /*      WRITE (*,100) NIT, IERR */
11343 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11344 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11345     return 0;
11346 
11347 /* Error flag 1 returned by OPTIM. */
11348 
11349 L26:
11350     *ier = 6;
11351     return 0;
11352 } /* delnod_ */
11353 
11354 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11355         double *tol, int *nseg)
11356 {
11357     /* System generated locals */
11358     int i__1;
11359     double d__1;
11360 
11361     /* Builtin functions */
11362     //double sqrt(double);
11363 
11364     /* Local variables */
11365     static int i__, k;
11366     static double s, p1[3], p2[3], u1, u2, v1, v2;
11367     static int na;
11368     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11369 
11370 
11371 /* *********************************************************** */
11372 
11373 /*                                              From STRIPACK */
11374 /*                                            Robert J. Renka */
11375 /*                                  Dept. of Computer Science */
11376 /*                                       Univ. of North Texas */
11377 /*                                           renka@cs.unt.edu */
11378 /*                                                   03/04/03 */
11379 
11380 /*   Given unit vectors P and Q corresponding to northern */
11381 /* hemisphere points (with positive third components), this */
11382 /* subroutine draws a polygonal line which approximates the */
11383 /* projection of arc P-Q onto the plane containing the */
11384 /* equator. */
11385 
11386 /*   The line segment is drawn by writing a sequence of */
11387 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11388 /* is assumed that an open file is attached to the unit, */
11389 /* header comments have been written to the file, a window- */
11390 /* to-viewport mapping has been established, etc. */
11391 
11392 /* On input: */
11393 
11394 /*       LUN = long int unit number in the range 0 to 99. */
11395 
11396 /*       P,Q = Arrays of length 3 containing the endpoints of */
11397 /*             the arc to be drawn. */
11398 
11399 /*       TOL = Maximum distance in world coordinates between */
11400 /*             the projected arc and polygonal line. */
11401 
11402 /* Input parameters are not altered by this routine. */
11403 
11404 /* On output: */
11405 
11406 /*       NSEG = Number of line segments in the polygonal */
11407 /*              approximation to the projected arc.  This is */
11408 /*              a decreasing function of TOL.  NSEG = 0 and */
11409 /*              no drawing is performed if P = Q or P = -Q */
11410 /*              or an error is encountered in writing to unit */
11411 /*              LUN. */
11412 
11413 /* STRIPACK modules required by DRWARC:  None */
11414 
11415 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11416 
11417 /* *********************************************************** */
11418 
11419 
11420 /* Local parameters: */
11421 
11422 /* DP =    (Q-P)/NSEG */
11423 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11424 /*           onto the projection plane */
11425 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11426 /* ERR =   Orthogonal distance from the projected midpoint */
11427 /*           PM' to the line defined by P' and Q': */
11428 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11429 /* I,K =   DO-loop indexes */
11430 /* NA =    Number of arcs (segments) in the partition of P-Q */
11431 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11432 /*           arc P-Q into NSEG segments; obtained by normal- */
11433 /*           izing PM values */
11434 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11435 /*           uniform partition of the line segment P-Q into */
11436 /*           NSEG segments */
11437 /* S =     Scale factor 1/NA */
11438 /* U1,V1 = Components of P' */
11439 /* U2,V2 = Components of Q' */
11440 /* UM,VM = Components of the midpoint PM' */
11441 
11442 
11443 /* Compute the midpoint PM of arc P-Q. */
11444 
11445     /* Parameter adjustments */
11446     --q;
11447     --p;
11448 
11449     /* Function Body */
11450     enrm = 0.;
11451     for (i__ = 1; i__ <= 3; ++i__) {
11452         pm[i__ - 1] = p[i__] + q[i__];
11453         enrm += pm[i__ - 1] * pm[i__ - 1];
11454 /* L1: */
11455     }
11456     if (enrm == 0.) {
11457         goto L5;
11458     }
11459     enrm = sqrt(enrm);
11460     pm[0] /= enrm;
11461     pm[1] /= enrm;
11462     pm[2] /= enrm;
11463 
11464 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11465 /*   PM' = (UM,VM), respectively. */
11466 
11467     u1 = p[1];
11468     v1 = p[2];
11469     u2 = q[1];
11470     v2 = q[2];
11471     um = pm[0];
11472     vm = pm[1];
11473 
11474 /* Compute the orthogonal distance ERR from PM' to the line */
11475 /*   defined by P' and Q'.  This is the maximum deviation */
11476 /*   between the projected arc and the line segment.  It is */
11477 /*   undefined if P' = Q'. */
11478 
11479     du = u2 - u1;
11480     dv = v2 - v1;
11481     enrm = du * du + dv * dv;
11482     if (enrm == 0.) {
11483         goto L5;
11484     }
11485     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11486 
11487 /* Compute the number of arcs into which P-Q will be parti- */
11488 /*   tioned (the number of line segments to be drawn): */
11489 /*   NA = ERR/TOL. */
11490 
11491     na = (int) (err / *tol + 1.);
11492 
11493 /* Initialize for loop on arcs P1-P2, where the intermediate */
11494 /*   points are obtained by normalizing PM = P + k*DP for */
11495 /*   DP = (Q-P)/NA */
11496 
11497     s = 1. / (double) na;
11498     for (i__ = 1; i__ <= 3; ++i__) {
11499         dp[i__ - 1] = s * (q[i__] - p[i__]);
11500         pm[i__ - 1] = p[i__];
11501         p1[i__ - 1] = p[i__];
11502 /* L2: */
11503     }
11504 
11505 /* Loop on arcs P1-P2, drawing the line segments associated */
11506 /*   with the projected endpoints. */
11507 
11508     i__1 = na - 1;
11509     for (k = 1; k <= i__1; ++k) {
11510         enrm = 0.;
11511         for (i__ = 1; i__ <= 3; ++i__) {
11512             pm[i__ - 1] += dp[i__ - 1];
11513             enrm += pm[i__ - 1] * pm[i__ - 1];
11514 /* L3: */
11515         }
11516         if (enrm == 0.) {
11517             goto L5;
11518         }
11519         enrm = sqrt(enrm);
11520         p2[0] = pm[0] / enrm;
11521         p2[1] = pm[1] / enrm;
11522         p2[2] = pm[2] / enrm;
11523 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11524 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11525         p1[0] = p2[0];
11526         p1[1] = p2[1];
11527         p1[2] = p2[2];
11528 /* L4: */
11529     }
11530 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11531 
11532 /* No error encountered. */
11533 
11534     *nseg = na;
11535     return 0;
11536 
11537 /* Invalid input value of P or Q. */
11538 
11539 L5:
11540     *nseg = 0;
11541     return 0;
11542 } /* drwarc_ */
11543 
11544 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11545         double *y, double *z__, int *lwk, int *iwk, int *
11546         list, int *lptr, int *lend, int *ier)
11547 {
11548     /* System generated locals */
11549     int i__1;
11550 
11551     /* Local variables */
11552     static int i__, n0, n1, n2;
11553     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11554     static int nl, lp, nr;
11555     static double dp12;
11556     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11557     static double dp1l, dp2l, dp1r, dp2r;
11558     static int ierr;
11559     /* Subroutine */ int swap_(int *, int *, int *,
11560             int *, int *, int *, int *, int *);
11561     static int next, iwcp1, n1lst, iwend;
11562     /* Subroutine */ int optim_(double *, double *, double
11563             *, int *, int *, int *, int *, int *, int
11564             *, int *);
11565     static int n1frst;
11566 
11567 
11568 /* *********************************************************** */
11569 
11570 /*                                              From STRIPACK */
11571 /*                                            Robert J. Renka */
11572 /*                                  Dept. of Computer Science */
11573 /*                                       Univ. of North Texas */
11574 /*                                           renka@cs.unt.edu */
11575 /*                                                   07/30/98 */
11576 
11577 /*   Given a triangulation of N nodes and a pair of nodal */
11578 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11579 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11580 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11581 /* lation is input, the resulting triangulation is as close */
11582 /* as possible to a Delaunay triangulation in the sense that */
11583 /* all arcs other than IN1-IN2 are locally optimal. */
11584 
11585 /*   A sequence of calls to EDGE may be used to force the */
11586 /* presence of a set of edges defining the boundary of a non- */
11587 /* convex and/or multiply connected region, or to introduce */
11588 /* barriers into the triangulation.  Note that Subroutine */
11589 /* GETNP will not necessarily return closest nodes if the */
11590 /* triangulation has been constrained by a call to EDGE. */
11591 /* However, this is appropriate in some applications, such */
11592 /* as triangle-based interpolation on a nonconvex domain. */
11593 
11594 
11595 /* On input: */
11596 
11597 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11598 /*                 N defining a pair of nodes to be connected */
11599 /*                 by an arc. */
11600 
11601 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11602 /*               coordinates of the nodes. */
11603 
11604 /* The above parameters are not altered by this routine. */
11605 
11606 /*       LWK = Number of columns reserved for IWK.  This must */
11607 /*             be at least NI -- the number of arcs that */
11608 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11609 
11610 /*       IWK = int work array of length at least 2*LWK. */
11611 
11612 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11613 /*                        gulation.  Refer to Subroutine */
11614 /*                        TRMESH. */
11615 
11616 /* On output: */
11617 
11618 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11619 /*             not more than the input value of LWK) unless */
11620 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11621 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11622 
11623 /*       IWK = Array containing the indexes of the endpoints */
11624 /*             of the new arcs other than IN1-IN2 unless */
11625 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11626 /*             IN1->IN2 are stored in the first K-1 columns */
11627 /*             (left portion of IWK), column K contains */
11628 /*             zeros, and new arcs to the right of IN1->IN2 */
11629 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11630 /*             mined by searching IWK for the zeros.) */
11631 
11632 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11633 /*                        to reflect the presence of an arc */
11634 /*                        connecting IN1 and IN2 unless IER > */
11635 /*                        0.  The data structure has been */
11636 /*                        altered if IER >= 4. */
11637 
11638 /*       IER = Error indicator: */
11639 /*             IER = 0 if no errors were encountered. */
11640 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11641 /*                     or LWK < 0 on input. */
11642 /*             IER = 2 if more space is required in IWK. */
11643 /*                     Refer to LWK. */
11644 /*             IER = 3 if IN1 and IN2 could not be connected */
11645 /*                     due to either an invalid data struc- */
11646 /*                     ture or collinear nodes (and floating */
11647 /*                     point error). */
11648 /*             IER = 4 if an error flag other than IER = 1 */
11649 /*                     was returned by OPTIM. */
11650 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11651 /*                     This is not necessarily an error, but */
11652 /*                     the arcs other than IN1-IN2 may not */
11653 /*                     be optimal. */
11654 
11655 /*   An error message is written to the standard output unit */
11656 /* in the case of IER = 3 or IER = 4. */
11657 
11658 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11659 /*                              SWPTST */
11660 
11661 /* Intrinsic function called by EDGE:  ABS */
11662 
11663 /* *********************************************************** */
11664 
11665 
11666 /* Local parameters: */
11667 
11668 /* DPij =     Dot product <Ni,Nj> */
11669 /* I =        DO-loop index and column index for IWK */
11670 /* IERR =     Error flag returned by Subroutine OPTIM */
11671 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11672 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11673 /* IWCP1 =    IWC + 1 */
11674 /* IWEND =    Input or output value of LWK */
11675 /* IWF =      IWK (column) index of the first (leftmost) arc */
11676 /*              which intersects IN1->IN2 */
11677 /* IWL =      IWK (column) index of the last (rightmost) are */
11678 /*              which intersects IN1->IN2 */
11679 /* LFT =      Flag used to determine if a swap results in the */
11680 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11681 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11682 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11683 /* LP =       List pointer (index for LIST and LPTR) */
11684 /* LP21 =     Unused parameter returned by SWAP */
11685 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11686 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11687 /* N1,N2 =    Local copies of IN1 and IN2 */
11688 /* N1FRST =   First neighbor of IN1 */
11689 /* N1LST =    (Signed) last neighbor of IN1 */
11690 /* NEXT =     Node opposite NL->NR */
11691 /* NIT =      Flag or number of iterations employed by OPTIM */
11692 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11693 /*              with NL LEFT IN1->IN2 */
11694 /* X0,Y0,Z0 = Coordinates of N0 */
11695 /* X1,Y1,Z1 = Coordinates of IN1 */
11696 /* X2,Y2,Z2 = Coordinates of IN2 */
11697 
11698 
11699 /* Store IN1, IN2, and LWK in local variables and test for */
11700 /*   errors. */
11701 
11702     /* Parameter adjustments */
11703     --lend;
11704     --lptr;
11705     --list;
11706     iwk -= 3;
11707     --z__;
11708     --y;
11709     --x;
11710 
11711     /* Function Body */
11712     n1 = *in1;
11713     n2 = *in2;
11714     iwend = *lwk;
11715     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11716         goto L31;
11717     }
11718 
11719 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11720 /*   neighbor of N1. */
11721 
11722     lpl = lend[n1];
11723     n0 = (i__1 = list[lpl], abs(i__1));
11724     lp = lpl;
11725 L1:
11726     if (n0 == n2) {
11727         goto L30;
11728     }
11729     lp = lptr[lp];
11730     n0 = list[lp];
11731     if (lp != lpl) {
11732         goto L1;
11733     }
11734 
11735 /* Initialize parameters. */
11736 
11737     iwl = 0;
11738     nit = 0;
11739 
11740 /* Store the coordinates of N1 and N2. */
11741 
11742 L2:
11743     x1 = x[n1];
11744     y1 = y[n1];
11745     z1 = z__[n1];
11746     x2 = x[n2];
11747     y2 = y[n2];
11748     z2 = z__[n2];
11749 
11750 /* Set NR and NL to adjacent neighbors of N1 such that */
11751 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11752 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11753 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11754 
11755 /*   Initialization:  Set N1FRST and N1LST to the first and */
11756 /*     (signed) last neighbors of N1, respectively, and */
11757 /*     initialize NL to N1FRST. */
11758 
11759     lpl = lend[n1];
11760     n1lst = list[lpl];
11761     lp = lptr[lpl];
11762     n1frst = list[lp];
11763     nl = n1frst;
11764     if (n1lst < 0) {
11765         goto L4;
11766     }
11767 
11768 /*   N1 is an interior node.  Set NL to the first candidate */
11769 /*     for NR (NL LEFT N2->N1). */
11770 
11771 L3:
11772     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11773         goto L4;
11774     }
11775     lp = lptr[lp];
11776     nl = list[lp];
11777     if (nl != n1frst) {
11778         goto L3;
11779     }
11780 
11781 /*   All neighbors of N1 are strictly left of N1->N2. */
11782 
11783     goto L5;
11784 
11785 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11786 /*     following neighbor of N1. */
11787 
11788 L4:
11789     nr = nl;
11790     lp = lptr[lp];
11791     nl = (i__1 = list[lp], abs(i__1));
11792     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11793 
11794 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11795 /*     are employed to avoid an error associated with */
11796 /*     collinear nodes. */
11797 
11798         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11799         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11800         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11801         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11802         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11803         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11804                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11805             goto L6;
11806         }
11807 
11808 /*   NL-NR does not intersect N1-N2.  However, there is */
11809 /*     another candidate for the first arc if NL lies on */
11810 /*     the line N1-N2. */
11811 
11812         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11813             goto L5;
11814         }
11815     }
11816 
11817 /*   Bottom of loop. */
11818 
11819     if (nl != n1frst) {
11820         goto L4;
11821     }
11822 
11823 /* Either the triangulation is invalid or N1-N2 lies on the */
11824 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11825 /*   intersecting N1-N2) was not found due to floating point */
11826 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11827 /*   has already been done. */
11828 
11829 L5:
11830     if (nit > 0) {
11831         goto L33;
11832     }
11833     nit = 1;
11834     n1 = n2;
11835     n2 = *in1;
11836     goto L2;
11837 
11838 /* Store the ordered sequence of intersecting edges NL->NR in */
11839 /*   IWK(1,IWL)->IWK(2,IWL). */
11840 
11841 L6:
11842     ++iwl;
11843     if (iwl > iwend) {
11844         goto L32;
11845     }
11846     iwk[(iwl << 1) + 1] = nl;
11847     iwk[(iwl << 1) + 2] = nr;
11848 
11849 /*   Set NEXT to the neighbor of NL which follows NR. */
11850 
11851     lpl = lend[nl];
11852     lp = lptr[lpl];
11853 
11854 /*   Find NR as a neighbor of NL.  The search begins with */
11855 /*     the first neighbor. */
11856 
11857 L7:
11858     if (list[lp] == nr) {
11859         goto L8;
11860     }
11861     lp = lptr[lp];
11862     if (lp != lpl) {
11863         goto L7;
11864     }
11865 
11866 /*   NR must be the last neighbor, and NL->NR cannot be a */
11867 /*     boundary edge. */
11868 
11869     if (list[lp] != nr) {
11870         goto L33;
11871     }
11872 
11873 /*   Set NEXT to the neighbor following NR, and test for */
11874 /*     termination of the store loop. */
11875 
11876 L8:
11877     lp = lptr[lp];
11878     next = (i__1 = list[lp], abs(i__1));
11879     if (next == n2) {
11880         goto L9;
11881     }
11882 
11883 /*   Set NL or NR to NEXT. */
11884 
11885     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11886         nl = next;
11887     } else {
11888         nr = next;
11889     }
11890     goto L6;
11891 
11892 /* IWL is the number of arcs which intersect N1-N2. */
11893 /*   Store LWK. */
11894 
11895 L9:
11896     *lwk = iwl;
11897     iwend = iwl;
11898 
11899 /* Initialize for edge swapping loop -- all possible swaps */
11900 /*   are applied (even if the new arc again intersects */
11901 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11902 /*   left portion of IWK, and arcs to the right are stored in */
11903 /*   the right portion.  IWF and IWL index the first and last */
11904 /*   intersecting arcs. */
11905 
11906     iwf = 1;
11907 
11908 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11909 /*   IWC points to the arc currently being processed.  LFT */
11910 /*   .LE. 0 iff N0 LEFT N1->N2. */
11911 
11912 L10:
11913     lft = 0;
11914     n0 = n1;
11915     x0 = x1;
11916     y0 = y1;
11917     z0 = z1;
11918     nl = iwk[(iwf << 1) + 1];
11919     nr = iwk[(iwf << 1) + 2];
11920     iwc = iwf;
11921 
11922 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
11923 /*     last arc. */
11924 
11925 L11:
11926     if (iwc == iwl) {
11927         goto L21;
11928     }
11929     iwcp1 = iwc + 1;
11930     next = iwk[(iwcp1 << 1) + 1];
11931     if (next != nl) {
11932         goto L16;
11933     }
11934     next = iwk[(iwcp1 << 1) + 2];
11935 
11936 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
11937 /*     swap. */
11938 
11939     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11940             z__[next])) {
11941         goto L14;
11942     }
11943     if (lft >= 0) {
11944         goto L12;
11945     }
11946     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11947             z__[next])) {
11948         goto L14;
11949     }
11950 
11951 /*   Replace NL->NR with N0->NEXT. */
11952 
11953     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11954     iwk[(iwc << 1) + 1] = n0;
11955     iwk[(iwc << 1) + 2] = next;
11956     goto L15;
11957 
11958 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
11959 /*     the left, and store N0-NEXT in the right portion of */
11960 /*     IWK. */
11961 
11962 L12:
11963     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11964     i__1 = iwl;
11965     for (i__ = iwcp1; i__ <= i__1; ++i__) {
11966         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11967         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11968 /* L13: */
11969     }
11970     iwk[(iwl << 1) + 1] = n0;
11971     iwk[(iwl << 1) + 2] = next;
11972     --iwl;
11973     nr = next;
11974     goto L11;
11975 
11976 /*   A swap is not possible.  Set N0 to NR. */
11977 
11978 L14:
11979     n0 = nr;
11980     x0 = x[n0];
11981     y0 = y[n0];
11982     z0 = z__[n0];
11983     lft = 1;
11984 
11985 /*   Advance to the next arc. */
11986 
11987 L15:
11988     nr = next;
11989     ++iwc;
11990     goto L11;
11991 
11992 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
11993 /*     Test for a possible swap. */
11994 
11995 L16:
11996     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11997             z__[next])) {
11998         goto L19;
11999     }
12000     if (lft <= 0) {
12001         goto L17;
12002     }
12003     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12004             z__[next])) {
12005         goto L19;
12006     }
12007 
12008 /*   Replace NL->NR with NEXT->N0. */
12009 
12010     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12011     iwk[(iwc << 1) + 1] = next;
12012     iwk[(iwc << 1) + 2] = n0;
12013     goto L20;
12014 
12015 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
12016 /*     the right, and store N0-NEXT in the left portion of */
12017 /*     IWK. */
12018 
12019 L17:
12020     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12021     i__1 = iwf;
12022     for (i__ = iwc - 1; i__ >= i__1; --i__) {
12023         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12024         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12025 /* L18: */
12026     }
12027     iwk[(iwf << 1) + 1] = n0;
12028     iwk[(iwf << 1) + 2] = next;
12029     ++iwf;
12030     goto L20;
12031 
12032 /*   A swap is not possible.  Set N0 to NL. */
12033 
12034 L19:
12035     n0 = nl;
12036     x0 = x[n0];
12037     y0 = y[n0];
12038     z0 = z__[n0];
12039     lft = -1;
12040 
12041 /*   Advance to the next arc. */
12042 
12043 L20:
12044     nl = next;
12045     ++iwc;
12046     goto L11;
12047 
12048 /*   N2 is opposite NL->NR (IWC = IWL). */
12049 
12050 L21:
12051     if (n0 == n1) {
12052         goto L24;
12053     }
12054     if (lft < 0) {
12055         goto L22;
12056     }
12057 
12058 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
12059 
12060     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
12061         goto L10;
12062     }
12063 
12064 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
12065 /*     portion of IWK. */
12066 
12067     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12068     iwk[(iwl << 1) + 1] = n0;
12069     iwk[(iwl << 1) + 2] = n2;
12070     --iwl;
12071     goto L10;
12072 
12073 /*   N0 LEFT N1->N2.  Test for a possible swap. */
12074 
12075 L22:
12076     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12077         goto L10;
12078     }
12079 
12080 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
12081 /*     right, and store N0-N2 in the left portion of IWK. */
12082 
12083     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12084     i__ = iwl;
12085 L23:
12086     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12087     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12088     --i__;
12089     if (i__ > iwf) {
12090         goto L23;
12091     }
12092     iwk[(iwf << 1) + 1] = n0;
12093     iwk[(iwf << 1) + 2] = n2;
12094     ++iwf;
12095     goto L10;
12096 
12097 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
12098 /*   store zeros in IWK. */
12099 
12100 L24:
12101     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12102     iwk[(iwc << 1) + 1] = 0;
12103     iwk[(iwc << 1) + 2] = 0;
12104 
12105 /* Optimization procedure -- */
12106 
12107     *ier = 0;
12108     if (iwc > 1) {
12109 
12110 /*   Optimize the set of new arcs to the left of IN1->IN2. */
12111 
12112         nit = iwc - (1<<2);
12113         i__1 = iwc - 1;
12114         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12115                 nit, &iwk[3], &ierr);
12116         if (ierr != 0 && ierr != 1) {
12117             goto L34;
12118         }
12119         if (ierr == 1) {
12120             *ier = 5;
12121         }
12122     }
12123     if (iwc < iwend) {
12124 
12125 /*   Optimize the set of new arcs to the right of IN1->IN2. */
12126 
12127         nit = iwend - (iwc<<2);
12128         i__1 = iwend - iwc;
12129         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12130                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12131         if (ierr != 0 && ierr != 1) {
12132             goto L34;
12133         }
12134         if (ierr == 1) {
12135             goto L35;
12136         }
12137     }
12138     if (*ier == 5) {
12139         goto L35;
12140     }
12141 
12142 /* Successful termination (IER = 0). */
12143 
12144     return 0;
12145 
12146 /* IN1 and IN2 were adjacent on input. */
12147 
12148 L30:
12149     *ier = 0;
12150     return 0;
12151 
12152 /* Invalid input parameter. */
12153 
12154 L31:
12155     *ier = 1;
12156     return 0;
12157 
12158 /* Insufficient space reserved for IWK. */
12159 
12160 L32:
12161     *ier = 2;
12162     return 0;
12163 
12164 /* Invalid triangulation data structure or collinear nodes */
12165 /*   on convex hull boundary. */
12166 
12167 L33:
12168     *ier = 3;
12169 /*      WRITE (*,130) IN1, IN2 */
12170 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12171 /*     .        'tion or null triangles on boundary'/ */
12172 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12173     return 0;
12174 
12175 /* Error flag (other than 1) returned by OPTIM. */
12176 
12177 L34:
12178     *ier = 4;
12179 /*      WRITE (*,140) NIT, IERR */
12180 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12181 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12182     return 0;
12183 
12184 /* Error flag 1 returned by OPTIM. */
12185 
12186 L35:
12187     *ier = 5;
12188     return 0;
12189 } /* edge_ */
12190 
12191 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12192         int *list, int *lptr, int *lend, int *l, int *
12193         npts, double *df, int *ier)
12194 {
12195     /* System generated locals */
12196     int i__1, i__2;
12197 
12198     /* Local variables */
12199     static int i__, n1;
12200     static double x1, y1, z1;
12201     static int nb, ni, lp, np, lm1;
12202     static double dnb, dnp;
12203     static int lpl;
12204 
12205 
12206 /* *********************************************************** */
12207 
12208 /*                                              From STRIPACK */
12209 /*                                            Robert J. Renka */
12210 /*                                  Dept. of Computer Science */
12211 /*                                       Univ. of North Texas */
12212 /*                                           renka@cs.unt.edu */
12213 /*                                                   07/28/98 */
12214 
12215 /*   Given a Delaunay triangulation of N nodes on the unit */
12216 /* sphere and an array NPTS containing the indexes of L-1 */
12217 /* nodes ordered by angular distance from NPTS(1), this sub- */
12218 /* routine sets NPTS(L) to the index of the next node in the */
12219 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12220 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12221 /* of K closest nodes to N1 (including N1) may be determined */
12222 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12223 /* for K .GE. 2. */
12224 
12225 /*   The algorithm uses the property of a Delaunay triangula- */
12226 /* tion that the K-th closest node to N1 is a neighbor of one */
12227 /* of the K-1 closest nodes to N1. */
12228 
12229 
12230 /* On input: */
12231 
12232 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12233 /*               coordinates of the nodes. */
12234 
12235 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12236 /*                        fer to Subroutine TRMESH. */
12237 
12238 /*       L = Number of nodes in the sequence on output.  2 */
12239 /*           .LE. L .LE. N. */
12240 
12241 /* The above parameters are not altered by this routine. */
12242 
12243 /*       NPTS = Array of length .GE. L containing the indexes */
12244 /*              of the L-1 closest nodes to NPTS(1) in the */
12245 /*              first L-1 locations. */
12246 
12247 /* On output: */
12248 
12249 /*       NPTS = Array updated with the index of the L-th */
12250 /*              closest node to NPTS(1) in position L unless */
12251 /*              IER = 1. */
12252 
12253 /*       DF = Value of an increasing function (negative cos- */
12254 /*            ine) of the angular distance between NPTS(1) */
12255 /*            and NPTS(L) unless IER = 1. */
12256 
12257 /*       IER = Error indicator: */
12258 /*             IER = 0 if no errors were encountered. */
12259 /*             IER = 1 if L < 2. */
12260 
12261 /* Modules required by GETNP:  None */
12262 
12263 /* Intrinsic function called by GETNP:  ABS */
12264 
12265 /* *********************************************************** */
12266 
12267 
12268 /* Local parameters: */
12269 
12270 /* DNB,DNP =  Negative cosines of the angular distances from */
12271 /*              N1 to NB and to NP, respectively */
12272 /* I =        NPTS index and DO-loop index */
12273 /* LM1 =      L-1 */
12274 /* LP =       LIST pointer of a neighbor of NI */
12275 /* LPL =      Pointer to the last neighbor of NI */
12276 /* N1 =       NPTS(1) */
12277 /* NB =       Neighbor of NI and candidate for NP */
12278 /* NI =       NPTS(I) */
12279 /* NP =       Candidate for NPTS(L) */
12280 /* X1,Y1,Z1 = Coordinates of N1 */
12281 
12282     /* Parameter adjustments */
12283     --x;
12284     --y;
12285     --z__;
12286     --list;
12287     --lptr;
12288     --lend;
12289     --npts;
12290 
12291     /* Function Body */
12292     lm1 = *l - 1;
12293     if (lm1 < 1) {
12294         goto L6;
12295     }
12296     *ier = 0;
12297 
12298 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12299 
12300     n1 = npts[1];
12301     x1 = x[n1];
12302     y1 = y[n1];
12303     z1 = z__[n1];
12304     i__1 = lm1;
12305     for (i__ = 1; i__ <= i__1; ++i__) {
12306         ni = npts[i__];
12307         lend[ni] = -lend[ni];
12308 /* L1: */
12309     }
12310 
12311 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12312 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12313 /*   (the maximum distance). */
12314 
12315     dnp = 2.;
12316 
12317 /* Loop on nodes NI in NPTS. */
12318 
12319     i__1 = lm1;
12320     for (i__ = 1; i__ <= i__1; ++i__) {
12321         ni = npts[i__];
12322         lpl = -lend[ni];
12323         lp = lpl;
12324 
12325 /* Loop on neighbors NB of NI. */
12326 
12327 L2:
12328         nb = (i__2 = list[lp], abs(i__2));
12329         if (lend[nb] < 0) {
12330             goto L3;
12331         }
12332 
12333 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12334 /*   closer to N1. */
12335 
12336         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12337         if (dnb >= dnp) {
12338             goto L3;
12339         }
12340         np = nb;
12341         dnp = dnb;
12342 L3:
12343         lp = lptr[lp];
12344         if (lp != lpl) {
12345             goto L2;
12346         }
12347 /* L4: */
12348     }
12349     npts[*l] = np;
12350     *df = dnp;
12351 
12352 /* Unmark the elements of NPTS. */
12353 
12354     i__1 = lm1;
12355     for (i__ = 1; i__ <= i__1; ++i__) {
12356         ni = npts[i__];
12357         lend[ni] = -lend[ni];
12358 /* L5: */
12359     }
12360     return 0;
12361 
12362 /* L is outside its valid range. */
12363 
12364 L6:
12365     *ier = 1;
12366     return 0;
12367 } /* getnp_ */
12368 
12369 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12370         lptr, int *lnew)
12371 {
12372     static int lsav;
12373 
12374 
12375 /* *********************************************************** */
12376 
12377 /*                                              From STRIPACK */
12378 /*                                            Robert J. Renka */
12379 /*                                  Dept. of Computer Science */
12380 /*                                       Univ. of North Texas */
12381 /*                                           renka@cs.unt.edu */
12382 /*                                                   07/17/96 */
12383 
12384 /*   This subroutine inserts K as a neighbor of N1 following */
12385 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12386 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12387 /* become the first neighbor (even if N1 is a boundary node). */
12388 
12389 /*   This routine is identical to the similarly named routine */
12390 /* in TRIPACK. */
12391 
12392 
12393 /* On input: */
12394 
12395 /*       K = Index of the node to be inserted. */
12396 
12397 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12398 
12399 /* The above parameters are not altered by this routine. */
12400 
12401 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12402 /*                        gulation.  Refer to Subroutine */
12403 /*                        TRMESH. */
12404 
12405 /* On output: */
12406 
12407 /*       LIST,LPTR,LNEW = Data structure updated with the */
12408 /*                        addition of node K. */
12409 
12410 /* Modules required by INSERT:  None */
12411 
12412 /* *********************************************************** */
12413 
12414 
12415     /* Parameter adjustments */
12416     --lptr;
12417     --list;
12418 
12419     /* Function Body */
12420     lsav = lptr[*lp];
12421     lptr[*lp] = *lnew;
12422     list[*lnew] = *k;
12423     lptr[*lnew] = lsav;
12424     ++(*lnew);
12425     return 0;
12426 } /* insert_ */
12427 
12428 long int inside_(double *p, int *lv, double *xv, double *yv,
12429         double *zv, int *nv, int *listv, int *ier)
12430 {
12431     /* Initialized data */
12432 
12433     static double eps = .001;
12434 
12435     /* System generated locals */
12436     int i__1;
12437     long int ret_val = 0;
12438 
12439     /* Builtin functions */
12440     //double sqrt(double);
12441 
12442     /* Local variables */
12443     static double b[3], d__;
12444     static int k, n;
12445     static double q[3];
12446     static int i1, i2, k0;
12447     static double v1[3], v2[3], cn[3], bp, bq;
12448     static int ni;
12449     static double pn[3], qn[3], vn[3];
12450     static int imx;
12451     static long int lft1, lft2, even;
12452     static int ierr;
12453     static long int pinr, qinr;
12454     static double qnrm, vnrm;
12455     /* Subroutine */ int intrsc_(double *, double *,
12456             double *, double *, int *);
12457 
12458 
12459 /* *********************************************************** */
12460 
12461 /*                                              From STRIPACK */
12462 /*                                            Robert J. Renka */
12463 /*                                  Dept. of Computer Science */
12464 /*                                       Univ. of North Texas */
12465 /*                                           renka@cs.unt.edu */
12466 /*                                                   12/27/93 */
12467 
12468 /*   This function locates a point P relative to a polygonal */
12469 /* region R on the surface of the unit sphere, returning */
12470 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12471 /* defined by a cyclically ordered sequence of vertices which */
12472 /* form a positively-oriented simple closed curve.  Adjacent */
12473 /* vertices need not be distinct but the curve must not be */
12474 /* self-intersecting.  Also, while polygon edges are by defi- */
12475 /* nition restricted to a single hemisphere, R is not so */
12476 /* restricted.  Its interior is the region to the left as the */
12477 /* vertices are traversed in order. */
12478 
12479 /*   The algorithm consists of selecting a point Q in R and */
12480 /* then finding all points at which the great circle defined */
12481 /* by P and Q intersects the boundary of R.  P lies inside R */
12482 /* if and only if there is an even number of intersection */
12483 /* points between Q and P.  Q is taken to be a point immedi- */
12484 /* ately to the left of a directed boundary edge -- the first */
12485 /* one that results in no consistency-check failures. */
12486 
12487 /*   If P is close to the polygon boundary, the problem is */
12488 /* ill-conditioned and the decision may be incorrect.  Also, */
12489 /* an incorrect decision may result from a poor choice of Q */
12490 /* (if, for example, a boundary edge lies on the great cir- */
12491 /* cle defined by P and Q).  A more reliable result could be */
12492 /* obtained by a sequence of calls to INSIDE with the ver- */
12493 /* tices cyclically permuted before each call (to alter the */
12494 /* choice of Q). */
12495 
12496 
12497 /* On input: */
12498 
12499 /*       P = Array of length 3 containing the Cartesian */
12500 /*           coordinates of the point (unit vector) to be */
12501 /*           located. */
12502 
12503 /*       LV = Length of arrays XV, YV, and ZV. */
12504 
12505 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12506 /*                  sian coordinates of unit vectors (points */
12507 /*                  on the unit sphere).  These values are */
12508 /*                  not tested for validity. */
12509 
12510 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12511 /*            .LE. LV. */
12512 
12513 /*       LISTV = Array of length NV containing the indexes */
12514 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12515 /*               (and CCW-ordered) sequence of vertices that */
12516 /*               define R.  The last vertex (indexed by */
12517 /*               LISTV(NV)) is followed by the first (indexed */
12518 /*               by LISTV(1)).  LISTV entries must be in the */
12519 /*               range 1 to LV. */
12520 
12521 /* Input parameters are not altered by this function. */
12522 
12523 /* On output: */
12524 
12525 /*       INSIDE = TRUE if and only if P lies inside R unless */
12526 /*                IER .NE. 0, in which case the value is not */
12527 /*                altered. */
12528 
12529 /*       IER = Error indicator: */
12530 /*             IER = 0 if no errors were encountered. */
12531 /*             IER = 1 if LV or NV is outside its valid */
12532 /*                     range. */
12533 /*             IER = 2 if a LISTV entry is outside its valid */
12534 /*                     range. */
12535 /*             IER = 3 if the polygon boundary was found to */
12536 /*                     be self-intersecting.  This error will */
12537 /*                     not necessarily be detected. */
12538 /*             IER = 4 if every choice of Q (one for each */
12539 /*                     boundary edge) led to failure of some */
12540 /*                     internal consistency check.  The most */
12541 /*                     likely cause of this error is invalid */
12542 /*                     input:  P = (0,0,0), a null or self- */
12543 /*                     intersecting polygon, etc. */
12544 
12545 /* Module required by INSIDE:  INTRSC */
12546 
12547 /* Intrinsic function called by INSIDE:  SQRT */
12548 
12549 /* *********************************************************** */
12550 
12551 
12552 /* Local parameters: */
12553 
12554 /* B =         Intersection point between the boundary and */
12555 /*               the great circle defined by P and Q */
12556 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12557 /*               intersection points B that lie between P and */
12558 /*               Q (on the shorter arc) -- used to find the */
12559 /*               closest intersection points to P and Q */
12560 /* CN =        Q X P = normal to the plane of P and Q */
12561 /* D =         Dot product <B,P> or <B,Q> */
12562 /* EPS =       Parameter used to define Q as the point whose */
12563 /*               orthogonal distance to (the midpoint of) */
12564 /*               boundary edge V1->V2 is approximately EPS/ */
12565 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12566 /* EVEN =      TRUE iff an even number of intersection points */
12567 /*               lie between P and Q (on the shorter arc) */
12568 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12569 /*               boundary vertices (endpoints of a boundary */
12570 /*               edge) */
12571 /* IERR =      Error flag for calls to INTRSC (not tested) */
12572 /* IMX =       Local copy of LV and maximum value of I1 and */
12573 /*               I2 */
12574 /* K =         DO-loop index and LISTV index */
12575 /* K0 =        LISTV index of the first endpoint of the */
12576 /*               boundary edge used to compute Q */
12577 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12578 /*               the boundary traversal:  TRUE iff the vertex */
12579 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12580 /* N =         Local copy of NV */
12581 /* NI =        Number of intersections (between the boundary */
12582 /*               curve and the great circle P-Q) encountered */
12583 /* PINR =      TRUE iff P is to the left of the directed */
12584 /*               boundary edge associated with the closest */
12585 /*               intersection point to P that lies between P */
12586 /*               and Q (a left-to-right intersection as */
12587 /*               viewed from Q), or there is no intersection */
12588 /*               between P and Q (on the shorter arc) */
12589 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12590 /*               locate intersections B relative to arc Q->P */
12591 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12592 /*               the boundary edge indexed by LISTV(K0) -> */
12593 /*               LISTV(K0+1) */
12594 /* QINR =      TRUE iff Q is to the left of the directed */
12595 /*               boundary edge associated with the closest */
12596 /*               intersection point to Q that lies between P */
12597 /*               and Q (a right-to-left intersection as */
12598 /*               viewed from Q), or there is no intersection */
12599 /*               between P and Q (on the shorter arc) */
12600 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12601 /*               compute (normalize) Q */
12602 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12603 /*               traversal */
12604 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12605 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12606 /* VNRM =      Euclidean norm of VN */
12607 
12608     /* Parameter adjustments */
12609     --p;
12610     --zv;
12611     --yv;
12612     --xv;
12613     --listv;
12614 
12615     /* Function Body */
12616 
12617 /* Store local parameters, test for error 1, and initialize */
12618 /*   K0. */
12619 
12620     imx = *lv;
12621     n = *nv;
12622     if (n < 3 || n > imx) {
12623         goto L11;
12624     }
12625     k0 = 0;
12626     i1 = listv[1];
12627     if (i1 < 1 || i1 > imx) {
12628         goto L12;
12629     }
12630 
12631 /* Increment K0 and set Q to a point immediately to the left */
12632 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12633 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12634 
12635 L1:
12636     ++k0;
12637     if (k0 > n) {
12638         goto L14;
12639     }
12640     i1 = listv[k0];
12641     if (k0 < n) {
12642         i2 = listv[k0 + 1];
12643     } else {
12644         i2 = listv[1];
12645     }
12646     if (i2 < 1 || i2 > imx) {
12647         goto L12;
12648     }
12649     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12650     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12651     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12652     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12653     if (vnrm == 0.) {
12654         goto L1;
12655     }
12656     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12657     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12658     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12659     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12660     q[0] /= qnrm;
12661     q[1] /= qnrm;
12662     q[2] /= qnrm;
12663 
12664 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12665 
12666     cn[0] = q[1] * p[3] - q[2] * p[2];
12667     cn[1] = q[2] * p[1] - q[0] * p[3];
12668     cn[2] = q[0] * p[2] - q[1] * p[1];
12669     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12670         goto L1;
12671     }
12672     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12673     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12674     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12675     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12676     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12677     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12678 
12679 /* Initialize parameters for the boundary traversal. */
12680 
12681     ni = 0;
12682     even = TRUE_;
12683     bp = -2.;
12684     bq = -2.;
12685     pinr = TRUE_;
12686     qinr = TRUE_;
12687     i2 = listv[n];
12688     if (i2 < 1 || i2 > imx) {
12689         goto L12;
12690     }
12691     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12692 
12693 /* Loop on boundary arcs I1->I2. */
12694 
12695     i__1 = n;
12696     for (k = 1; k <= i__1; ++k) {
12697         i1 = i2;
12698         lft1 = lft2;
12699         i2 = listv[k];
12700         if (i2 < 1 || i2 > imx) {
12701             goto L12;
12702         }
12703         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12704         if (lft1 == lft2) {
12705             goto L2;
12706         }
12707 
12708 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12709 /*     point of intersection B. */
12710 
12711         ++ni;
12712         v1[0] = xv[i1];
12713         v1[1] = yv[i1];
12714         v1[2] = zv[i1];
12715         v2[0] = xv[i2];
12716         v2[1] = yv[i2];
12717         v2[2] = zv[i2];
12718         intrsc_(v1, v2, cn, b, &ierr);
12719 
12720 /*   B is between Q and P (on the shorter arc) iff */
12721 /*     B Forward Q->P and B Forward P->Q       iff */
12722 /*     <B,QN> > 0 and <B,PN> > 0. */
12723 
12724         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12725                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12726 
12727 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12728 
12729             even = ! even;
12730             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12731             if (d__ > bq) {
12732                 bq = d__;
12733                 qinr = lft2;
12734             }
12735             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12736             if (d__ > bp) {
12737                 bp = d__;
12738                 pinr = lft1;
12739             }
12740         }
12741 L2:
12742         ;
12743     }
12744 
12745 /* Test for consistency:  NI must be even and QINR must be */
12746 /*   TRUE. */
12747 
12748     if (ni != ni / 2 << 1 || ! qinr) {
12749         goto L1;
12750     }
12751 
12752 /* Test for error 3:  different values of PINR and EVEN. */
12753 
12754     if (pinr != even) {
12755         goto L13;
12756     }
12757 
12758 /* No error encountered. */
12759 
12760     *ier = 0;
12761     ret_val = even;
12762     return ret_val;
12763 
12764 /* LV or NV is outside its valid range. */
12765 
12766 L11:
12767     *ier = 1;
12768     return ret_val;
12769 
12770 /* A LISTV entry is outside its valid range. */
12771 
12772 L12:
12773     *ier = 2;
12774     return ret_val;
12775 
12776 /* The polygon boundary is self-intersecting. */
12777 
12778 L13:
12779     *ier = 3;
12780     return ret_val;
12781 
12782 /* Consistency tests failed for all values of Q. */
12783 
12784 L14:
12785     *ier = 4;
12786     return ret_val;
12787 } /* inside_ */
12788 
12789 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12790         i3, int *list, int *lptr, int *lend, int *lnew)
12791 {
12792     static int k, n1, n2, n3, lp;
12793     /* Subroutine */ int insert_(int *, int *, int *,
12794             int *, int *);
12795     int lstptr_(int *, int *, int *, int *);
12796 
12797 
12798 /* *********************************************************** */
12799 
12800 /*                                              From STRIPACK */
12801 /*                                            Robert J. Renka */
12802 /*                                  Dept. of Computer Science */
12803 /*                                       Univ. of North Texas */
12804 /*                                           renka@cs.unt.edu */
12805 /*                                                   07/17/96 */
12806 
12807 /*   This subroutine adds an interior node to a triangulation */
12808 /* of a set of points on the unit sphere.  The data structure */
12809 /* is updated with the insertion of node KK into the triangle */
12810 /* whose vertices are I1, I2, and I3.  No optimization of the */
12811 /* triangulation is performed. */
12812 
12813 /*   This routine is identical to the similarly named routine */
12814 /* in TRIPACK. */
12815 
12816 
12817 /* On input: */
12818 
12819 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12820 /*            and KK must not be equal to I1, I2, or I3. */
12821 
12822 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12823 /*                  sequence of vertices of a triangle which */
12824 /*                  contains node KK. */
12825 
12826 /* The above parameters are not altered by this routine. */
12827 
12828 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12829 /*                             triangulation.  Refer to Sub- */
12830 /*                             routine TRMESH.  Triangle */
12831 /*                             (I1,I2,I3) must be included */
12832 /*                             in the triangulation. */
12833 
12834 /* On output: */
12835 
12836 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12837 /*                             the addition of node KK.  KK */
12838 /*                             will be connected to nodes I1, */
12839 /*                             I2, and I3. */
12840 
12841 /* Modules required by INTADD:  INSERT, LSTPTR */
12842 
12843 /* *********************************************************** */
12844 
12845 
12846 /* Local parameters: */
12847 
12848 /* K =        Local copy of KK */
12849 /* LP =       LIST pointer */
12850 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12851 
12852     /* Parameter adjustments */
12853     --lend;
12854     --lptr;
12855     --list;
12856 
12857     /* Function Body */
12858     k = *kk;
12859 
12860 /* Initialization. */
12861 
12862     n1 = *i1;
12863     n2 = *i2;
12864     n3 = *i3;
12865 
12866 /* Add K as a neighbor of I1, I2, and I3. */
12867 
12868     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12869     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12870     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12871     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12872     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12873     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12874 
12875 /* Add I1, I2, and I3 as neighbors of K. */
12876 
12877     list[*lnew] = n1;
12878     list[*lnew + 1] = n2;
12879     list[*lnew + 2] = n3;
12880     lptr[*lnew] = *lnew + 1;
12881     lptr[*lnew + 1] = *lnew + 2;
12882     lptr[*lnew + 2] = *lnew;
12883     lend[k] = *lnew + 2;
12884     *lnew += 3;
12885     return 0;
12886 } /* intadd_ */
12887 
12888 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12889         double *p, int *ier)
12890 {
12891     /* Builtin functions */
12892     //double sqrt(double);
12893 
12894     /* Local variables */
12895     static int i__;
12896     static double t, d1, d2, pp[3], ppn;
12897 
12898 
12899 /* *********************************************************** */
12900 
12901 /*                                              From STRIPACK */
12902 /*                                            Robert J. Renka */
12903 /*                                  Dept. of Computer Science */
12904 /*                                       Univ. of North Texas */
12905 /*                                           renka@cs.unt.edu */
12906 /*                                                   07/19/90 */
12907 
12908 /*   Given a great circle C and points P1 and P2 defining an */
12909 /* arc A on the surface of the unit sphere, where A is the */
12910 /* shorter of the two portions of the great circle C12 assoc- */
12911 /* iated with P1 and P2, this subroutine returns the point */
12912 /* of intersection P between C and C12 that is closer to A. */
12913 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12914 /* C, P is the point of intersection of C with A. */
12915 
12916 
12917 /* On input: */
12918 
12919 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
12920 /*               coordinates of unit vectors. */
12921 
12922 /*       CN = Array of length 3 containing the Cartesian */
12923 /*            coordinates of a nonzero vector which defines C */
12924 /*            as the intersection of the plane whose normal */
12925 /*            is CN with the unit sphere.  Thus, if C is to */
12926 /*            be the great circle defined by P and Q, CN */
12927 /*            should be P X Q. */
12928 
12929 /* The above parameters are not altered by this routine. */
12930 
12931 /*       P = Array of length 3. */
12932 
12933 /* On output: */
12934 
12935 /*       P = Point of intersection defined above unless IER */
12936 /*           .NE. 0, in which case P is not altered. */
12937 
12938 /*       IER = Error indicator. */
12939 /*             IER = 0 if no errors were encountered. */
12940 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
12941 /*                     iff P1 = P2 or CN = 0 or there are */
12942 /*                     two intersection points at the same */
12943 /*                     distance from A. */
12944 /*             IER = 2 if P2 = -P1 and the definition of A is */
12945 /*                     therefore ambiguous. */
12946 
12947 /* Modules required by INTRSC:  None */
12948 
12949 /* Intrinsic function called by INTRSC:  SQRT */
12950 
12951 /* *********************************************************** */
12952 
12953 
12954 /* Local parameters: */
12955 
12956 /* D1 =  <CN,P1> */
12957 /* D2 =  <CN,P2> */
12958 /* I =   DO-loop index */
12959 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
12960 /*         line defined by P1 and P2 */
12961 /* PPN = Norm of PP */
12962 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
12963 /*         in the plane of C */
12964 
12965     /* Parameter adjustments */
12966     --p;
12967     --cn;
12968     --p2;
12969     --p1;
12970 
12971     /* Function Body */
12972     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12973     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12974 
12975     if (d1 == d2) {
12976         *ier = 1;
12977         return 0;
12978     }
12979 
12980 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
12981 
12982     t = d1 / (d1 - d2);
12983     ppn = 0.;
12984     for (i__ = 1; i__ <= 3; ++i__) {
12985         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12986         ppn += pp[i__ - 1] * pp[i__ - 1];
12987 /* L1: */
12988     }
12989 
12990 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
12991 
12992     if (ppn == 0.) {
12993         *ier = 2;
12994         return 0;
12995     }
12996     ppn = sqrt(ppn);
12997 
12998 /* Compute P = PP/PPN. */
12999 
13000     for (i__ = 1; i__ <= 3; ++i__) {
13001         p[i__] = pp[i__ - 1] / ppn;
13002 /* L2: */
13003     }
13004     *ier = 0;
13005     return 0;
13006 } /* intrsc_ */
13007 
13008 int jrand_(int *n, int *ix, int *iy, int *iz)
13009 {
13010     /* System generated locals */
13011     int ret_val;
13012 
13013     /* Local variables */
13014     static float u, x;
13015 
13016 
13017 /* *********************************************************** */
13018 
13019 /*                                              From STRIPACK */
13020 /*                                            Robert J. Renka */
13021 /*                                  Dept. of Computer Science */
13022 /*                                       Univ. of North Texas */
13023 /*                                           renka@cs.unt.edu */
13024 /*                                                   07/28/98 */
13025 
13026 /*   This function returns a uniformly distributed pseudo- */
13027 /* random int in the range 1 to N. */
13028 
13029 
13030 /* On input: */
13031 
13032 /*       N = Maximum value to be returned. */
13033 
13034 /* N is not altered by this function. */
13035 
13036 /*       IX,IY,IZ = int seeds initialized to values in */
13037 /*                  the range 1 to 30,000 before the first */
13038 /*                  call to JRAND, and not altered between */
13039 /*                  subsequent calls (unless a sequence of */
13040 /*                  random numbers is to be repeated by */
13041 /*                  reinitializing the seeds). */
13042 
13043 /* On output: */
13044 
13045 /*       IX,IY,IZ = Updated int seeds. */
13046 
13047 /*       JRAND = Random int in the range 1 to N. */
13048 
13049 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
13050 /*             and Portable Pseudo-random Number Generator", */
13051 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
13052 /*             pp. 188-190. */
13053 
13054 /* Modules required by JRAND:  None */
13055 
13056 /* Intrinsic functions called by JRAND:  INT, MOD, float */
13057 
13058 /* *********************************************************** */
13059 
13060 
13061 /* Local parameters: */
13062 
13063 /* U = Pseudo-random number uniformly distributed in the */
13064 /*     interval (0,1). */
13065 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
13066 /*       tional part is U. */
13067 
13068     *ix = *ix * 171 % 30269;
13069     *iy = *iy * 172 % 30307;
13070     *iz = *iz * 170 % 30323;
13071     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13072             30323.f;
13073     u = x - (int) x;
13074     ret_val = (int) ((float) (*n) * u + 1.f);
13075     return ret_val;
13076 } /* jrand_ */
13077 
13078 long int left_(double *x1, double *y1, double *z1, double *x2,
13079         double *y2, double *z2, double *x0, double *y0,
13080         double *z0)
13081 {
13082     /* System generated locals */
13083     long int ret_val;
13084 
13085 
13086 /* *********************************************************** */
13087 
13088 /*                                              From STRIPACK */
13089 /*                                            Robert J. Renka */
13090 /*                                  Dept. of Computer Science */
13091 /*                                       Univ. of North Texas */
13092 /*                                           renka@cs.unt.edu */
13093 /*                                                   07/15/96 */
13094 
13095 /*   This function determines whether node N0 is in the */
13096 /* (closed) left hemisphere defined by the plane containing */
13097 /* N1, N2, and the origin, where left is defined relative to */
13098 /* an observer at N1 facing N2. */
13099 
13100 
13101 /* On input: */
13102 
13103 /*       X1,Y1,Z1 = Coordinates of N1. */
13104 
13105 /*       X2,Y2,Z2 = Coordinates of N2. */
13106 
13107 /*       X0,Y0,Z0 = Coordinates of N0. */
13108 
13109 /* Input parameters are not altered by this function. */
13110 
13111 /* On output: */
13112 
13113 /*       LEFT = TRUE if and only if N0 is in the closed */
13114 /*              left hemisphere. */
13115 
13116 /* Modules required by LEFT:  None */
13117 
13118 /* *********************************************************** */
13119 
13120 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
13121 
13122     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13123             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13124 
13125 
13126     return ret_val;
13127 } /* left_ */
13128 
13129 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13130 {
13131     /* System generated locals */
13132     int ret_val;
13133 
13134     /* Local variables */
13135     static int nd, lp;
13136 
13137 
13138 /* *********************************************************** */
13139 
13140 /*                                              From STRIPACK */
13141 /*                                            Robert J. Renka */
13142 /*                                  Dept. of Computer Science */
13143 /*                                       Univ. of North Texas */
13144 /*                                           renka@cs.unt.edu */
13145 /*                                                   07/15/96 */
13146 
13147 /*   This function returns the index (LIST pointer) of NB in */
13148 /* the adjacency list for N0, where LPL = LEND(N0). */
13149 
13150 /*   This function is identical to the similarly named */
13151 /* function in TRIPACK. */
13152 
13153 
13154 /* On input: */
13155 
13156 /*       LPL = LEND(N0) */
13157 
13158 /*       NB = Index of the node whose pointer is to be re- */
13159 /*            turned.  NB must be connected to N0. */
13160 
13161 /*       LIST,LPTR = Data structure defining the triangula- */
13162 /*                   tion.  Refer to Subroutine TRMESH. */
13163 
13164 /* Input parameters are not altered by this function. */
13165 
13166 /* On output: */
13167 
13168 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13169 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13170 /*                neighbor of N0, in which case LSTPTR = LPL. */
13171 
13172 /* Modules required by LSTPTR:  None */
13173 
13174 /* *********************************************************** */
13175 
13176 
13177 /* Local parameters: */
13178 
13179 /* LP = LIST pointer */
13180 /* ND = Nodal index */
13181 
13182     /* Parameter adjustments */
13183     --lptr;
13184     --list;
13185 
13186     /* Function Body */
13187     lp = lptr[*lpl];
13188 L1:
13189     nd = list[lp];
13190     if (nd == *nb) {
13191         goto L2;
13192     }
13193     lp = lptr[lp];
13194     if (lp != *lpl) {
13195         goto L1;
13196     }
13197 
13198 L2:
13199     ret_val = lp;
13200     return ret_val;
13201 } /* lstptr_ */
13202 
13203 int nbcnt_(int *lpl, int *lptr)
13204 {
13205     /* System generated locals */
13206     int ret_val;
13207 
13208     /* Local variables */
13209     static int k, lp;
13210 
13211 
13212 /* *********************************************************** */
13213 
13214 /*                                              From STRIPACK */
13215 /*                                            Robert J. Renka */
13216 /*                                  Dept. of Computer Science */
13217 /*                                       Univ. of North Texas */
13218 /*                                           renka@cs.unt.edu */
13219 /*                                                   07/15/96 */
13220 
13221 /*   This function returns the number of neighbors of a node */
13222 /* N0 in a triangulation created by Subroutine TRMESH. */
13223 
13224 /*   This function is identical to the similarly named */
13225 /* function in TRIPACK. */
13226 
13227 
13228 /* On input: */
13229 
13230 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13231 /*             LPL = LEND(N0). */
13232 
13233 /*       LPTR = Array of pointers associated with LIST. */
13234 
13235 /* Input parameters are not altered by this function. */
13236 
13237 /* On output: */
13238 
13239 /*       NBCNT = Number of neighbors of N0. */
13240 
13241 /* Modules required by NBCNT:  None */
13242 
13243 /* *********************************************************** */
13244 
13245 
13246 /* Local parameters: */
13247 
13248 /* K =  Counter for computing the number of neighbors */
13249 /* LP = LIST pointer */
13250 
13251     /* Parameter adjustments */
13252     --lptr;
13253 
13254     /* Function Body */
13255     lp = *lpl;
13256     k = 1;
13257 
13258 L1:
13259     lp = lptr[lp];
13260     if (lp == *lpl) {
13261         goto L2;
13262     }
13263     ++k;
13264     goto L1;
13265 
13266 L2:
13267     ret_val = k;
13268     return ret_val;
13269 } /* nbcnt_ */
13270 
13271 int nearnd_(double *p, int *ist, int *n, double *x,
13272         double *y, double *z__, int *list, int *lptr, int
13273         *lend, double *al)
13274 {
13275     /* System generated locals */
13276     int ret_val, i__1;
13277 
13278     /* Builtin functions */
13279     //double acos(double);
13280 
13281     /* Local variables */
13282     static int l;
13283     static double b1, b2, b3;
13284     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13285     static double ds1;
13286     static int lp1, lp2;
13287     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13288     static int lpl;
13289     static double dsr;
13290     static int nst, listp[25], lptrp[25];
13291     /* Subroutine */ int trfind_(int *, double *, int *,
13292             double *, double *, double *, int *, int *,
13293             int *, double *, double *, double *, int *,
13294             int *, int *);
13295     int lstptr_(int *, int *, int *, int *);
13296 
13297 
13298 /* *********************************************************** */
13299 
13300 /*                                              From STRIPACK */
13301 /*                                            Robert J. Renka */
13302 /*                                  Dept. of Computer Science */
13303 /*                                       Univ. of North Texas */
13304 /*                                           renka@cs.unt.edu */
13305 /*                                                   07/28/98 */
13306 
13307 /*   Given a point P on the surface of the unit sphere and a */
13308 /* Delaunay triangulation created by Subroutine TRMESH, this */
13309 /* function returns the index of the nearest triangulation */
13310 /* node to P. */
13311 
13312 /*   The algorithm consists of implicitly adding P to the */
13313 /* triangulation, finding the nearest neighbor to P, and */
13314 /* implicitly deleting P from the triangulation.  Thus, it */
13315 /* is based on the fact that, if P is a node in a Delaunay */
13316 /* triangulation, the nearest node to P is a neighbor of P. */
13317 
13318 
13319 /* On input: */
13320 
13321 /*       P = Array of length 3 containing the Cartesian coor- */
13322 /*           dinates of the point P to be located relative to */
13323 /*           the triangulation.  It is assumed without a test */
13324 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13325 
13326 /*       IST = Index of a node at which TRFIND begins the */
13327 /*             search.  Search time depends on the proximity */
13328 /*             of this node to P. */
13329 
13330 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13331 
13332 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13333 /*               coordinates of the nodes. */
13334 
13335 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13336 /*                        gulation.  Refer to TRMESH. */
13337 
13338 /* Input parameters are not altered by this function. */
13339 
13340 /* On output: */
13341 
13342 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13343 /*                if N < 3 or the triangulation data struc- */
13344 /*                ture is invalid. */
13345 
13346 /*       AL = Arc length (angular distance in radians) be- */
13347 /*            tween P and NEARND unless NEARND = 0. */
13348 
13349 /*       Note that the number of candidates for NEARND */
13350 /*       (neighbors of P) is limited to LMAX defined in */
13351 /*       the PARAMETER statement below. */
13352 
13353 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13354 
13355 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13356 
13357 /* *********************************************************** */
13358 
13359 
13360 /* Local parameters: */
13361 
13362 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13363 /*               by TRFIND */
13364 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13365 /* DSR =       (Negative cosine of the) distance from P to NR */
13366 /* DX1,..DZ3 = Components of vectors used by the swap test */
13367 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13368 /*               the rightmost (I1) and leftmost (I2) visible */
13369 /*               boundary nodes as viewed from P */
13370 /* L =         Length of LISTP/LPTRP and number of neighbors */
13371 /*               of P */
13372 /* LMAX =      Maximum value of L */
13373 /* LISTP =     Indexes of the neighbors of P */
13374 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13375 /*               LISTP elements */
13376 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13377 /*               pointer */
13378 /* LP1,LP2 =   LISTP indexes (pointers) */
13379 /* LPL =       Pointer to the last neighbor of N1 */
13380 /* N1 =        Index of a node visible from P */
13381 /* N2 =        Index of an endpoint of an arc opposite P */
13382 /* N3 =        Index of the node opposite N1->N2 */
13383 /* NN =        Local copy of N */
13384 /* NR =        Index of a candidate for the nearest node to P */
13385 /* NST =       Index of the node at which TRFIND begins the */
13386 /*               search */
13387 
13388 
13389 /* Store local parameters and test for N invalid. */
13390 
13391     /* Parameter adjustments */
13392     --p;
13393     --lend;
13394     --z__;
13395     --y;
13396     --x;
13397     --list;
13398     --lptr;
13399 
13400     /* Function Body */
13401     nn = *n;
13402     if (nn < 3) {
13403         goto L6;
13404     }
13405     nst = *ist;
13406     if (nst < 1 || nst > nn) {
13407         nst = 1;
13408     }
13409 
13410 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13411 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13412 /*   from P. */
13413 
13414     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13415             1], &b1, &b2, &b3, &i1, &i2, &i3);
13416 
13417 /* Test for collinear nodes. */
13418 
13419     if (i1 == 0) {
13420         goto L6;
13421     }
13422 
13423 /* Store the linked list of 'neighbors' of P in LISTP and */
13424 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13425 /*   the last neighbor if P is not contained in a triangle. */
13426 /*   L is the length of LISTP and LPTRP, and is limited to */
13427 /*   LMAX. */
13428 
13429     if (i3 != 0) {
13430         listp[0] = i1;
13431         lptrp[0] = 2;
13432         listp[1] = i2;
13433         lptrp[1] = 3;
13434         listp[2] = i3;
13435         lptrp[2] = 1;
13436         l = 3;
13437     } else {
13438         n1 = i1;
13439         l = 1;
13440         lp1 = 2;
13441         listp[l - 1] = n1;
13442         lptrp[l - 1] = lp1;
13443 
13444 /*   Loop on the ordered sequence of visible boundary nodes */
13445 /*     N1 from I1 to I2. */
13446 
13447 L1:
13448         lpl = lend[n1];
13449         n1 = -list[lpl];
13450         l = lp1;
13451         lp1 = l + 1;
13452         listp[l - 1] = n1;
13453         lptrp[l - 1] = lp1;
13454         if (n1 != i2 && lp1 < 25) {
13455             goto L1;
13456         }
13457         l = lp1;
13458         listp[l - 1] = 0;
13459         lptrp[l - 1] = 1;
13460     }
13461 
13462 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13463 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13464 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13465 /*   indexes of N1 and N2. */
13466 
13467     lp2 = 1;
13468     n2 = i1;
13469     lp1 = lptrp[0];
13470     n1 = listp[lp1 - 1];
13471 
13472 /* Begin loop:  find the node N3 opposite N1->N2. */
13473 
13474 L2:
13475     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13476     if (list[lp] < 0) {
13477         goto L3;
13478     }
13479     lp = lptr[lp];
13480     n3 = (i__1 = list[lp], abs(i__1));
13481 
13482 /* Swap test:  Exit the loop if L = LMAX. */
13483 
13484     if (l == 25) {
13485         goto L4;
13486     }
13487     dx1 = x[n1] - p[1];
13488     dy1 = y[n1] - p[2];
13489     dz1 = z__[n1] - p[3];
13490 
13491     dx2 = x[n2] - p[1];
13492     dy2 = y[n2] - p[2];
13493     dz2 = z__[n2] - p[3];
13494 
13495     dx3 = x[n3] - p[1];
13496     dy3 = y[n3] - p[2];
13497     dz3 = z__[n3] - p[3];
13498     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13499             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13500         goto L3;
13501     }
13502 
13503 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13504 /*        The two new arcs opposite P must be tested. */
13505 
13506     ++l;
13507     lptrp[lp2 - 1] = l;
13508     listp[l - 1] = n3;
13509     lptrp[l - 1] = lp1;
13510     lp1 = l;
13511     n1 = n3;
13512     goto L2;
13513 
13514 /* No swap:  Advance to the next arc and test for termination */
13515 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13516 
13517 L3:
13518     if (lp1 == 1) {
13519         goto L4;
13520     }
13521     lp2 = lp1;
13522     n2 = n1;
13523     lp1 = lptrp[lp1 - 1];
13524     n1 = listp[lp1 - 1];
13525     if (n1 == 0) {
13526         goto L4;
13527     }
13528     goto L2;
13529 
13530 /* Set NR and DSR to the index of the nearest node to P and */
13531 /*   an increasing function (negative cosine) of its distance */
13532 /*   from P, respectively. */
13533 
13534 L4:
13535     nr = i1;
13536     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13537     i__1 = l;
13538     for (lp = 2; lp <= i__1; ++lp) {
13539         n1 = listp[lp - 1];
13540         if (n1 == 0) {
13541             goto L5;
13542         }
13543         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13544         if (ds1 < dsr) {
13545             nr = n1;
13546             dsr = ds1;
13547         }
13548 L5:
13549         ;
13550     }
13551     dsr = -dsr;
13552     if (dsr > 1.) {
13553         dsr = 1.;
13554     }
13555     *al = acos(dsr);
13556     ret_val = nr;
13557     return ret_val;
13558 
13559 /* Invalid input. */
13560 
13561 L6:
13562     ret_val = 0;
13563     return ret_val;
13564 } /* nearnd_ */
13565 
13566 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13567         int *na, int *list, int *lptr, int *lend, int *
13568         nit, int *iwk, int *ier)
13569 {
13570     /* System generated locals */
13571     int i__1, i__2;
13572 
13573     /* Local variables */
13574     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13575     static long int swp;
13576     static int iter;
13577     /* Subroutine */ int swap_(int *, int *, int *,
13578             int *, int *, int *, int *, int *);
13579     static int maxit;
13580     long int swptst_(int *, int *, int *, int *,
13581             double *, double *, double *);
13582 
13583 
13584 /* *********************************************************** */
13585 
13586 /*                                              From STRIPACK */
13587 /*                                            Robert J. Renka */
13588 /*                                  Dept. of Computer Science */
13589 /*                                       Univ. of North Texas */
13590 /*                                           renka@cs.unt.edu */
13591 /*                                                   07/30/98 */
13592 
13593 /*   Given a set of NA triangulation arcs, this subroutine */
13594 /* optimizes the portion of the triangulation consisting of */
13595 /* the quadrilaterals (pairs of adjacent triangles) which */
13596 /* have the arcs as diagonals by applying the circumcircle */
13597 /* test and appropriate swaps to the arcs. */
13598 
13599 /*   An iteration consists of applying the swap test and */
13600 /* swaps to all NA arcs in the order in which they are */
13601 /* stored.  The iteration is repeated until no swap occurs */
13602 /* or NIT iterations have been performed.  The bound on the */
13603 /* number of iterations may be necessary to prevent an */
13604 /* infinite loop caused by cycling (reversing the effect of a */
13605 /* previous swap) due to floating point inaccuracy when four */
13606 /* or more nodes are nearly cocircular. */
13607 
13608 
13609 /* On input: */
13610 
13611 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13612 
13613 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13614 
13615 /* The above parameters are not altered by this routine. */
13616 
13617 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13618 /*                        gulation.  Refer to Subroutine */
13619 /*                        TRMESH. */
13620 
13621 /*       NIT = Maximum number of iterations to be performed. */
13622 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13623 
13624 /*       IWK = int array dimensioned 2 by NA containing */
13625 /*             the nodal indexes of the arc endpoints (pairs */
13626 /*             of endpoints are stored in columns). */
13627 
13628 /* On output: */
13629 
13630 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13631 /*                        ture reflecting the swaps. */
13632 
13633 /*       NIT = Number of iterations performed. */
13634 
13635 /*       IWK = Endpoint indexes of the new set of arcs */
13636 /*             reflecting the swaps. */
13637 
13638 /*       IER = Error indicator: */
13639 /*             IER = 0 if no errors were encountered. */
13640 /*             IER = 1 if a swap occurred on the last of */
13641 /*                     MAXIT iterations, where MAXIT is the */
13642 /*                     value of NIT on input.  The new set */
13643 /*                     of arcs is not necessarily optimal */
13644 /*                     in this case. */
13645 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13646 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13647 /*                     IWK(1,I) for some I in the range 1 */
13648 /*                     to NA.  A swap may have occurred in */
13649 /*                     this case. */
13650 /*             IER = 4 if a zero pointer was returned by */
13651 /*                     Subroutine SWAP. */
13652 
13653 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13654 
13655 /* Intrinsic function called by OPTIM:  ABS */
13656 
13657 /* *********************************************************** */
13658 
13659 
13660 /* Local parameters: */
13661 
13662 /* I =       Column index for IWK */
13663 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13664 /* ITER =    Iteration count */
13665 /* LP =      LIST pointer */
13666 /* LP21 =    Parameter returned by SWAP (not used) */
13667 /* LPL =     Pointer to the last neighbor of IO1 */
13668 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13669 /*             of IO1 */
13670 /* MAXIT =   Input value of NIT */
13671 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13672 /*             respectively */
13673 /* NNA =     Local copy of NA */
13674 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13675 /*             optimization loop */
13676 
13677     /* Parameter adjustments */
13678     --x;
13679     --y;
13680     --z__;
13681     iwk -= 3;
13682     --list;
13683     --lptr;
13684     --lend;
13685 
13686     /* Function Body */
13687     nna = *na;
13688     maxit = *nit;
13689     if (nna < 0 || maxit < 1) {
13690         goto L7;
13691     }
13692 
13693 /* Initialize iteration count ITER and test for NA = 0. */
13694 
13695     iter = 0;
13696     if (nna == 0) {
13697         goto L5;
13698     }
13699 
13700 /* Top of loop -- */
13701 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13702 
13703 L1:
13704     if (iter == maxit) {
13705         goto L6;
13706     }
13707     ++iter;
13708     swp = FALSE_;
13709 
13710 /*   Inner loop on arcs IO1-IO2 -- */
13711 
13712     i__1 = nna;
13713     for (i__ = 1; i__ <= i__1; ++i__) {
13714         io1 = iwk[(i__ << 1) + 1];
13715         io2 = iwk[(i__ << 1) + 2];
13716 
13717 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13718 /*     IO2->IO1, respectively.  Determine the following: */
13719 
13720 /*     LPL = pointer to the last neighbor of IO1, */
13721 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13722 /*     LPP = pointer to the node N2 preceding IO2. */
13723 
13724         lpl = lend[io1];
13725         lpp = lpl;
13726         lp = lptr[lpp];
13727 L2:
13728         if (list[lp] == io2) {
13729             goto L3;
13730         }
13731         lpp = lp;
13732         lp = lptr[lpp];
13733         if (lp != lpl) {
13734             goto L2;
13735         }
13736 
13737 /*   IO2 should be the last neighbor of IO1.  Test for no */
13738 /*     arc and bypass the swap test if IO1 is a boundary */
13739 /*     node. */
13740 
13741         if ((i__2 = list[lp], abs(i__2)) != io2) {
13742             goto L8;
13743         }
13744         if (list[lp] < 0) {
13745             goto L4;
13746         }
13747 
13748 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13749 /*     boundary node and IO2 is its first neighbor. */
13750 
13751 L3:
13752         n2 = list[lpp];
13753         if (n2 < 0) {
13754             goto L4;
13755         }
13756         lp = lptr[lp];
13757         n1 = (i__2 = list[lp], abs(i__2));
13758 
13759 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13760 
13761         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13762             goto L4;
13763         }
13764         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13765         if (lp21 == 0) {
13766             goto L9;
13767         }
13768         swp = TRUE_;
13769         iwk[(i__ << 1) + 1] = n1;
13770         iwk[(i__ << 1) + 2] = n2;
13771 L4:
13772         ;
13773     }
13774     if (swp) {
13775         goto L1;
13776     }
13777 
13778 /* Successful termination. */
13779 
13780 L5:
13781     *nit = iter;
13782     *ier = 0;
13783     return 0;
13784 
13785 /* MAXIT iterations performed without convergence. */
13786 
13787 L6:
13788     *nit = maxit;
13789     *ier = 1;
13790     return 0;
13791 
13792 /* Invalid input parameter. */
13793 
13794 L7:
13795     *nit = 0;
13796     *ier = 2;
13797     return 0;
13798 
13799 /* IO2 is not a neighbor of IO1. */
13800 
13801 L8:
13802     *nit = iter;
13803     *ier = 3;
13804     return 0;
13805 
13806 /* Zero pointer returned by SWAP. */
13807 
13808 L9:
13809     *nit = iter;
13810     *ier = 4;
13811     return 0;
13812 } /* optim_ */
13813 
13814 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13815         double *ox, double *oy, double *oz, double *ex,
13816         double *ey, double *ez, double *vx, double *vy,
13817         double *vz, long int *init, double *x, double *y,
13818         double *z__, int *ier)
13819 {
13820     /* Builtin functions */
13821     //double sqrt(double);
13822 
13823     /* Local variables */
13824     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13825             oes, xoe, yoe, zoe, xep, yep, zep;
13826 
13827 
13828 /* *********************************************************** */
13829 
13830 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13831 /*                                            Robert J. Renka */
13832 /*                                  Dept. of Computer Science */
13833 /*                                       Univ. of North Texas */
13834 /*                                           renka@cs.unt.edu */
13835 /*                                                   07/18/90 */
13836 
13837 /*   Given a projection plane and associated coordinate sys- */
13838 /* tem defined by an origin O, eye position E, and up-vector */
13839 /* V, this subroutine applies a perspective depth transform- */
13840 /* ation T to a point P = (PX,PY,PZ), returning the point */
13841 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13842 /* coordinates of the point that lies in the projection */
13843 /* plane and on the line defined by P and E, and Z is the */
13844 /* depth associated with P. */
13845 
13846 /*   The projection plane is defined to be the plane that */
13847 /* contains O and has normal defined by O and E. */
13848 
13849 /*   The depth Z is defined in such a way that Z < 1, T maps */
13850 /* lines to lines (and planes to planes), and if two distinct */
13851 /* points have the same projection plane coordinates, then */
13852 /* the one closer to E has a smaller depth.  (Z increases */
13853 /* monotonically with orthogonal distance from P to the plane */
13854 /* that is parallel to the projection plane and contains E.) */
13855 /* This depth value facilitates depth sorting and depth buf- */
13856 /* fer methods. */
13857 
13858 
13859 /* On input: */
13860 
13861 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13862 /*                  be mapped onto the projection plane.  The */
13863 /*                  half line that contains P and has end- */
13864 /*                  point at E must intersect the plane. */
13865 
13866 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13867 /*                  nate system in the projection plane).  A */
13868 /*                  reasonable value for O is a point near */
13869 /*                  the center of an object or scene to be */
13870 /*                  viewed. */
13871 
13872 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13873 /*                  ing the normal to the plane and the line */
13874 /*                  of sight for the projection.  E must not */
13875 /*                  coincide with O or P, and the angle be- */
13876 /*                  tween the vectors O-E and P-E must be */
13877 /*                  less than 90 degrees.  Note that E and P */
13878 /*                  may lie on opposite sides of the projec- */
13879 /*                  tion plane. */
13880 
13881 /*       VX,VY,VZ = Coordinates of a point V which defines */
13882 /*                  the positive Y axis of an X-Y coordinate */
13883 /*                  system in the projection plane as the */
13884 /*                  half-line containing O and the projection */
13885 /*                  of O+V onto the plane.  The positive X */
13886 /*                  axis has direction defined by the cross */
13887 /*                  product V X (E-O). */
13888 
13889 /* The above parameters are not altered by this routine. */
13890 
13891 /*       INIT = long int switch which must be set to TRUE on */
13892 /*              the first call and when the values of O, E, */
13893 /*              or V have been altered since a previous call. */
13894 /*              If INIT = FALSE, it is assumed that only the */
13895 /*              coordinates of P have changed since a previ- */
13896 /*              ous call.  Previously stored quantities are */
13897 /*              used for increased efficiency in this case. */
13898 
13899 /* On output: */
13900 
13901 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13902 
13903 /*       X,Y = Projection plane coordinates of the point */
13904 /*             that lies in the projection plane and on the */
13905 /*             line defined by E and P.  X and Y are not */
13906 /*             altered if IER .NE. 0. */
13907 
13908 /*       Z = Depth value defined above unless IER .NE. 0. */
13909 
13910 /*       IER = Error indicator. */
13911 /*             IER = 0 if no errors were encountered. */
13912 /*             IER = 1 if the inner product of O-E with P-E */
13913 /*                     is not positive, implying that E is */
13914 /*                     too close to the plane. */
13915 /*             IER = 2 if O, E, and O+V are collinear.  See */
13916 /*                     the description of VX,VY,VZ. */
13917 
13918 /* Modules required by PROJCT:  None */
13919 
13920 /* Intrinsic function called by PROJCT:  SQRT */
13921 
13922 /* *********************************************************** */
13923 
13924 
13925 /* Local parameters: */
13926 
13927 /* OES =         Norm squared of OE -- inner product (OE,OE) */
13928 /* S =           Scale factor for computing projections */
13929 /* SC =          Scale factor for normalizing VN and HN */
13930 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
13931 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
13932 /* XH,YH,ZH =    Components of a unit vector HN defining the */
13933 /*                 positive X-axis in the plane */
13934 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
13935 /* XV,YV,ZV =    Components of a unit vector VN defining the */
13936 /*                 positive Y-axis in the plane */
13937 /* XW,YW,ZW =    Components of the vector W from O to the */
13938 /*                 projection of P onto the plane */
13939 
13940     if (*init) {
13941 
13942 /* Compute parameters defining the transformation: */
13943 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
13944 /*   2 square roots. */
13945 
13946 /* Set the coordinates of E to local variables, compute */
13947 /*   OE = E-O and OES, and test for OE = 0. */
13948 
13949         xe = *ex;
13950         ye = *ey;
13951         ze = *ez;
13952         xoe = xe - *ox;
13953         yoe = ye - *oy;
13954         zoe = ze - *oz;
13955         oes = xoe * xoe + yoe * yoe + zoe * zoe;
13956         if (oes == 0.) {
13957             goto L1;
13958         }
13959 
13960 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
13961 
13962         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13963         xv = *vx - s * xoe;
13964         yv = *vy - s * yoe;
13965         zv = *vz - s * zoe;
13966 
13967 /* Normalize VN to a unit vector. */
13968 
13969         sc = xv * xv + yv * yv + zv * zv;
13970         if (sc == 0.) {
13971             goto L2;
13972         }
13973         sc = 1. / sqrt(sc);
13974         xv = sc * xv;
13975         yv = sc * yv;
13976         zv = sc * zv;
13977 
13978 /* Compute HN = VN X OE (normalized). */
13979 
13980         xh = yv * zoe - yoe * zv;
13981         yh = xoe * zv - xv * zoe;
13982         zh = xv * yoe - xoe * yv;
13983         sc = sqrt(xh * xh + yh * yh + zh * zh);
13984         if (sc == 0.) {
13985             goto L2;
13986         }
13987         sc = 1. / sc;
13988         xh = sc * xh;
13989         yh = sc * yh;
13990         zh = sc * zh;
13991     }
13992 
13993 /* Apply the transformation:  13 adds, 12 multiplies, */
13994 /*                            1 divide, and 1 compare. */
13995 
13996 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
13997 
13998     xep = *px - xe;
13999     yep = *py - ye;
14000     zep = *pz - ze;
14001     s = xoe * xep + yoe * yep + zoe * zep;
14002     if (s >= 0.) {
14003         goto L1;
14004     }
14005     s = oes / s;
14006     xw = xoe - s * xep;
14007     yw = yoe - s * yep;
14008     zw = zoe - s * zep;
14009 
14010 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
14011 /*   reset INIT. */
14012 
14013     *x = xw * xh + yw * yh + zw * zh;
14014     *y = xw * xv + yw * yv + zw * zv;
14015     *z__ = s + 1.;
14016     *init = FALSE_;
14017     *ier = 0;
14018     return 0;
14019 
14020 /* (OE,EP) .GE. 0. */
14021 
14022 L1:
14023     *ier = 1;
14024     return 0;
14025 
14026 /* O, E, and O+V are collinear. */
14027 
14028 L2:
14029     *ier = 2;
14030     return 0;
14031 } /* projct_ */
14032 
14033 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
14034         double *plat, double *plon, double *pnrm)
14035 {
14036     /* Builtin functions */
14037     //double sqrt(double), atan2(double, double), asin(double);
14038 
14039 
14040 /* *********************************************************** */
14041 
14042 /*                                              From STRIPACK */
14043 /*                                            Robert J. Renka */
14044 /*                                  Dept. of Computer Science */
14045 /*                                       Univ. of North Texas */
14046 /*                                           renka@cs.unt.edu */
14047 /*                                                   08/27/90 */
14048 
14049 /*   This subroutine converts a point P from Cartesian coor- */
14050 /* dinates to spherical coordinates. */
14051 
14052 
14053 /* On input: */
14054 
14055 /*       PX,PY,PZ = Cartesian coordinates of P. */
14056 
14057 /* Input parameters are not altered by this routine. */
14058 
14059 /* On output: */
14060 
14061 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
14062 /*              0 if PNRM = 0.  PLAT should be scaled by */
14063 /*              180/PI to obtain the value in degrees. */
14064 
14065 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
14066 /*              if P lies on the Z-axis.  PLON should be */
14067 /*              scaled by 180/PI to obtain the value in */
14068 /*              degrees. */
14069 
14070 /*       PNRM = Magnitude (Euclidean norm) of P. */
14071 
14072 /* Modules required by SCOORD:  None */
14073 
14074 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
14075 
14076 /* *********************************************************** */
14077 
14078     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14079     if (*px != 0. || *py != 0.) {
14080         *plon = atan2(*py, *px);
14081     } else {
14082         *plon = 0.;
14083     }
14084     if (*pnrm != 0.) {
14085         *plat = asin(*pz / *pnrm);
14086     } else {
14087         *plat = 0.;
14088     }
14089     return 0;
14090 } /* scoord_ */
14091 
14092 double store_(double *x)
14093 {
14094     /* System generated locals */
14095     double ret_val;
14096 
14097 
14098 /* *********************************************************** */
14099 
14100 /*                                              From STRIPACK */
14101 /*                                            Robert J. Renka */
14102 /*                                  Dept. of Computer Science */
14103 /*                                       Univ. of North Texas */
14104 /*                                           renka@cs.unt.edu */
14105 /*                                                   05/09/92 */
14106 
14107 /*   This function forces its argument X to be stored in a */
14108 /* memory location, thus providing a means of determining */
14109 /* floating point number characteristics (such as the machine */
14110 /* precision) when it is necessary to avoid computation in */
14111 /* high precision registers. */
14112 
14113 
14114 /* On input: */
14115 
14116 /*       X = Value to be stored. */
14117 
14118 /* X is not altered by this function. */
14119 
14120 /* On output: */
14121 
14122 /*       STORE = Value of X after it has been stored and */
14123 /*               possibly truncated or rounded to the single */
14124 /*               precision word length. */
14125 
14126 /* Modules required by STORE:  None */
14127 
14128 /* *********************************************************** */
14129 
14130     stcom_1.y = *x;
14131     ret_val = stcom_1.y;
14132     return ret_val;
14133 } /* store_ */
14134 
14135 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
14136         io2, int *list, int *lptr, int *lend, int *lp21)
14137 {
14138     /* System generated locals */
14139     int i__1;
14140 
14141     /* Local variables */
14142     static int lp, lph, lpsav;
14143     int lstptr_(int *, int *, int *, int *);
14144 
14145 
14146 /* *********************************************************** */
14147 
14148 /*                                              From STRIPACK */
14149 /*                                            Robert J. Renka */
14150 /*                                  Dept. of Computer Science */
14151 /*                                       Univ. of North Texas */
14152 /*                                           renka@cs.unt.edu */
14153 /*                                                   06/22/98 */
14154 
14155 /*   Given a triangulation of a set of points on the unit */
14156 /* sphere, this subroutine replaces a diagonal arc in a */
14157 /* strictly convex quadrilateral (defined by a pair of adja- */
14158 /* cent triangles) with the other diagonal.  Equivalently, a */
14159 /* pair of adjacent triangles is replaced by another pair */
14160 /* having the same union. */
14161 
14162 
14163 /* On input: */
14164 
14165 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14166 /*                         the quadrilateral.  IO1-IO2 is re- */
14167 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14168 /*                         and (IO2,IO1,IN2) must be trian- */
14169 /*                         gles on input. */
14170 
14171 /* The above parameters are not altered by this routine. */
14172 
14173 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14174 /*                        gulation.  Refer to Subroutine */
14175 /*                        TRMESH. */
14176 
14177 /* On output: */
14178 
14179 /*       LIST,LPTR,LEND = Data structure updated with the */
14180 /*                        swap -- triangles (IO1,IO2,IN1) and */
14181 /*                        (IO2,IO1,IN2) are replaced by */
14182 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14183 /*                        unless LP21 = 0. */
14184 
14185 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14186 /*              swap is performed unless IN1 and IN2 are */
14187 /*              adjacent on input, in which case LP21 = 0. */
14188 
14189 /* Module required by SWAP:  LSTPTR */
14190 
14191 /* Intrinsic function called by SWAP:  ABS */
14192 
14193 /* *********************************************************** */
14194 
14195 
14196 /* Local parameters: */
14197 
14198 /* LP,LPH,LPSAV = LIST pointers */
14199 
14200 
14201 /* Test for IN1 and IN2 adjacent. */
14202 
14203     /* Parameter adjustments */
14204     --lend;
14205     --lptr;
14206     --list;
14207 
14208     /* Function Body */
14209     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14210     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14211         *lp21 = 0;
14212         return 0;
14213     }
14214 
14215 /* Delete IO2 as a neighbor of IO1. */
14216 
14217     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14218     lph = lptr[lp];
14219     lptr[lp] = lptr[lph];
14220 
14221 /* If IO2 is the last neighbor of IO1, make IN2 the */
14222 /*   last neighbor. */
14223 
14224     if (lend[*io1] == lph) {
14225         lend[*io1] = lp;
14226     }
14227 
14228 /* Insert IN2 as a neighbor of IN1 following IO1 */
14229 /*   using the hole created above. */
14230 
14231     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14232     lpsav = lptr[lp];
14233     lptr[lp] = lph;
14234     list[lph] = *in2;
14235     lptr[lph] = lpsav;
14236 
14237 /* Delete IO1 as a neighbor of IO2. */
14238 
14239     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14240     lph = lptr[lp];
14241     lptr[lp] = lptr[lph];
14242 
14243 /* If IO1 is the last neighbor of IO2, make IN1 the */
14244 /*   last neighbor. */
14245 
14246     if (lend[*io2] == lph) {
14247         lend[*io2] = lp;
14248     }
14249 
14250 /* Insert IN1 as a neighbor of IN2 following IO2. */
14251 
14252     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14253     lpsav = lptr[lp];
14254     lptr[lp] = lph;
14255     list[lph] = *in1;
14256     lptr[lph] = lpsav;
14257     *lp21 = lph;
14258     return 0;
14259 } /* swap_ */
14260 
14261 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14262         double *x, double *y, double *z__)
14263 {
14264     /* System generated locals */
14265     long int ret_val;
14266 
14267     /* Local variables */
14268     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14269 
14270 
14271 /* *********************************************************** */
14272 
14273 /*                                              From STRIPACK */
14274 /*                                            Robert J. Renka */
14275 /*                                  Dept. of Computer Science */
14276 /*                                       Univ. of North Texas */
14277 /*                                           renka@cs.unt.edu */
14278 /*                                                   03/29/91 */
14279 
14280 /*   This function decides whether or not to replace a */
14281 /* diagonal arc in a quadrilateral with the other diagonal. */
14282 /* The decision will be to swap (SWPTST = TRUE) if and only */
14283 /* if N4 lies above the plane (in the half-space not contain- */
14284 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14285 /* the projection of N4 onto this plane is interior to the */
14286 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14287 /* swap if the quadrilateral is not strictly convex. */
14288 
14289 
14290 /* On input: */
14291 
14292 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14293 /*                     quadrilateral with N1 adjacent to N2, */
14294 /*                     and (N1,N2,N3) in counterclockwise */
14295 /*                     order.  The arc connecting N1 to N2 */
14296 /*                     should be replaced by an arc connec- */
14297 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14298 /*                     to Subroutine SWAP. */
14299 
14300 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14301 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14302 /*               define node I for I = N1, N2, N3, and N4. */
14303 
14304 /* Input parameters are not altered by this routine. */
14305 
14306 /* On output: */
14307 
14308 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14309 /*                and N2 should be swapped for an arc con- */
14310 /*                necting N3 and N4. */
14311 
14312 /* Modules required by SWPTST:  None */
14313 
14314 /* *********************************************************** */
14315 
14316 
14317 /* Local parameters: */
14318 
14319 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14320 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14321 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14322 /* X4,Y4,Z4 =    Coordinates of N4 */
14323 
14324     /* Parameter adjustments */
14325     --z__;
14326     --y;
14327     --x;
14328 
14329     /* Function Body */
14330     x4 = x[*n4];
14331     y4 = y[*n4];
14332     z4 = z__[*n4];
14333     dx1 = x[*n1] - x4;
14334     dx2 = x[*n2] - x4;
14335     dx3 = x[*n3] - x4;
14336     dy1 = y[*n1] - y4;
14337     dy2 = y[*n2] - y4;
14338     dy3 = y[*n3] - y4;
14339     dz1 = z__[*n1] - z4;
14340     dz2 = z__[*n2] - z4;
14341     dz3 = z__[*n3] - z4;
14342 
14343 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14344 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14345 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14346 
14347     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14348             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14349     return ret_val;
14350 } /* swptst_ */
14351 
14352 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14353         double *x, double *y, double *z__)
14354 {
14355     /* System generated locals */
14356     int i__1;
14357 
14358     /* Builtin functions */
14359     //double cos(double), sin(double);
14360 
14361     /* Local variables */
14362     static int i__, nn;
14363     static double phi, theta, cosphi;
14364 
14365 
14366 /* *********************************************************** */
14367 
14368 /*                                              From STRIPACK */
14369 /*                                            Robert J. Renka */
14370 /*                                  Dept. of Computer Science */
14371 /*                                       Univ. of North Texas */
14372 /*                                           renka@cs.unt.edu */
14373 /*                                                   04/08/90 */
14374 
14375 /*   This subroutine transforms spherical coordinates into */
14376 /* Cartesian coordinates on the unit sphere for input to */
14377 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14378 /* storage for RLAT and RLON if the latter need not be saved. */
14379 
14380 
14381 /* On input: */
14382 
14383 /*       N = Number of nodes (points on the unit sphere) */
14384 /*           whose coordinates are to be transformed. */
14385 
14386 /*       RLAT = Array of length N containing latitudinal */
14387 /*              coordinates of the nodes in radians. */
14388 
14389 /*       RLON = Array of length N containing longitudinal */
14390 /*              coordinates of the nodes in radians. */
14391 
14392 /* The above parameters are not altered by this routine. */
14393 
14394 /*       X,Y,Z = Arrays of length at least N. */
14395 
14396 /* On output: */
14397 
14398 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14399 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14400 /*               to N. */
14401 
14402 /* Modules required by TRANS:  None */
14403 
14404 /* Intrinsic functions called by TRANS:  COS, SIN */
14405 
14406 /* *********************************************************** */
14407 
14408 
14409 /* Local parameters: */
14410 
14411 /* COSPHI = cos(PHI) */
14412 /* I =      DO-loop index */
14413 /* NN =     Local copy of N */
14414 /* PHI =    Latitude */
14415 /* THETA =  Longitude */
14416 
14417     /* Parameter adjustments */
14418     --z__;
14419     --y;
14420     --x;
14421     --rlon;
14422     --rlat;
14423 
14424     /* Function Body */
14425     nn = *n;
14426     i__1 = nn;
14427     for (i__ = 1; i__ <= i__1; ++i__) {
14428         phi = rlat[i__];
14429         theta = rlon[i__];
14430         cosphi = cos(phi);
14431         x[i__] = cosphi * cos(theta);
14432         y[i__] = cosphi * sin(theta);
14433         z__[i__] = sin(phi);
14434 /* L1: */
14435     }
14436     return 0;
14437 } /* trans_ */
14438 
14439 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14440         double *x, double *y, double *z__, int *list, int
14441         *lptr, int *lend, double *b1, double *b2, double *b3,
14442         int *i1, int *i2, int *i3)
14443 {
14444     /* Initialized data */
14445 
14446     static int ix = 1;
14447     static int iy = 2;
14448     static int iz = 3;
14449 
14450     /* System generated locals */
14451     int i__1;
14452     double d__1, d__2;
14453 
14454     /* Local variables */
14455     static double q[3];
14456     static int n0, n1, n2, n3, n4, nf;
14457     static double s12;
14458     static int nl, lp;
14459     static double xp, yp, zp;
14460     static int n1s, n2s;
14461     static double eps, tol, ptn1, ptn2;
14462     static int next;
14463     int jrand_(int *, int *, int *, int *);
14464     double store_(double *);
14465     int lstptr_(int *, int *, int *, int *);
14466 
14467 
14468 /* *********************************************************** */
14469 
14470 /*                                              From STRIPACK */
14471 /*                                            Robert J. Renka */
14472 /*                                  Dept. of Computer Science */
14473 /*                                       Univ. of North Texas */
14474 /*                                           renka@cs.unt.edu */
14475 /*                                                   11/30/99 */
14476 
14477 /*   This subroutine locates a point P relative to a triangu- */
14478 /* lation created by Subroutine TRMESH.  If P is contained in */
14479 /* a triangle, the three vertex indexes and barycentric coor- */
14480 /* dinates are returned.  Otherwise, the indexes of the */
14481 /* visible boundary nodes are returned. */
14482 
14483 
14484 /* On input: */
14485 
14486 /*       NST = Index of a node at which TRFIND begins its */
14487 /*             search.  Search time depends on the proximity */
14488 /*             of this node to P. */
14489 
14490 /*       P = Array of length 3 containing the x, y, and z */
14491 /*           coordinates (in that order) of the point P to be */
14492 /*           located. */
14493 
14494 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14495 
14496 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14497 /*               coordinates of the triangulation nodes (unit */
14498 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14499 /*               for I = 1 to N. */
14500 
14501 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14502 /*                        gulation.  Refer to Subroutine */
14503 /*                        TRMESH. */
14504 
14505 /* Input parameters are not altered by this routine. */
14506 
14507 /* On output: */
14508 
14509 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14510 /*                  the central projection of P onto the un- */
14511 /*                  derlying planar triangle if P is in the */
14512 /*                  convex hull of the nodes.  These parame- */
14513 /*                  ters are not altered if I1 = 0. */
14514 
14515 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14516 /*                  of a triangle containing P if P is con- */
14517 /*                  tained in a triangle.  If P is not in the */
14518 /*                  convex hull of the nodes, I1 and I2 are */
14519 /*                  the rightmost and leftmost (boundary) */
14520 /*                  nodes that are visible from P, and */
14521 /*                  I3 = 0.  (If all boundary nodes are vis- */
14522 /*                  ible from P, then I1 and I2 coincide.) */
14523 /*                  I1 = I2 = I3 = 0 if P and all of the */
14524 /*                  nodes are coplanar (lie on a common great */
14525 /*                  circle. */
14526 
14527 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14528 
14529 /* Intrinsic function called by TRFIND:  ABS */
14530 
14531 /* *********************************************************** */
14532 
14533 
14534     /* Parameter adjustments */
14535     --p;
14536     --lend;
14537     --z__;
14538     --y;
14539     --x;
14540     --list;
14541     --lptr;
14542 
14543     /* Function Body */
14544 
14545 /* Local parameters: */
14546 
14547 /* EPS =      Machine precision */
14548 /* IX,IY,IZ = int seeds for JRAND */
14549 /* LP =       LIST pointer */
14550 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14551 /*              cone (with vertex N0) containing P, or end- */
14552 /*              points of a boundary edge such that P Right */
14553 /*              N1->N2 */
14554 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14555 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14556 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14557 /* NF,NL =    First and last neighbors of N0, or first */
14558 /*              (rightmost) and last (leftmost) nodes */
14559 /*              visible from P when P is exterior to the */
14560 /*              triangulation */
14561 /* PTN1 =     Scalar product <P,N1> */
14562 /* PTN2 =     Scalar product <P,N2> */
14563 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14564 /*              the boundary traversal when P is exterior */
14565 /* S12 =      Scalar product <N1,N2> */
14566 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14567 /*              bound on the magnitude of a negative bary- */
14568 /*              centric coordinate (B1 or B2) for P in a */
14569 /*              triangle -- used to avoid an infinite number */
14570 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14571 /*              B2 < 0 but small in magnitude */
14572 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14573 /* X0,Y0,Z0 = Dummy arguments for DET */
14574 /* X1,Y1,Z1 = Dummy arguments for DET */
14575 /* X2,Y2,Z2 = Dummy arguments for DET */
14576 
14577 /* Statement function: */
14578 
14579 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14580 /*                       (closed) left hemisphere defined by */
14581 /*                       the plane containing (0,0,0), */
14582 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14583 /*                       left is defined relative to an ob- */
14584 /*                       server at (X1,Y1,Z1) facing */
14585 /*                       (X2,Y2,Z2). */
14586 
14587 
14588 /* Initialize variables. */
14589 
14590     xp = p[1];
14591     yp = p[2];
14592     zp = p[3];
14593     n0 = *nst;
14594     if (n0 < 1 || n0 > *n) {
14595         n0 = jrand_(n, &ix, &iy, &iz);
14596     }
14597 
14598 /* Compute the relative machine precision EPS and TOL. */
14599 
14600     eps = 1.;
14601 L1:
14602     eps /= 2.;
14603     d__1 = eps + 1.;
14604     if (store_(&d__1) > 1.) {
14605         goto L1;
14606     }
14607     eps *= 2.;
14608     tol = eps * 4.;
14609 
14610 /* Set NF and NL to the first and last neighbors of N0, and */
14611 /*   initialize N1 = NF. */
14612 
14613 L2:
14614     lp = lend[n0];
14615     nl = list[lp];
14616     lp = lptr[lp];
14617     nf = list[lp];
14618     n1 = nf;
14619 
14620 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14621 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14622 
14623     if (nl > 0) {
14624 
14625 /*   N0 is an interior node.  Find N1. */
14626 
14627 L3:
14628         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14629                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14630                 -1e-10) {
14631             lp = lptr[lp];
14632             n1 = list[lp];
14633             if (n1 == nl) {
14634                 goto L6;
14635             }
14636             goto L3;
14637         }
14638     } else {
14639 
14640 /*   N0 is a boundary node.  Test for P exterior. */
14641 
14642         nl = -nl;
14643         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14644                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14645                 -1e-10) {
14646 
14647 /*   P is to the right of the boundary edge N0->NF. */
14648 
14649             n1 = n0;
14650             n2 = nf;
14651             goto L9;
14652         }
14653         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14654                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14655                 -1e-10) {
14656 
14657 /*   P is to the right of the boundary edge NL->N0. */
14658 
14659             n1 = nl;
14660             n2 = n0;
14661             goto L9;
14662         }
14663     }
14664 
14665 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14666 /*   next neighbor of N0 (following N1). */
14667 
14668 L4:
14669     lp = lptr[lp];
14670     n2 = (i__1 = list[lp], abs(i__1));
14671     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14672             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14673         goto L7;
14674     }
14675     n1 = n2;
14676     if (n1 != nl) {
14677         goto L4;
14678     }
14679     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14680             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14681         goto L6;
14682     }
14683 
14684 /* P is left of or on arcs N0->NB for all neighbors NB */
14685 /*   of N0.  Test for P = +/-N0. */
14686 
14687     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14688     if (store_(&d__2) < 1. - eps * 4.) {
14689 
14690 /*   All points are collinear iff P Left NB->N0 for all */
14691 /*     neighbors NB of N0.  Search the neighbors of N0. */
14692 /*     Note:  N1 = NL and LP points to NL. */
14693 
14694 L5:
14695         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14696                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14697                 -1e-10) {
14698             lp = lptr[lp];
14699             n1 = (i__1 = list[lp], abs(i__1));
14700             if (n1 == nl) {
14701                 goto L14;
14702             }
14703             goto L5;
14704         }
14705     }
14706 
14707 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14708 /*   and start over. */
14709 
14710     n0 = n1;
14711     goto L2;
14712 
14713 /* P is between arcs N0->N1 and N0->NF. */
14714 
14715 L6:
14716     n2 = nf;
14717 
14718 /* P is contained in a wedge defined by geodesics N0-N1 and */
14719 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14720 /*   test for cycling. */
14721 
14722 L7:
14723     n3 = n0;
14724     n1s = n1;
14725     n2s = n2;
14726 
14727 /* Top of edge-hopping loop: */
14728 
14729 L8:
14730 
14731     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14732             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14733      if (*b3 < -1e-10) {
14734 
14735 /*   Set N4 to the first neighbor of N2 following N1 (the */
14736 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14737 
14738         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14739         if (list[lp] < 0) {
14740             goto L9;
14741         }
14742         lp = lptr[lp];
14743         n4 = (i__1 = list[lp], abs(i__1));
14744 
14745 /*   Define a new arc N1->N2 which intersects the geodesic */
14746 /*     N0-P. */
14747         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14748                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14749                 -1e-10) {
14750             n3 = n2;
14751             n2 = n4;
14752             n1s = n1;
14753             if (n2 != n2s && n2 != n0) {
14754                 goto L8;
14755             }
14756         } else {
14757             n3 = n1;
14758             n1 = n4;
14759             n2s = n2;
14760             if (n1 != n1s && n1 != n0) {
14761                 goto L8;
14762             }
14763         }
14764 
14765 /*   The starting node N0 or edge N1-N2 was encountered */
14766 /*     again, implying a cycle (infinite loop).  Restart */
14767 /*     with N0 randomly selected. */
14768 
14769         n0 = jrand_(n, &ix, &iy, &iz);
14770         goto L2;
14771     }
14772 
14773 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14774 /*   or P is close to -N0. */
14775 
14776     if (*b3 >= eps) {
14777 
14778 /*   B3 .NE. 0. */
14779 
14780         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14781                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14782         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14783                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14784         if (*b1 < -tol || *b2 < -tol) {
14785 
14786 /*   Restart with N0 randomly selected. */
14787 
14788             n0 = jrand_(n, &ix, &iy, &iz);
14789             goto L2;
14790         }
14791     } else {
14792 
14793 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14794 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14795 
14796         *b3 = 0.;
14797         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14798         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14799         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14800         *b1 = ptn1 - s12 * ptn2;
14801         *b2 = ptn2 - s12 * ptn1;
14802         if (*b1 < -tol || *b2 < -tol) {
14803 
14804 /*   Restart with N0 randomly selected. */
14805 
14806             n0 = jrand_(n, &ix, &iy, &iz);
14807             goto L2;
14808         }
14809     }
14810 
14811 /* P is in (N1,N2,N3). */
14812 
14813     *i1 = n1;
14814     *i2 = n2;
14815     *i3 = n3;
14816     if (*b1 < 0.f) {
14817         *b1 = 0.f;
14818     }
14819     if (*b2 < 0.f) {
14820         *b2 = 0.f;
14821     }
14822     return 0;
14823 
14824 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14825 /*   Save N1 and N2, and set NL = 0 to indicate that */
14826 /*   NL has not yet been found. */
14827 
14828 L9:
14829     n1s = n1;
14830     n2s = n2;
14831     nl = 0;
14832 
14833 /*           Counterclockwise Boundary Traversal: */
14834 
14835 L10:
14836 
14837     lp = lend[n2];
14838     lp = lptr[lp];
14839     next = list[lp];
14840      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14841              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14842             >= -1e-10) {
14843 
14844 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14845 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14846 
14847         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14848         q[0] = x[n1] - s12 * x[n2];
14849         q[1] = y[n1] - s12 * y[n2];
14850         q[2] = z__[n1] - s12 * z__[n2];
14851         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14852             goto L11;
14853         }
14854         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14855             goto L11;
14856         }
14857 
14858 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14859 /*     the leftmost visible node. */
14860 
14861         nl = n2;
14862     }
14863 
14864 /* Bottom of counterclockwise loop: */
14865 
14866     n1 = n2;
14867     n2 = next;
14868     if (n2 != n1s) {
14869         goto L10;
14870     }
14871 
14872 /* All boundary nodes are visible from P. */
14873 
14874     *i1 = n1s;
14875     *i2 = n1s;
14876     *i3 = 0;
14877     return 0;
14878 
14879 /* N2 is the rightmost visible node. */
14880 
14881 L11:
14882     nf = n2;
14883     if (nl == 0) {
14884 
14885 /* Restore initial values of N1 and N2, and begin the search */
14886 /*   for the leftmost visible node. */
14887 
14888         n2 = n2s;
14889         n1 = n1s;
14890 
14891 /*           Clockwise Boundary Traversal: */
14892 
14893 L12:
14894         lp = lend[n1];
14895         next = -list[lp];
14896         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14897                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14898                  y[next]) >= -1e-10) {
14899 
14900 /*   N1 is the leftmost visible node if P or NEXT is */
14901 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14902 
14903             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14904             q[0] = x[n2] - s12 * x[n1];
14905             q[1] = y[n2] - s12 * y[n1];
14906             q[2] = z__[n2] - s12 * z__[n1];
14907             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14908                 goto L13;
14909             }
14910             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14911                 goto L13;
14912             }
14913 
14914 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14915 /*     rightmost visible node. */
14916 
14917             nf = n1;
14918         }
14919 
14920 /* Bottom of clockwise loop: */
14921 
14922         n2 = n1;
14923         n1 = next;
14924         if (n1 != n1s) {
14925             goto L12;
14926         }
14927 
14928 /* All boundary nodes are visible from P. */
14929 
14930         *i1 = n1;
14931         *i2 = n1;
14932         *i3 = 0;
14933         return 0;
14934 
14935 /* N1 is the leftmost visible node. */
14936 
14937 L13:
14938         nl = n1;
14939     }
14940 
14941 /* NF and NL have been found. */
14942 
14943     *i1 = nf;
14944     *i2 = nl;
14945     *i3 = 0;
14946     return 0;
14947 
14948 /* All points are collinear (coplanar). */
14949 
14950 L14:
14951     *i1 = 0;
14952     *i2 = 0;
14953     *i3 = 0;
14954     return 0;
14955 } /* trfind_ */
14956 
14957 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
14958         int *lend, int *nrow, int *nt, int *ltri, int *
14959         ier)
14960 {
14961     /* System generated locals */
14962     int ltri_dim1, ltri_offset, i__1, i__2;
14963 
14964     /* Local variables */
14965     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14966             lpl, isv;
14967     static long int arcs;
14968     static int lpln1;
14969 
14970 
14971 /* *********************************************************** */
14972 
14973 /*                                              From STRIPACK */
14974 /*                                            Robert J. Renka */
14975 /*                                  Dept. of Computer Science */
14976 /*                                       Univ. of North Texas */
14977 /*                                           renka@cs.unt.edu */
14978 /*                                                   07/20/96 */
14979 
14980 /*   This subroutine converts a triangulation data structure */
14981 /* from the linked list created by Subroutine TRMESH to a */
14982 /* triangle list. */
14983 
14984 /* On input: */
14985 
14986 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14987 
14988 /*       LIST,LPTR,LEND = Linked list data structure defin- */
14989 /*                        ing the triangulation.  Refer to */
14990 /*                        Subroutine TRMESH. */
14991 
14992 /*       NROW = Number of rows (entries per triangle) re- */
14993 /*              served for the triangle list LTRI.  The value */
14994 /*              must be 6 if only the vertex indexes and */
14995 /*              neighboring triangle indexes are to be */
14996 /*              stored, or 9 if arc indexes are also to be */
14997 /*              assigned and stored.  Refer to LTRI. */
14998 
14999 /* The above parameters are not altered by this routine. */
15000 
15001 /*       LTRI = int array of length at least NROW*NT, */
15002 /*              where NT is at most 2N-4.  (A sufficient */
15003 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
15004 
15005 /* On output: */
15006 
15007 /*       NT = Number of triangles in the triangulation unless */
15008 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
15009 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
15010 /*            number of boundary nodes. */
15011 
15012 /*       LTRI = NROW by NT array whose J-th column contains */
15013 /*              the vertex nodal indexes (first three rows), */
15014 /*              neighboring triangle indexes (second three */
15015 /*              rows), and, if NROW = 9, arc indexes (last */
15016 /*              three rows) associated with triangle J for */
15017 /*              J = 1,...,NT.  The vertices are ordered */
15018 /*              counterclockwise with the first vertex taken */
15019 /*              to be the one with smallest index.  Thus, */
15020 /*              LTRI(2,J) and LTRI(3,J) are larger than */
15021 /*              LTRI(1,J) and index adjacent neighbors of */
15022 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
15023 /*              and LTRI(I+6,J) index the triangle and arc, */
15024 /*              respectively, which are opposite (not shared */
15025 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
15026 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
15027 /*              indexes range from 1 to N, triangle indexes */
15028 /*              from 0 to NT, and, if included, arc indexes */
15029 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
15030 /*              or 3N-6 if NB = 0.  The triangles are or- */
15031 /*              dered on first (smallest) vertex indexes. */
15032 
15033 /*       IER = Error indicator. */
15034 /*             IER = 0 if no errors were encountered. */
15035 /*             IER = 1 if N or NROW is outside its valid */
15036 /*                     range on input. */
15037 /*             IER = 2 if the triangulation data structure */
15038 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
15039 /*                     however, that these arrays are not */
15040 /*                     completely tested for validity. */
15041 
15042 /* Modules required by TRLIST:  None */
15043 
15044 /* Intrinsic function called by TRLIST:  ABS */
15045 
15046 /* *********************************************************** */
15047 
15048 
15049 /* Local parameters: */
15050 
15051 /* ARCS =     long int variable with value TRUE iff are */
15052 /*              indexes are to be stored */
15053 /* I,J =      LTRI row indexes (1 to 3) associated with */
15054 /*              triangles KT and KN, respectively */
15055 /* I1,I2,I3 = Nodal indexes of triangle KN */
15056 /* ISV =      Variable used to permute indexes I1,I2,I3 */
15057 /* KA =       Arc index and number of currently stored arcs */
15058 /* KN =       Index of the triangle that shares arc I1-I2 */
15059 /*              with KT */
15060 /* KT =       Triangle index and number of currently stored */
15061 /*              triangles */
15062 /* LP =       LIST pointer */
15063 /* LP2 =      Pointer to N2 as a neighbor of N1 */
15064 /* LPL =      Pointer to the last neighbor of I1 */
15065 /* LPLN1 =    Pointer to the last neighbor of N1 */
15066 /* N1,N2,N3 = Nodal indexes of triangle KT */
15067 /* NM2 =      N-2 */
15068 
15069 
15070 /* Test for invalid input parameters. */
15071 
15072     /* Parameter adjustments */
15073     --lend;
15074     --list;
15075     --lptr;
15076     ltri_dim1 = *nrow;
15077     ltri_offset = 1 + ltri_dim1;
15078     ltri -= ltri_offset;
15079 
15080     /* Function Body */
15081     if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15082         goto L11;
15083     }
15084 
15085 /* Initialize parameters for loop on triangles KT = (N1,N2, */
15086 /*   N3), where N1 < N2 and N1 < N3. */
15087 
15088 /*   ARCS = TRUE iff arc indexes are to be stored. */
15089 /*   KA,KT = Numbers of currently stored arcs and triangles. */
15090 /*   NM2 = Upper bound on candidates for N1. */
15091 
15092     arcs = *nrow == 9;
15093     ka = 0;
15094     kt = 0;
15095     nm2 = *n - 2;
15096 
15097 /* Loop on nodes N1. */
15098 
15099     i__1 = nm2;
15100     for (n1 = 1; n1 <= i__1; ++n1) {
15101 
15102 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
15103 /*   to the last neighbor of N1, and LP2 points to N2. */
15104 
15105         lpln1 = lend[n1];
15106         lp2 = lpln1;
15107 L1:
15108         lp2 = lptr[lp2];
15109         n2 = list[lp2];
15110         lp = lptr[lp2];
15111         n3 = (i__2 = list[lp], abs(i__2));
15112         if (n2 < n1 || n3 < n1) {
15113             goto L8;
15114         }
15115 
15116 /* Add a new triangle KT = (N1,N2,N3). */
15117 
15118         ++kt;
15119         ltri[kt * ltri_dim1 + 1] = n1;
15120         ltri[kt * ltri_dim1 + 2] = n2;
15121         ltri[kt * ltri_dim1 + 3] = n3;
15122 
15123 /* Loop on triangle sides (I2,I1) with neighboring triangles */
15124 /*   KN = (I1,I2,I3). */
15125 
15126         for (i__ = 1; i__ <= 3; ++i__) {
15127             if (i__ == 1) {
15128                 i1 = n3;
15129                 i2 = n2;
15130             } else if (i__ == 2) {
15131                 i1 = n1;
15132                 i2 = n3;
15133             } else {
15134                 i1 = n2;
15135                 i2 = n1;
15136             }
15137 
15138 /* Set I3 to the neighbor of I1 that follows I2 unless */
15139 /*   I2->I1 is a boundary arc. */
15140 
15141             lpl = lend[i1];
15142             lp = lptr[lpl];
15143 L2:
15144             if (list[lp] == i2) {
15145                 goto L3;
15146             }
15147             lp = lptr[lp];
15148             if (lp != lpl) {
15149                 goto L2;
15150             }
15151 
15152 /*   I2 is the last neighbor of I1 unless the data structure */
15153 /*     is invalid.  Bypass the search for a neighboring */
15154 /*     triangle if I2->I1 is a boundary arc. */
15155 
15156             if ((i__2 = list[lp], abs(i__2)) != i2) {
15157                 goto L12;
15158             }
15159             kn = 0;
15160             if (list[lp] < 0) {
15161                 goto L6;
15162             }
15163 
15164 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15165 /*     a neighbor of I1. */
15166 
15167 L3:
15168             lp = lptr[lp];
15169             i3 = (i__2 = list[lp], abs(i__2));
15170 
15171 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15172 /*   and permute the vertex indexes of KN so that I1 is */
15173 /*   smallest. */
15174 
15175             if (i1 < i2 && i1 < i3) {
15176                 j = 3;
15177             } else if (i2 < i3) {
15178                 j = 2;
15179                 isv = i1;
15180                 i1 = i2;
15181                 i2 = i3;
15182                 i3 = isv;
15183             } else {
15184                 j = 1;
15185                 isv = i1;
15186                 i1 = i3;
15187                 i3 = i2;
15188                 i2 = isv;
15189             }
15190 
15191 /* Test for KN > KT (triangle index not yet assigned). */
15192 
15193             if (i1 > n1) {
15194                 goto L7;
15195             }
15196 
15197 /* Find KN, if it exists, by searching the triangle list in */
15198 /*   reverse order. */
15199 
15200             for (kn = kt - 1; kn >= 1; --kn) {
15201                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15202                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15203                     goto L5;
15204                 }
15205 /* L4: */
15206             }
15207             goto L7;
15208 
15209 /* Store KT as a neighbor of KN. */
15210 
15211 L5:
15212             ltri[j + 3 + kn * ltri_dim1] = kt;
15213 
15214 /* Store KN as a neighbor of KT, and add a new arc KA. */
15215 
15216 L6:
15217             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15218             if (arcs) {
15219                 ++ka;
15220                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15221                 if (kn != 0) {
15222                     ltri[j + 6 + kn * ltri_dim1] = ka;
15223                 }
15224             }
15225 L7:
15226             ;
15227         }
15228 
15229 /* Bottom of loop on triangles. */
15230 
15231 L8:
15232         if (lp2 != lpln1) {
15233             goto L1;
15234         }
15235 /* L9: */
15236     }
15237 
15238 /* No errors encountered. */
15239 
15240     *nt = kt;
15241     *ier = 0;
15242     return 0;
15243 
15244 /* Invalid input parameter. */
15245 
15246 L11:
15247     *nt = 0;
15248     *ier = 1;
15249     return 0;
15250 
15251 /* Invalid triangulation data structure:  I1 is a neighbor of */
15252 /*   I2, but I2 is not a neighbor of I1. */
15253 
15254 L12:
15255     *nt = 0;
15256     *ier = 2;
15257     return 0;
15258 } /* trlist_ */
15259 
15260 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15261         double *z__, int *iflag, int *nrow, int *nt, int *
15262         ltri, int *lout)
15263 {
15264     /* Initialized data */
15265 
15266     static int nmax = 9999;
15267     static int nlmax = 58;
15268 
15269     /* System generated locals */
15270     int ltri_dim1, ltri_offset, i__1;
15271 
15272     /* Local variables */
15273     static int i__, k, na, nb, nl, lun;
15274 
15275 
15276 /* *********************************************************** */
15277 
15278 /*                                              From STRIPACK */
15279 /*                                            Robert J. Renka */
15280 /*                                  Dept. of Computer Science */
15281 /*                                       Univ. of North Texas */
15282 /*                                           renka@cs.unt.edu */
15283 /*                                                   07/02/98 */
15284 
15285 /*   This subroutine prints the triangle list created by Sub- */
15286 /* routine TRLIST and, optionally, the nodal coordinates */
15287 /* (either latitude and longitude or Cartesian coordinates) */
15288 /* on long int unit LOUT.  The numbers of boundary nodes, */
15289 /* triangles, and arcs are also printed. */
15290 
15291 
15292 /* On input: */
15293 
15294 /*       N = Number of nodes in the triangulation. */
15295 /*           3 .LE. N .LE. 9999. */
15296 
15297 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15298 /*               coordinates of the nodes if IFLAG = 0, or */
15299 /*               (X and Y only) arrays of length N containing */
15300 /*               longitude and latitude, respectively, if */
15301 /*               IFLAG > 0, or unused dummy parameters if */
15302 /*               IFLAG < 0. */
15303 
15304 /*       IFLAG = Nodal coordinate option indicator: */
15305 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15306 /*                         Cartesian coordinates) are to be */
15307 /*                         printed (to 6 decimal places). */
15308 /*               IFLAG > 0 if only X and Y (assumed to con- */
15309 /*                         tain longitude and latitude) are */
15310 /*                         to be printed (to 6 decimal */
15311 /*                         places). */
15312 /*               IFLAG < 0 if only the adjacency lists are to */
15313 /*                         be printed. */
15314 
15315 /*       NROW = Number of rows (entries per triangle) re- */
15316 /*              served for the triangle list LTRI.  The value */
15317 /*              must be 6 if only the vertex indexes and */
15318 /*              neighboring triangle indexes are stored, or 9 */
15319 /*              if arc indexes are also stored. */
15320 
15321 /*       NT = Number of triangles in the triangulation. */
15322 /*            1 .LE. NT .LE. 9999. */
15323 
15324 /*       LTRI = NROW by NT array whose J-th column contains */
15325 /*              the vertex nodal indexes (first three rows), */
15326 /*              neighboring triangle indexes (second three */
15327 /*              rows), and, if NROW = 9, arc indexes (last */
15328 /*              three rows) associated with triangle J for */
15329 /*              J = 1,...,NT. */
15330 
15331 /*       LOUT = long int unit number for output.  If LOUT is */
15332 /*              not in the range 0 to 99, output is written */
15333 /*              to unit 6. */
15334 
15335 /* Input parameters are not altered by this routine. */
15336 
15337 /* On output: */
15338 
15339 /*   The triangle list and nodal coordinates (as specified by */
15340 /* IFLAG) are written to unit LOUT. */
15341 
15342 /* Modules required by TRLPRT:  None */
15343 
15344 /* *********************************************************** */
15345 
15346     /* Parameter adjustments */
15347     --z__;
15348     --y;
15349     --x;
15350     ltri_dim1 = *nrow;
15351     ltri_offset = 1 + ltri_dim1;
15352     ltri -= ltri_offset;
15353 
15354     /* Function Body */
15355 
15356 /* Local parameters: */
15357 
15358 /* I =     DO-loop, nodal index, and row index for LTRI */
15359 /* K =     DO-loop and triangle index */
15360 /* LUN =   long int unit number for output */
15361 /* NA =    Number of triangulation arcs */
15362 /* NB =    Number of boundary nodes */
15363 /* NL =    Number of lines printed on the current page */
15364 /* NLMAX = Maximum number of print lines per page (except */
15365 /*           for the last page which may have two addi- */
15366 /*           tional lines) */
15367 /* NMAX =  Maximum value of N and NT (4-digit format) */
15368 
15369     lun = *lout;
15370     if (lun < 0 || lun > 99) {
15371         lun = 6;
15372     }
15373 
15374 /* Print a heading and test for invalid input. */
15375 
15376 /*      WRITE (LUN,100) N */
15377     nl = 3;
15378     if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15379             nmax) {
15380 
15381 /* Print an error message and exit. */
15382 
15383 /*        WRITE (LUN,110) N, NROW, NT */
15384         return 0;
15385     }
15386     if (*iflag == 0) {
15387 
15388 /* Print X, Y, and Z. */
15389 
15390 /*        WRITE (LUN,101) */
15391         nl = 6;
15392         i__1 = *n;
15393         for (i__ = 1; i__ <= i__1; ++i__) {
15394             if (nl >= nlmax) {
15395 /*            WRITE (LUN,108) */
15396                 nl = 0;
15397             }
15398 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15399             ++nl;
15400 /* L1: */
15401         }
15402     } else if (*iflag > 0) {
15403 
15404 /* Print X (longitude) and Y (latitude). */
15405 
15406 /*        WRITE (LUN,102) */
15407         nl = 6;
15408         i__1 = *n;
15409         for (i__ = 1; i__ <= i__1; ++i__) {
15410             if (nl >= nlmax) {
15411 /*            WRITE (LUN,108) */
15412                 nl = 0;
15413             }
15414 /*          WRITE (LUN,104) I, X(I), Y(I) */
15415             ++nl;
15416 /* L2: */
15417         }
15418     }
15419 
15420 /* Print the triangulation LTRI. */
15421 
15422     if (nl > nlmax / 2) {
15423 /*        WRITE (LUN,108) */
15424         nl = 0;
15425     }
15426     if (*nrow == 6) {
15427 /*        WRITE (LUN,105) */
15428     } else {
15429 /*        WRITE (LUN,106) */
15430     }
15431     nl += 5;
15432     i__1 = *nt;
15433     for (k = 1; k <= i__1; ++k) {
15434         if (nl >= nlmax) {
15435 /*          WRITE (LUN,108) */
15436             nl = 0;
15437         }
15438 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15439         ++nl;
15440 /* L3: */
15441     }
15442 
15443 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15444 /*   triangles). */
15445 
15446     nb = (*n << 1) - *nt - 2;
15447     if (nb < 3) {
15448         nb = 0;
15449         na = *n * 3 - 6;
15450     } else {
15451         na = *nt + *n - 1;
15452     }
15453 /*      WRITE (LUN,109) NB, NA, NT */
15454     return 0;
15455 
15456 /* Print formats: */
15457 
15458 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15459 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15460 /*     .        'Z(Node)'//) */
15461 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15462 /*  103 FORMAT (8X,I4,3D17.6) */
15463 /*  104 FORMAT (16X,I4,2D17.6) */
15464 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15465 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15466 /*     .        'KT2',4X,'KT3'/) */
15467 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15468 /*     .        14X,'Arcs'/ */
15469 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15470 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15471 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15472 /*  108 FORMAT (///) */
15473 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15474 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15475 /*     .        ' Triangles') */
15476 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15477 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15478 } /* trlprt_ */
15479 
15480 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15481         double *z__, int *list, int *lptr, int *lend, int
15482         *lnew, int *near__, int *next, double *dist, int *ier)
15483 {
15484     /* System generated locals */
15485     int i__1, i__2;
15486 
15487     /* Local variables */
15488     static double d__;
15489     static int i__, j, k;
15490     static double d1, d2, d3;
15491     static int i0, lp, nn, lpl;
15492     long int left_(double *, double *, double *, double
15493             *, double *, double *, double *, double *,
15494             double *);
15495     static int nexti;
15496 
15497 
15498 /* *********************************************************** */
15499 
15500 /*                                              From STRIPACK */
15501 /*                                            Robert J. Renka */
15502 /*                                  Dept. of Computer Science */
15503 /*                                       Univ. of North Texas */
15504 /*                                           renka@cs.unt.edu */
15505 /*                                                   03/04/03 */
15506 
15507 /*   This subroutine creates a Delaunay triangulation of a */
15508 /* set of N arbitrarily distributed points, referred to as */
15509 /* nodes, on the surface of the unit sphere.  The Delaunay */
15510 /* triangulation is defined as a set of (spherical) triangles */
15511 /* with the following five properties: */
15512 
15513 /*  1)  The triangle vertices are nodes. */
15514 /*  2)  No triangle contains a node other than its vertices. */
15515 /*  3)  The interiors of the triangles are pairwise disjoint. */
15516 /*  4)  The union of triangles is the convex hull of the set */
15517 /*        of nodes (the smallest convex set that contains */
15518 /*        the nodes).  If the nodes are not contained in a */
15519 /*        single hemisphere, their convex hull is the en- */
15520 /*        tire sphere and there are no boundary nodes. */
15521 /*        Otherwise, there are at least three boundary nodes. */
15522 /*  5)  The interior of the circumcircle of each triangle */
15523 /*        contains no node. */
15524 
15525 /* The first four properties define a triangulation, and the */
15526 /* last property results in a triangulation which is as close */
15527 /* as possible to equiangular in a certain sense and which is */
15528 /* uniquely defined unless four or more nodes lie in a common */
15529 /* plane.  This property makes the triangulation well-suited */
15530 /* for solving closest-point problems and for triangle-based */
15531 /* interpolation. */
15532 
15533 /*   The algorithm has expected time complexity O(N*log(N)) */
15534 /* for most nodal distributions. */
15535 
15536 /*   Spherical coordinates (latitude and longitude) may be */
15537 /* converted to Cartesian coordinates by Subroutine TRANS. */
15538 
15539 /*   The following is a list of the software package modules */
15540 /* which a user may wish to call directly: */
15541 
15542 /*  ADDNOD - Updates the triangulation by appending a new */
15543 /*             node. */
15544 
15545 /*  AREAS  - Returns the area of a spherical triangle. */
15546 
15547 /*  AREAV  - Returns the area of a Voronoi region associated */
15548 /*           with an interior node without requiring that the */
15549 /*           entire Voronoi diagram be computed and stored. */
15550 
15551 /*  BNODES - Returns an array containing the indexes of the */
15552 /*             boundary nodes (if any) in counterclockwise */
15553 /*             order.  Counts of boundary nodes, triangles, */
15554 /*             and arcs are also returned. */
15555 
15556 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15557 /*           formly spaced points on the unit circle centered */
15558 /*           at (0,0). */
15559 
15560 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15561 /*             gle. */
15562 
15563 /*  CRLIST - Returns the set of triangle circumcenters */
15564 /*             (Voronoi vertices) and circumradii associated */
15565 /*             with a triangulation. */
15566 
15567 /*  DELARC - Deletes a boundary arc from a triangulation. */
15568 
15569 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15570 
15571 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15572 /*             ted by an arc in the triangulation. */
15573 
15574 /*  GETNP  - Determines the ordered sequence of L closest */
15575 /*             nodes to a given node, along with the associ- */
15576 /*             ated distances. */
15577 
15578 /*  INSIDE - Locates a point relative to a polygon on the */
15579 /*             surface of the sphere. */
15580 
15581 /*  INTRSC - Returns the point of intersection between a */
15582 /*             pair of great circle arcs. */
15583 
15584 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15585 /*             int. */
15586 
15587 /*  LEFT   - Locates a point relative to a great circle. */
15588 
15589 /*  NEARND - Returns the index of the nearest node to an */
15590 /*             arbitrary point, along with its squared */
15591 /*             distance. */
15592 
15593 /*  PROJCT - Applies a perspective-depth projection to a */
15594 /*             point in 3-space. */
15595 
15596 /*  SCOORD - Converts a point from Cartesian coordinates to */
15597 /*             spherical coordinates. */
15598 
15599 /*  STORE  - Forces a value to be stored in main memory so */
15600 /*             that the precision of floating point numbers */
15601 /*             in memory locations rather than registers is */
15602 /*             computed. */
15603 
15604 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15605 /*             coordinates on the unit sphere for input to */
15606 /*             Subroutine TRMESH. */
15607 
15608 /*  TRLIST - Converts the triangulation data structure to a */
15609 /*             triangle list more suitable for use in a fin- */
15610 /*             ite element code. */
15611 
15612 /*  TRLPRT - Prints the triangle list created by Subroutine */
15613 /*             TRLIST. */
15614 
15615 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15616 /*             nodes. */
15617 
15618 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15619 /*             file containing a triangulation plot. */
15620 
15621 /*  TRPRNT - Prints the triangulation data structure and, */
15622 /*             optionally, the nodal coordinates. */
15623 
15624 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15625 /*             file containing a Voronoi diagram plot. */
15626 
15627 
15628 /* On input: */
15629 
15630 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15631 
15632 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15633 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15634 /*               Z(K)) is referred to as node K, and K is re- */
15635 /*               ferred to as a nodal index.  It is required */
15636 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15637 /*               K.  The first three nodes must not be col- */
15638 /*               linear (lie on a common great circle). */
15639 
15640 /* The above parameters are not altered by this routine. */
15641 
15642 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15643 
15644 /*       LEND = Array of length at least N. */
15645 
15646 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15647 /*                        least N.  The space is used to */
15648 /*                        efficiently determine the nearest */
15649 /*                        triangulation node to each un- */
15650 /*                        processed node for use by ADDNOD. */
15651 
15652 /* On output: */
15653 
15654 /*       LIST = Set of nodal indexes which, along with LPTR, */
15655 /*              LEND, and LNEW, define the triangulation as a */
15656 /*              set of N adjacency lists -- counterclockwise- */
15657 /*              ordered sequences of neighboring nodes such */
15658 /*              that the first and last neighbors of a bound- */
15659 /*              ary node are boundary nodes (the first neigh- */
15660 /*              bor of an interior node is arbitrary).  In */
15661 /*              order to distinguish between interior and */
15662 /*              boundary nodes, the last neighbor of each */
15663 /*              boundary node is represented by the negative */
15664 /*              of its index. */
15665 
15666 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15667 /*              correspondence with the elements of LIST. */
15668 /*              LIST(LPTR(I)) indexes the node which follows */
15669 /*              LIST(I) in cyclical counterclockwise order */
15670 /*              (the first neighbor follows the last neigh- */
15671 /*              bor). */
15672 
15673 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15674 /*              points to the last neighbor of node K for */
15675 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15676 /*              only if K is a boundary node. */
15677 
15678 /*       LNEW = Pointer to the first empty location in LIST */
15679 /*              and LPTR (list length plus one).  LIST, LPTR, */
15680 /*              LEND, and LNEW are not altered if IER < 0, */
15681 /*              and are incomplete if IER > 0. */
15682 
15683 /*       NEAR,NEXT,DIST = Garbage. */
15684 
15685 /*       IER = Error indicator: */
15686 /*             IER =  0 if no errors were encountered. */
15687 /*             IER = -1 if N < 3 on input. */
15688 /*             IER = -2 if the first three nodes are */
15689 /*                      collinear. */
15690 /*             IER =  L if nodes L and M coincide for some */
15691 /*                      M > L.  The data structure represents */
15692 /*                      a triangulation of nodes 1 to M-1 in */
15693 /*                      this case. */
15694 
15695 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15696 /*                                INSERT, INTADD, JRAND, */
15697 /*                                LEFT, LSTPTR, STORE, SWAP, */
15698 /*                                SWPTST, TRFIND */
15699 
15700 /* Intrinsic function called by TRMESH:  ABS */
15701 
15702 /* *********************************************************** */
15703 
15704 
15705 /* Local parameters: */
15706 
15707 /* D =        (Negative cosine of) distance from node K to */
15708 /*              node I */
15709 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15710 /*              respectively */
15711 /* I,J =      Nodal indexes */
15712 /* I0 =       Index of the node preceding I in a sequence of */
15713 /*              unprocessed nodes:  I = NEXT(I0) */
15714 /* K =        Index of node to be added and DO-loop index: */
15715 /*              K > 3 */
15716 /* LP =       LIST index (pointer) of a neighbor of K */
15717 /* LPL =      Pointer to the last neighbor of K */
15718 /* NEXTI =    NEXT(I) */
15719 /* NN =       Local copy of N */
15720 
15721     /* Parameter adjustments */
15722     --dist;
15723     --next;
15724     --near__;
15725     --lend;
15726     --z__;
15727     --y;
15728     --x;
15729     --list;
15730     --lptr;
15731 
15732     /* Function Body */
15733     nn = *n;
15734     if (nn < 3) {
15735         *ier = -1;
15736         return 0;
15737     }
15738 
15739 /* Store the first triangle in the linked list. */
15740 
15741     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15742             z__[3])) {
15743 
15744 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15745 
15746         list[1] = 3;
15747         lptr[1] = 2;
15748         list[2] = -2;
15749         lptr[2] = 1;
15750         lend[1] = 2;
15751 
15752         list[3] = 1;
15753         lptr[3] = 4;
15754         list[4] = -3;
15755         lptr[4] = 3;
15756         lend[2] = 4;
15757 
15758         list[5] = 2;
15759         lptr[5] = 6;
15760         list[6] = -1;
15761         lptr[6] = 5;
15762         lend[3] = 6;
15763 
15764     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15765             y[3], &z__[3])) {
15766 
15767 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15768 /*     i.e., node 3 lies in the left hemisphere defined by */
15769 /*     arc 1->2. */
15770 
15771         list[1] = 2;
15772         lptr[1] = 2;
15773         list[2] = -3;
15774         lptr[2] = 1;
15775         lend[1] = 2;
15776 
15777         list[3] = 3;
15778         lptr[3] = 4;
15779         list[4] = -1;
15780         lptr[4] = 3;
15781         lend[2] = 4;
15782 
15783         list[5] = 1;
15784         lptr[5] = 6;
15785         list[6] = -2;
15786         lptr[6] = 5;
15787         lend[3] = 6;
15788 
15789     } else {
15790 
15791 /*   The first three nodes are collinear. */
15792 
15793         *ier = -2;
15794         return 0;
15795     }
15796 
15797 /* Initialize LNEW and test for N = 3. */
15798 
15799     *lnew = 7;
15800     if (nn == 3) {
15801         *ier = 0;
15802         return 0;
15803     }
15804 
15805 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15806 /*   used to obtain an expected-time (N*log(N)) incremental */
15807 /*   algorithm by enabling constant search time for locating */
15808 /*   each new node in the triangulation. */
15809 
15810 /* For each unprocessed node K, NEAR(K) is the index of the */
15811 /*   triangulation node closest to K (used as the starting */
15812 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15813 /*   is an increasing function of the arc length (angular */
15814 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15815 /*   length a. */
15816 
15817 /* Since it is necessary to efficiently find the subset of */
15818 /*   unprocessed nodes associated with each triangulation */
15819 /*   node J (those that have J as their NEAR entries), the */
15820 /*   subsets are stored in NEAR and NEXT as follows:  for */
15821 /*   each node J in the triangulation, I = NEAR(J) is the */
15822 /*   first unprocessed node in J's set (with I = 0 if the */
15823 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15824 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15825 /*   set are initially ordered by increasing indexes (which */
15826 /*   maximizes efficiency) but that ordering is not main- */
15827 /*   tained as the data structure is updated. */
15828 
15829 /* Initialize the data structure for the single triangle. */
15830 
15831     near__[1] = 0;
15832     near__[2] = 0;
15833     near__[3] = 0;
15834     for (k = nn; k >= 4; --k) {
15835         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15836         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15837         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15838         if (d1 <= d2 && d1 <= d3) {
15839             near__[k] = 1;
15840             dist[k] = d1;
15841             next[k] = near__[1];
15842             near__[1] = k;
15843         } else if (d2 <= d1 && d2 <= d3) {
15844             near__[k] = 2;
15845             dist[k] = d2;
15846             next[k] = near__[2];
15847             near__[2] = k;
15848         } else {
15849             near__[k] = 3;
15850             dist[k] = d3;
15851             next[k] = near__[3];
15852             near__[3] = k;
15853         }
15854 /* L1: */
15855     }
15856 
15857 /* Add the remaining nodes */
15858 
15859     i__1 = nn;
15860     for (k = 4; k <= i__1; ++k) {
15861         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15862                 lend[1], lnew, ier);
15863         if (*ier != 0) {
15864             return 0;
15865         }
15866 
15867 /* Remove K from the set of unprocessed nodes associated */
15868 /*   with NEAR(K). */
15869 
15870         i__ = near__[k];
15871         if (near__[i__] == k) {
15872             near__[i__] = next[k];
15873         } else {
15874             i__ = near__[i__];
15875 L2:
15876             i0 = i__;
15877             i__ = next[i0];
15878             if (i__ != k) {
15879                 goto L2;
15880             }
15881             next[i0] = next[k];
15882         }
15883         near__[k] = 0;
15884 
15885 /* Loop on neighbors J of node K. */
15886 
15887         lpl = lend[k];
15888         lp = lpl;
15889 L3:
15890         lp = lptr[lp];
15891         j = (i__2 = list[lp], abs(i__2));
15892 
15893 /* Loop on elements I in the sequence of unprocessed nodes */
15894 /*   associated with J:  K is a candidate for replacing J */
15895 /*   as the nearest triangulation node to I.  The next value */
15896 /*   of I in the sequence, NEXT(I), must be saved before I */
15897 /*   is moved because it is altered by adding I to K's set. */
15898 
15899         i__ = near__[j];
15900 L4:
15901         if (i__ == 0) {
15902             goto L5;
15903         }
15904         nexti = next[i__];
15905 
15906 /* Test for the distance from I to K less than the distance */
15907 /*   from I to J. */
15908 
15909         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15910         if (d__ < dist[i__]) {
15911 
15912 /* Replace J by K as the nearest triangulation node to I: */
15913 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15914 /*   of unprocessed nodes and add it to K's set. */
15915 
15916             near__[i__] = k;
15917             dist[i__] = d__;
15918             if (i__ == near__[j]) {
15919                 near__[j] = nexti;
15920             } else {
15921                 next[i0] = nexti;
15922             }
15923             next[i__] = near__[k];
15924             near__[k] = i__;
15925         } else {
15926             i0 = i__;
15927         }
15928 
15929 /* Bottom of loop on I. */
15930 
15931         i__ = nexti;
15932         goto L4;
15933 
15934 /* Bottom of loop on neighbors J. */
15935 
15936 L5:
15937         if (lp != lpl) {
15938             goto L3;
15939         }
15940 /* L6: */
15941     }
15942     return 0;
15943 } /* trmesh_ */
15944 
15945 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
15946         elat, double *elon, double *a, int *n, double *x,
15947         double *y, double *z__, int *list, int *lptr, int
15948         *lend, char *, long int *numbr, int *ier, short )
15949 {
15950     /* Initialized data */
15951 
15952     static long int annot = TRUE_;
15953     static double fsizn = 10.;
15954     static double fsizt = 16.;
15955     static double tol = .5;
15956 
15957     /* System generated locals */
15958     int i__1, i__2;
15959     double d__1;
15960 
15961     /* Builtin functions */
15962     //double atan(double), sin(double);
15963     //int i_dnnt(double *);
15964     //double cos(double), sqrt(double);
15965 
15966     /* Local variables */
15967     static double t;
15968     static int n0, n1;
15969     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15970     static int ir, lp;
15971     static double ex, ey, ez, wr, tx, ty;
15972     static int lpl;
15973     static double wrs;
15974     static int ipx1, ipx2, ipy1, ipy2, nseg;
15975     /* Subroutine */ int drwarc_(int *, double *, double *,
15976              double *, int *);
15977 
15978 
15979 /* *********************************************************** */
15980 
15981 /*                                              From STRIPACK */
15982 /*                                            Robert J. Renka */
15983 /*                                  Dept. of Computer Science */
15984 /*                                       Univ. of North Texas */
15985 /*                                           renka@cs.unt.edu */
15986 /*                                                   03/04/03 */
15987 
15988 /*   This subroutine creates a level-2 Encapsulated Post- */
15989 /* script (EPS) file containing a graphical display of a */
15990 /* triangulation of a set of nodes on the surface of the unit */
15991 /* sphere.  The visible portion of the triangulation is */
15992 /* projected onto the plane that contains the origin and has */
15993 /* normal defined by a user-specified eye-position. */
15994 
15995 
15996 /* On input: */
15997 
15998 /*       LUN = long int unit number in the range 0 to 99. */
15999 /*             The unit should be opened with an appropriate */
16000 /*             file name before the call to this routine. */
16001 
16002 /*       PLTSIZ = Plot size in inches.  A circular window in */
16003 /*                the projection plane is mapped to a circu- */
16004 /*                lar viewport with diameter equal to .88* */
16005 /*                PLTSIZ (leaving room for labels outside the */
16006 /*                viewport).  The viewport is centered on the */
16007 /*                8.5 by 11 inch page, and its boundary is */
16008 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16009 
16010 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16011 /*                   the center of projection E (the center */
16012 /*                   of the plot).  The projection plane is */
16013 /*                   the plane that contains the origin and */
16014 /*                   has E as unit normal.  In a rotated */
16015 /*                   coordinate system for which E is the */
16016 /*                   north pole, the projection plane con- */
16017 /*                   tains the equator, and only northern */
16018 /*                   hemisphere nodes are visible (from the */
16019 /*                   point at infinity in the direction E). */
16020 /*                   These are projected orthogonally onto */
16021 /*                   the projection plane (by zeroing the z- */
16022 /*                   component in the rotated coordinate */
16023 /*                   system).  ELAT and ELON must be in the */
16024 /*                   range -90 to 90 and -180 to 180, respec- */
16025 /*                   tively. */
16026 
16027 /*       A = Angular distance in degrees from E to the boun- */
16028 /*           dary of a circular window against which the */
16029 /*           triangulation is clipped.  The projected window */
16030 /*           is a disk of radius r = Sin(A) centered at the */
16031 /*           origin, and only visible nodes whose projections */
16032 /*           are within distance r of the origin are included */
16033 /*           in the plot.  Thus, if A = 90, the plot includes */
16034 /*           the entire hemisphere centered at E.  0 .LT. A */
16035 /*           .LE. 90. */
16036 
16037 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
16038 
16039 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16040 /*               coordinates of the nodes (unit vectors). */
16041 
16042 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16043 /*                        gulation.  Refer to Subroutine */
16044 /*                        TRMESH. */
16045 
16046 /*       TITLE = Type CHARACTER variable or constant contain- */
16047 /*               ing a string to be centered above the plot. */
16048 /*               The string must be enclosed in parentheses; */
16049 /*               i.e., the first and last characters must be */
16050 /*               '(' and ')', respectively, but these are not */
16051 /*               displayed.  TITLE may have at most 80 char- */
16052 /*               acters including the parentheses. */
16053 
16054 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16055 /*               nodal indexes are plotted next to the nodes. */
16056 
16057 /* Input parameters are not altered by this routine. */
16058 
16059 /* On output: */
16060 
16061 /*       IER = Error indicator: */
16062 /*             IER = 0 if no errors were encountered. */
16063 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
16064 /*                     valid range. */
16065 /*             IER = 2 if ELAT, ELON, or A is outside its */
16066 /*                     valid range. */
16067 /*             IER = 3 if an error was encountered in writing */
16068 /*                     to unit LUN. */
16069 
16070 /*   The values in the data statement below may be altered */
16071 /* in order to modify various plotting options. */
16072 
16073 /* Module required by TRPLOT:  DRWARC */
16074 
16075 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
16076 /*                                          DBLE, NINT, SIN, */
16077 /*                                          SQRT */
16078 
16079 /* *********************************************************** */
16080 
16081 
16082     /* Parameter adjustments */
16083     --lend;
16084     --z__;
16085     --y;
16086     --x;
16087     --list;
16088     --lptr;
16089 
16090     /* Function Body */
16091 
16092 /* Local parameters: */
16093 
16094 /* ANNOT =     long int variable with value TRUE iff the plot */
16095 /*               is to be annotated with the values of ELAT, */
16096 /*               ELON, and A */
16097 /* CF =        Conversion factor for degrees to radians */
16098 /* CT =        Cos(ELAT) */
16099 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16100 /* FSIZN =     Font size in points for labeling nodes with */
16101 /*               their indexes if NUMBR = TRUE */
16102 /* FSIZT =     Font size in points for the title (and */
16103 /*               annotation if ANNOT = TRUE) */
16104 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16105 /*               left corner of the bounding box or viewport */
16106 /*               box */
16107 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16108 /*               right corner of the bounding box or viewport */
16109 /*               box */
16110 /* IR =        Half the width (height) of the bounding box or */
16111 /*               viewport box in points -- viewport radius */
16112 /* LP =        LIST index (pointer) */
16113 /* LPL =       Pointer to the last neighbor of N0 */
16114 /* N0 =        Index of a node whose incident arcs are to be */
16115 /*               drawn */
16116 /* N1 =        Neighbor of N0 */
16117 /* NSEG =      Number of line segments used by DRWARC in a */
16118 /*               polygonal approximation to a projected edge */
16119 /* P0 =        Coordinates of N0 in the rotated coordinate */
16120 /*               system or label location (first two */
16121 /*               components) */
16122 /* P1 =        Coordinates of N1 in the rotated coordinate */
16123 /*               system or intersection of edge N0-N1 with */
16124 /*               the equator (in the rotated coordinate */
16125 /*               system) */
16126 /* R11...R23 = Components of the first two rows of a rotation */
16127 /*               that maps E to the north pole (0,0,1) */
16128 /* SF =        Scale factor for mapping world coordinates */
16129 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16130 /*               to viewport coordinates in [IPX1,IPX2] X */
16131 /*               [IPY1,IPY2] */
16132 /* T =         Temporary variable */
16133 /* TOL =       Maximum distance in points between a projected */
16134 /*               triangulation edge and its approximation by */
16135 /*               a polygonal curve */
16136 /* TX,TY =     Translation vector for mapping world coordi- */
16137 /*               nates to viewport coordinates */
16138 /* WR =        Window radius r = Sin(A) */
16139 /* WRS =       WR**2 */
16140 
16141 
16142 /* Test for invalid parameters. */
16143 
16144     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16145         goto L11;
16146     }
16147     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16148         goto L12;
16149     }
16150 
16151 /* Compute a conversion factor CF for degrees to radians */
16152 /*   and compute the window radius WR. */
16153 
16154     cf = atan(1.) / 45.;
16155     wr = sin(cf * *a);
16156     wrs = wr * wr;
16157 
16158 /* Compute the lower left (IPX1,IPY1) and upper right */
16159 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16160 /*   The coordinates, specified in default user space units */
16161 /*   (points, at 72 points/inch with origin at the lower */
16162 /*   left corner of the page), are chosen to preserve the */
16163 /*   square aspect ratio, and to center the plot on the 8.5 */
16164 /*   by 11 inch page.  The center of the page is (306,396), */
16165 /*   and IR = PLTSIZ/2 in points. */
16166 
16167     d__1 = *pltsiz * 36.;
16168     ir = i_dnnt(&d__1);
16169     ipx1 = 306 - ir;
16170     ipx2 = ir + 306;
16171     ipy1 = 396 - ir;
16172     ipy2 = ir + 396;
16173 
16174 /* Output header comments. */
16175 
16176 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16177 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16178 /*     .        '%%BoundingBox:',4I4/ */
16179 /*     .        '%%Title:  Triangulation'/ */
16180 /*     .        '%%Creator:  STRIPACK'/ */
16181 /*     .        '%%EndComments') */
16182 
16183 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16184 /*   of a viewport box obtained by shrinking the bounding box */
16185 /*   by 12% in each dimension. */
16186 
16187     d__1 = (double) ir * .88;
16188     ir = i_dnnt(&d__1);
16189     ipx1 = 306 - ir;
16190     ipx2 = ir + 306;
16191     ipy1 = 396 - ir;
16192     ipy2 = ir + 396;
16193 
16194 /* Set the line thickness to 2 points, and draw the */
16195 /*   viewport boundary. */
16196 
16197     t = 2.;
16198 /*      WRITE (LUN,110,ERR=13) T */
16199 /*      WRITE (LUN,120,ERR=13) IR */
16200 /*      WRITE (LUN,130,ERR=13) */
16201 /*  110 FORMAT (F12.6,' setlinewidth') */
16202 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16203 /*  130 FORMAT ('stroke') */
16204 
16205 /* Set up an affine mapping from the window box [-WR,WR] X */
16206 /*   [-WR,WR] to the viewport box. */
16207 
16208     sf = (double) ir / wr;
16209     tx = ipx1 + sf * wr;
16210     ty = ipy1 + sf * wr;
16211 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16212 /*  140 FORMAT (2F12.6,' translate'/ */
16213 /*    .        2F12.6,' scale') */
16214 
16215 /* The line thickness must be changed to reflect the new */
16216 /*   scaling which is applied to all subsequent output. */
16217 /*   Set it to 1.0 point. */
16218 
16219     t = 1. / sf;
16220 /*      WRITE (LUN,110,ERR=13) T */
16221 
16222 /* Save the current graphics state, and set the clip path to */
16223 /*   the boundary of the window. */
16224 
16225 /*      WRITE (LUN,150,ERR=13) */
16226 /*      WRITE (LUN,160,ERR=13) WR */
16227 /*      WRITE (LUN,170,ERR=13) */
16228 /*  150 FORMAT ('gsave') */
16229 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16230 /*  170 FORMAT ('clip newpath') */
16231 
16232 /* Compute the Cartesian coordinates of E and the components */
16233 /*   of a rotation R which maps E to the north pole (0,0,1). */
16234 /*   R is taken to be a rotation about the z-axis (into the */
16235 /*   yz-plane) followed by a rotation about the x-axis chosen */
16236 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16237 /*   E is the north or south pole. */
16238 
16239 /*           ( R11  R12  0   ) */
16240 /*       R = ( R21  R22  R23 ) */
16241 /*           ( EX   EY   EZ  ) */
16242 
16243     t = cf * *elon;
16244     ct = cos(cf * *elat);
16245     ex = ct * cos(t);
16246     ey = ct * sin(t);
16247     ez = sin(cf * *elat);
16248     if (ct != 0.) {
16249         r11 = -ey / ct;
16250         r12 = ex / ct;
16251     } else {
16252         r11 = 0.;
16253         r12 = 1.;
16254     }
16255     r21 = -ez * r12;
16256     r22 = ez * r11;
16257     r23 = ct;
16258 
16259 /* Loop on visible nodes N0 that project to points */
16260 /*   (P0(1),P0(2)) in the window. */
16261 
16262     i__1 = *n;
16263     for (n0 = 1; n0 <= i__1; ++n0) {
16264         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16265         if (p0[2] < 0.) {
16266             goto L3;
16267         }
16268         p0[0] = r11 * x[n0] + r12 * y[n0];
16269         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16270         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16271             goto L3;
16272         }
16273         lpl = lend[n0];
16274         lp = lpl;
16275 
16276 /* Loop on neighbors N1 of N0.  LPL points to the last */
16277 /*   neighbor of N0.  Copy the components of N1 into P. */
16278 
16279 L1:
16280         lp = lptr[lp];
16281         n1 = (i__2 = list[lp], abs(i__2));
16282         p1[0] = r11 * x[n1] + r12 * y[n1];
16283         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16284         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16285         if (p1[2] < 0.) {
16286 
16287 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16288 /*     intersection of edge N0-N1 with the equator so that */
16289 /*     the edge is clipped properly.  P1(3) is set to 0. */
16290 
16291             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16292             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16293             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16294             p1[0] /= t;
16295             p1[1] /= t;
16296         }
16297 
16298 /*   If node N1 is in the window and N1 < N0, bypass edge */
16299 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16300 
16301         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16302             goto L2;
16303         }
16304 
16305 /*   Add the edge to the path.  (TOL is converted to world */
16306 /*     coordinates.) */
16307 
16308         if (p1[2] < 0.) {
16309             p1[2] = 0.;
16310         }
16311         d__1 = tol / sf;
16312         drwarc_(lun, p0, p1, &d__1, &nseg);
16313 
16314 /* Bottom of loops. */
16315 
16316 L2:
16317         if (lp != lpl) {
16318             goto L1;
16319         }
16320 L3:
16321         ;
16322     }
16323 
16324 /* Paint the path and restore the saved graphics state (with */
16325 /*   no clip path). */
16326 
16327 /*      WRITE (LUN,130,ERR=13) */
16328 /*      WRITE (LUN,190,ERR=13) */
16329 /*  190 FORMAT ('grestore') */
16330     if (*numbr) {
16331 
16332 /* Nodes in the window are to be labeled with their indexes. */
16333 /*   Convert FSIZN from points to world coordinates, and */
16334 /*   output the commands to select a font and scale it. */
16335 
16336         t = fsizn / sf;
16337 /*        WRITE (LUN,200,ERR=13) T */
16338 /*  200   FORMAT ('/Helvetica findfont'/ */
16339 /*     .          F12.6,' scalefont setfont') */
16340 
16341 /* Loop on visible nodes N0 that project to points */
16342 /*   P0 = (P0(1),P0(2)) in the window. */
16343 
16344         i__1 = *n;
16345         for (n0 = 1; n0 <= i__1; ++n0) {
16346             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16347                 goto L4;
16348             }
16349             p0[0] = r11 * x[n0] + r12 * y[n0];
16350             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16351             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16352                 goto L4;
16353             }
16354 
16355 /*   Move to P0 and draw the label N0.  The first character */
16356 /*     will will have its lower left corner about one */
16357 /*     character width to the right of the nodal position. */
16358 
16359 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16360 /*          WRITE (LUN,220,ERR=13) N0 */
16361 /*  210     FORMAT (2F12.6,' moveto') */
16362 /*  220     FORMAT ('(',I3,') show') */
16363 L4:
16364             ;
16365         }
16366     }
16367 
16368 /* Convert FSIZT from points to world coordinates, and output */
16369 /*   the commands to select a font and scale it. */
16370 
16371     t = fsizt / sf;
16372 /*      WRITE (LUN,200,ERR=13) T */
16373 
16374 /* Display TITLE centered above the plot: */
16375 
16376     p0[1] = wr + t * 3.;
16377 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16378 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16379 /*     .        ' moveto') */
16380 /*      WRITE (LUN,240,ERR=13) TITLE */
16381 /*  240 FORMAT (A80/'  show') */
16382     if (annot) {
16383 
16384 /* Display the window center and radius below the plot. */
16385 
16386         p0[0] = -wr;
16387         p0[1] = -wr - 50. / sf;
16388 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16389 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16390         p0[1] -= t * 2.;
16391 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16392 /*        WRITE (LUN,260,ERR=13) A */
16393 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16394 /*     .          ',  ELON = ',F8.2,') show') */
16395 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16396     }
16397 
16398 /* Paint the path and output the showpage command and */
16399 /*   end-of-file indicator. */
16400 
16401 /*      WRITE (LUN,270,ERR=13) */
16402 /*  270 FORMAT ('stroke'/ */
16403 /*     .        'showpage'/ */
16404 /*     .        '%%EOF') */
16405 
16406 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16407 /*   indicator (to eliminate a timeout error message): */
16408 /*   ASCII 4. */
16409 
16410 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16411 /*  280 FORMAT (A1) */
16412 
16413 /* No error encountered. */
16414 
16415     *ier = 0;
16416     return 0;
16417 
16418 /* Invalid input parameter LUN, PLTSIZ, or N. */
16419 
16420 L11:
16421     *ier = 1;
16422     return 0;
16423 
16424 /* Invalid input parameter ELAT, ELON, or A. */
16425 
16426 L12:
16427     *ier = 2;
16428     return 0;
16429 
16430 /* Error writing to unit LUN. */
16431 
16432 /* L13: */
16433     *ier = 3;
16434     return 0;
16435 } /* trplot_ */
16436 
16437 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16438         double *z__, int *iflag, int *list, int *lptr,
16439         int *lend, int *lout)
16440 {
16441     /* Initialized data */
16442 
16443     static int nmax = 9999;
16444     static int nlmax = 58;
16445 
16446     /* System generated locals */
16447     int i__1;
16448 
16449     /* Local variables */
16450     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16451             400];
16452 
16453 
16454 /* *********************************************************** */
16455 
16456 /*                                              From STRIPACK */
16457 /*                                            Robert J. Renka */
16458 /*                                  Dept. of Computer Science */
16459 /*                                       Univ. of North Texas */
16460 /*                                           renka@cs.unt.edu */
16461 /*                                                   07/25/98 */
16462 
16463 /*   This subroutine prints the triangulation adjacency lists */
16464 /* created by Subroutine TRMESH and, optionally, the nodal */
16465 /* coordinates (either latitude and longitude or Cartesian */
16466 /* coordinates) on long int unit LOUT.  The list of neighbors */
16467 /* of a boundary node is followed by index 0.  The numbers of */
16468 /* boundary nodes, triangles, and arcs are also printed. */
16469 
16470 
16471 /* On input: */
16472 
16473 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16474 /*           and N .LE. 9999. */
16475 
16476 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16477 /*               coordinates of the nodes if IFLAG = 0, or */
16478 /*               (X and Y only) arrays of length N containing */
16479 /*               longitude and latitude, respectively, if */
16480 /*               IFLAG > 0, or unused dummy parameters if */
16481 /*               IFLAG < 0. */
16482 
16483 /*       IFLAG = Nodal coordinate option indicator: */
16484 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16485 /*                         Cartesian coordinates) are to be */
16486 /*                         printed (to 6 decimal places). */
16487 /*               IFLAG > 0 if only X and Y (assumed to con- */
16488 /*                         tain longitude and latitude) are */
16489 /*                         to be printed (to 6 decimal */
16490 /*                         places). */
16491 /*               IFLAG < 0 if only the adjacency lists are to */
16492 /*                         be printed. */
16493 
16494 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16495 /*                        gulation.  Refer to Subroutine */
16496 /*                        TRMESH. */
16497 
16498 /*       LOUT = long int unit for output.  If LOUT is not in */
16499 /*              the range 0 to 99, output is written to */
16500 /*              long int unit 6. */
16501 
16502 /* Input parameters are not altered by this routine. */
16503 
16504 /* On output: */
16505 
16506 /*   The adjacency lists and nodal coordinates (as specified */
16507 /* by IFLAG) are written to unit LOUT. */
16508 
16509 /* Modules required by TRPRNT:  None */
16510 
16511 /* *********************************************************** */
16512 
16513     /* Parameter adjustments */
16514     --lend;
16515     --z__;
16516     --y;
16517     --x;
16518     --list;
16519     --lptr;
16520 
16521     /* Function Body */
16522 
16523 /* Local parameters: */
16524 
16525 /* I =     NABOR index (1 to K) */
16526 /* INC =   Increment for NL associated with an adjacency list */
16527 /* K =     Counter and number of neighbors of NODE */
16528 /* LP =    LIST pointer of a neighbor of NODE */
16529 /* LPL =   Pointer to the last neighbor of NODE */
16530 /* LUN =   long int unit for output (copy of LOUT) */
16531 /* NA =    Number of arcs in the triangulation */
16532 /* NABOR = Array containing the adjacency list associated */
16533 /*           with NODE, with zero appended if NODE is a */
16534 /*           boundary node */
16535 /* NB =    Number of boundary nodes encountered */
16536 /* ND =    Index of a neighbor of NODE (or negative index) */
16537 /* NL =    Number of lines that have been printed on the */
16538 /*           current page */
16539 /* NLMAX = Maximum number of print lines per page (except */
16540 /*           for the last page which may have two addi- */
16541 /*           tional lines) */
16542 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16543 /* NODE =  Index of a node and DO-loop index (1 to N) */
16544 /* NN =    Local copy of N */
16545 /* NT =    Number of triangles in the triangulation */
16546 
16547     nn = *n;
16548     lun = *lout;
16549     if (lun < 0 || lun > 99) {
16550         lun = 6;
16551     }
16552 
16553 /* Print a heading and test the range of N. */
16554 
16555 /*      WRITE (LUN,100) NN */
16556     if (nn < 3 || nn > nmax) {
16557 
16558 /* N is outside its valid range. */
16559 
16560 /*        WRITE (LUN,110) */
16561         return 0;
16562     }
16563 
16564 /* Initialize NL (the number of lines printed on the current */
16565 /*   page) and NB (the number of boundary nodes encountered). */
16566 
16567     nl = 6;
16568     nb = 0;
16569     if (*iflag < 0) {
16570 
16571 /* Print LIST only.  K is the number of neighbors of NODE */
16572 /*   that have been stored in NABOR. */
16573 
16574 /*        WRITE (LUN,101) */
16575         i__1 = nn;
16576         for (node = 1; node <= i__1; ++node) {
16577             lpl = lend[node];
16578             lp = lpl;
16579             k = 0;
16580 
16581 L1:
16582             ++k;
16583             lp = lptr[lp];
16584             nd = list[lp];
16585             nabor[k - 1] = nd;
16586             if (lp != lpl) {
16587                 goto L1;
16588             }
16589             if (nd <= 0) {
16590 
16591 /*   NODE is a boundary node.  Correct the sign of the last */
16592 /*     neighbor, add 0 to the end of the list, and increment */
16593 /*     NB. */
16594 
16595                 nabor[k - 1] = -nd;
16596                 ++k;
16597                 nabor[k - 1] = 0;
16598                 ++nb;
16599             }
16600 
16601 /*   Increment NL and print the list of neighbors. */
16602 
16603             inc = (k - 1) / 14 + 2;
16604             nl += inc;
16605             if (nl > nlmax) {
16606 /*            WRITE (LUN,108) */
16607                 nl = inc;
16608             }
16609 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16610 /*          IF (K .NE. 14) */
16611 /*           WRITE (LUN,107) */
16612 /* L2: */
16613         }
16614     } else if (*iflag > 0) {
16615 
16616 /* Print X (longitude), Y (latitude), and LIST. */
16617 
16618 /*        WRITE (LUN,102) */
16619         i__1 = nn;
16620         for (node = 1; node <= i__1; ++node) {
16621             lpl = lend[node];
16622             lp = lpl;
16623             k = 0;
16624 
16625 L3:
16626             ++k;
16627             lp = lptr[lp];
16628             nd = list[lp];
16629             nabor[k - 1] = nd;
16630             if (lp != lpl) {
16631                 goto L3;
16632             }
16633             if (nd <= 0) {
16634 
16635 /*   NODE is a boundary node. */
16636 
16637                 nabor[k - 1] = -nd;
16638                 ++k;
16639                 nabor[k - 1] = 0;
16640                 ++nb;
16641             }
16642 
16643 /*   Increment NL and print X, Y, and NABOR. */
16644 
16645             inc = (k - 1) / 8 + 2;
16646             nl += inc;
16647             if (nl > nlmax) {
16648 /*            WRITE (LUN,108) */
16649                 nl = inc;
16650             }
16651 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16652 /*          IF (K .NE. 8) */
16653 /*           PRINT *,K */
16654 /*           WRITE (LUN,107) */
16655 /* L4: */
16656         }
16657     } else {
16658 
16659 /* Print X, Y, Z, and LIST. */
16660 
16661 /*        WRITE (LUN,103) */
16662         i__1 = nn;
16663         for (node = 1; node <= i__1; ++node) {
16664             lpl = lend[node];
16665             lp = lpl;
16666             k = 0;
16667 
16668 L5:
16669             ++k;
16670             lp = lptr[lp];
16671             nd = list[lp];
16672             nabor[k - 1] = nd;
16673             if (lp != lpl) {
16674                 goto L5;
16675             }
16676             if (nd <= 0) {
16677 
16678 /*   NODE is a boundary node. */
16679 
16680                 nabor[k - 1] = -nd;
16681                 ++k;
16682                 nabor[k - 1] = 0;
16683                 ++nb;
16684             }
16685 
16686 /*   Increment NL and print X, Y, Z, and NABOR. */
16687 
16688             inc = (k - 1) / 5 + 2;
16689             nl += inc;
16690             if (nl > nlmax) {
16691 /*            WRITE (LUN,108) */
16692                 nl = inc;
16693             }
16694 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16695 /*          IF (K .NE. 5) */
16696 /*           print *,K */
16697 /*           WRITE (LUN,107) */
16698 /* L6: */
16699         }
16700     }
16701 
16702 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16703 /*   triangles). */
16704 
16705     if (nb != 0) {
16706         na = nn * 3 - nb - 3;
16707         nt = (nn << 1) - nb - 2;
16708     } else {
16709         na = nn * 3 - 6;
16710         nt = (nn << 1) - 4;
16711     }
16712 /*      WRITE (LUN,109) NB, NA, NT */
16713     return 0;
16714 
16715 /* Print formats: */
16716 
16717 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16718 /*     .        'Structure,  N = ',I5//) */
16719 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16720 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16721 /*     .        18X,'Neighbors of Node'//) */
16722 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16723 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16724 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16725 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16726 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16727 /*  107 FORMAT (1X) */
16728 /*  108 FORMAT (///) */
16729 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16730 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16731 /*     .        ' Triangles') */
16732 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16733 /*     .        ' range ***') */
16734 } /* trprnt_ */
16735 
16736 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16737         elat, double *elon, double *a, int *n, double *x,
16738         double *y, double *z__, int *nt, int *listc, int *
16739         lptr, int *lend, double *xc, double *yc, double *zc,
16740         char *, long int *numbr, int *ier, short)
16741 {
16742     /* Initialized data */
16743 
16744     static long int annot = TRUE_;
16745     static double fsizn = 10.;
16746     static double fsizt = 16.;
16747     static double tol = .5;
16748 
16749     /* System generated locals */
16750     int i__1;
16751     double d__1;
16752 
16753     /* Builtin functions */
16754     //double atan(double), sin(double);
16755     //int i_dnnt(double *);
16756     //double cos(double), sqrt(double);
16757 
16758     /* Local variables */
16759     static double t;
16760     static int n0;
16761     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16762             sf;
16763     static int ir, lp;
16764     static double ex, ey, ez, wr, tx, ty;
16765     static long int in1, in2;
16766     static int kv1, kv2, lpl;
16767     static double wrs;
16768     static int ipx1, ipx2, ipy1, ipy2, nseg;
16769     /* Subroutine */ int drwarc_(int *, double *, double *,
16770              double *, int *);
16771 
16772 
16773 /* *********************************************************** */
16774 
16775 /*                                              From STRIPACK */
16776 /*                                            Robert J. Renka */
16777 /*                                  Dept. of Computer Science */
16778 /*                                       Univ. of North Texas */
16779 /*                                           renka@cs.unt.edu */
16780 /*                                                   03/04/03 */
16781 
16782 /*   This subroutine creates a level-2 Encapsulated Post- */
16783 /* script (EPS) file containing a graphical depiction of a */
16784 /* Voronoi diagram of a set of nodes on the unit sphere. */
16785 /* The visible portion of the diagram is projected orthog- */
16786 /* onally onto the plane that contains the origin and has */
16787 /* normal defined by a user-specified eye-position. */
16788 
16789 /*   The parameters defining the Voronoi diagram may be com- */
16790 /* puted by Subroutine CRLIST. */
16791 
16792 
16793 /* On input: */
16794 
16795 /*       LUN = long int unit number in the range 0 to 99. */
16796 /*             The unit should be opened with an appropriate */
16797 /*             file name before the call to this routine. */
16798 
16799 /*       PLTSIZ = Plot size in inches.  A circular window in */
16800 /*                the projection plane is mapped to a circu- */
16801 /*                lar viewport with diameter equal to .88* */
16802 /*                PLTSIZ (leaving room for labels outside the */
16803 /*                viewport).  The viewport is centered on the */
16804 /*                8.5 by 11 inch page, and its boundary is */
16805 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16806 
16807 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16808 /*                   the center of projection E (the center */
16809 /*                   of the plot).  The projection plane is */
16810 /*                   the plane that contains the origin and */
16811 /*                   has E as unit normal.  In a rotated */
16812 /*                   coordinate system for which E is the */
16813 /*                   north pole, the projection plane con- */
16814 /*                   tains the equator, and only northern */
16815 /*                   hemisphere points are visible (from the */
16816 /*                   point at infinity in the direction E). */
16817 /*                   These are projected orthogonally onto */
16818 /*                   the projection plane (by zeroing the z- */
16819 /*                   component in the rotated coordinate */
16820 /*                   system).  ELAT and ELON must be in the */
16821 /*                   range -90 to 90 and -180 to 180, respec- */
16822 /*                   tively. */
16823 
16824 /*       A = Angular distance in degrees from E to the boun- */
16825 /*           dary of a circular window against which the */
16826 /*           Voronoi diagram is clipped.  The projected win- */
16827 /*           dow is a disk of radius r = Sin(A) centered at */
16828 /*           the origin, and only visible vertices whose */
16829 /*           projections are within distance r of the origin */
16830 /*           are included in the plot.  Thus, if A = 90, the */
16831 /*           plot includes the entire hemisphere centered at */
16832 /*           E.  0 .LT. A .LE. 90. */
16833 
16834 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16835 /*           regions.  N .GE. 3. */
16836 
16837 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16838 /*               coordinates of the nodes (unit vectors). */
16839 
16840 /*       NT = Number of Voronoi region vertices (triangles, */
16841 /*            including those in the extended triangulation */
16842 /*            if the number of boundary nodes NB is nonzero): */
16843 /*            NT = 2*N-4. */
16844 
16845 /*       LISTC = Array of length 3*NT containing triangle */
16846 /*               indexes (indexes to XC, YC, and ZC) stored */
16847 /*               in 1-1 correspondence with LIST/LPTR entries */
16848 /*               (or entries that would be stored in LIST for */
16849 /*               the extended triangulation):  the index of */
16850 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16851 /*               LISTC(L), and LISTC(M), where LIST(K), */
16852 /*               LIST(L), and LIST(M) are the indexes of N2 */
16853 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16854 /*               and N1 as a neighbor of N3.  The Voronoi */
16855 /*               region associated with a node is defined by */
16856 /*               the CCW-ordered sequence of circumcenters in */
16857 /*               one-to-one correspondence with its adjacency */
16858 /*               list (in the extended triangulation). */
16859 
16860 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16861 /*              set of pointers (LISTC indexes) in one-to-one */
16862 /*              correspondence with the elements of LISTC. */
16863 /*              LISTC(LPTR(I)) indexes the triangle which */
16864 /*              follows LISTC(I) in cyclical counterclockwise */
16865 /*              order (the first neighbor follows the last */
16866 /*              neighbor). */
16867 
16868 /*       LEND = Array of length N containing a set of */
16869 /*              pointers to triangle lists.  LP = LEND(K) */
16870 /*              points to a triangle (indexed by LISTC(LP)) */
16871 /*              containing node K for K = 1 to N. */
16872 
16873 /*       XC,YC,ZC = Arrays of length NT containing the */
16874 /*                  Cartesian coordinates of the triangle */
16875 /*                  circumcenters (Voronoi vertices). */
16876 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16877 
16878 /*       TITLE = Type CHARACTER variable or constant contain- */
16879 /*               ing a string to be centered above the plot. */
16880 /*               The string must be enclosed in parentheses; */
16881 /*               i.e., the first and last characters must be */
16882 /*               '(' and ')', respectively, but these are not */
16883 /*               displayed.  TITLE may have at most 80 char- */
16884 /*               acters including the parentheses. */
16885 
16886 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16887 /*               nodal indexes are plotted at the Voronoi */
16888 /*               region centers. */
16889 
16890 /* Input parameters are not altered by this routine. */
16891 
16892 /* On output: */
16893 
16894 /*       IER = Error indicator: */
16895 /*             IER = 0 if no errors were encountered. */
16896 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16897 /*                     its valid range. */
16898 /*             IER = 2 if ELAT, ELON, or A is outside its */
16899 /*                     valid range. */
16900 /*             IER = 3 if an error was encountered in writing */
16901 /*                     to unit LUN. */
16902 
16903 /* Module required by VRPLOT:  DRWARC */
16904 
16905 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16906 /*                                          DBLE, NINT, SIN, */
16907 /*                                          SQRT */
16908 
16909 /* *********************************************************** */
16910 
16911 
16912     /* Parameter adjustments */
16913     --lend;
16914     --z__;
16915     --y;
16916     --x;
16917     --zc;
16918     --yc;
16919     --xc;
16920     --listc;
16921     --lptr;
16922 
16923     /* Function Body */
16924 
16925 /* Local parameters: */
16926 
16927 /* ANNOT =     long int variable with value TRUE iff the plot */
16928 /*               is to be annotated with the values of ELAT, */
16929 /*               ELON, and A */
16930 /* CF =        Conversion factor for degrees to radians */
16931 /* CT =        Cos(ELAT) */
16932 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16933 /* FSIZN =     Font size in points for labeling nodes with */
16934 /*               their indexes if NUMBR = TRUE */
16935 /* FSIZT =     Font size in points for the title (and */
16936 /*               annotation if ANNOT = TRUE) */
16937 /* IN1,IN2 =   long int variables with value TRUE iff the */
16938 /*               projections of vertices KV1 and KV2, respec- */
16939 /*               tively, are inside the window */
16940 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16941 /*               left corner of the bounding box or viewport */
16942 /*               box */
16943 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16944 /*               right corner of the bounding box or viewport */
16945 /*               box */
16946 /* IR =        Half the width (height) of the bounding box or */
16947 /*               viewport box in points -- viewport radius */
16948 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
16949 /* LP =        LIST index (pointer) */
16950 /* LPL =       Pointer to the last neighbor of N0 */
16951 /* N0 =        Index of a node */
16952 /* NSEG =      Number of line segments used by DRWARC in a */
16953 /*               polygonal approximation to a projected edge */
16954 /* P1 =        Coordinates of vertex KV1 in the rotated */
16955 /*               coordinate system */
16956 /* P2 =        Coordinates of vertex KV2 in the rotated */
16957 /*               coordinate system or intersection of edge */
16958 /*               KV1-KV2 with the equator (in the rotated */
16959 /*               coordinate system) */
16960 /* R11...R23 = Components of the first two rows of a rotation */
16961 /*               that maps E to the north pole (0,0,1) */
16962 /* SF =        Scale factor for mapping world coordinates */
16963 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16964 /*               to viewport coordinates in [IPX1,IPX2] X */
16965 /*               [IPY1,IPY2] */
16966 /* T =         Temporary variable */
16967 /* TOL =       Maximum distance in points between a projected */
16968 /*               Voronoi edge and its approximation by a */
16969 /*               polygonal curve */
16970 /* TX,TY =     Translation vector for mapping world coordi- */
16971 /*               nates to viewport coordinates */
16972 /* WR =        Window radius r = Sin(A) */
16973 /* WRS =       WR**2 */
16974 /* X0,Y0 =     Projection plane coordinates of node N0 or */
16975 /*               label location */
16976 
16977 
16978 /* Test for invalid parameters. */
16979 
16980     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16981             nt != 2 * *n - 4) {
16982         goto L11;
16983     }
16984     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16985         goto L12;
16986     }
16987 
16988 /* Compute a conversion factor CF for degrees to radians */
16989 /*   and compute the window radius WR. */
16990 
16991     cf = atan(1.) / 45.;
16992     wr = sin(cf * *a);
16993     wrs = wr * wr;
16994 
16995 /* Compute the lower left (IPX1,IPY1) and upper right */
16996 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16997 /*   The coordinates, specified in default user space units */
16998 /*   (points, at 72 points/inch with origin at the lower */
16999 /*   left corner of the page), are chosen to preserve the */
17000 /*   square aspect ratio, and to center the plot on the 8.5 */
17001 /*   by 11 inch page.  The center of the page is (306,396), */
17002 /*   and IR = PLTSIZ/2 in points. */
17003 
17004     d__1 = *pltsiz * 36.;
17005     ir = i_dnnt(&d__1);
17006     ipx1 = 306 - ir;
17007     ipx2 = ir + 306;
17008     ipy1 = 396 - ir;
17009     ipy2 = ir + 396;
17010 
17011 /* Output header comments. */
17012 
17013 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
17014 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
17015 /*     .        '%%BoundingBox:',4I4/ */
17016 /*     .        '%%Title:  Voronoi diagram'/ */
17017 /*     .        '%%Creator:  STRIPACK'/ */
17018 /*     .        '%%EndComments') */
17019 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
17020 /*   of a viewport box obtained by shrinking the bounding box */
17021 /*   by 12% in each dimension. */
17022 
17023     d__1 = (double) ir * .88;
17024     ir = i_dnnt(&d__1);
17025     ipx1 = 306 - ir;
17026     ipx2 = ir + 306;
17027     ipy1 = 396 - ir;
17028     ipy2 = ir + 396;
17029 
17030 /* Set the line thickness to 2 points, and draw the */
17031 /*   viewport boundary. */
17032 
17033     t = 2.;
17034 /*      WRITE (LUN,110,ERR=13) T */
17035 /*      WRITE (LUN,120,ERR=13) IR */
17036 /*      WRITE (LUN,130,ERR=13) */
17037 /*  110 FORMAT (F12.6,' setlinewidth') */
17038 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
17039 /*  130 FORMAT ('stroke') */
17040 
17041 /* Set up an affine mapping from the window box [-WR,WR] X */
17042 /*   [-WR,WR] to the viewport box. */
17043 
17044     sf = (double) ir / wr;
17045     tx = ipx1 + sf * wr;
17046     ty = ipy1 + sf * wr;
17047 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
17048 /*  140 FORMAT (2F12.6,' translate'/ */
17049 /*     .        2F12.6,' scale') */
17050 
17051 /* The line thickness must be changed to reflect the new */
17052 /*   scaling which is applied to all subsequent output. */
17053 /*   Set it to 1.0 point. */
17054 
17055     t = 1. / sf;
17056 /*      WRITE (LUN,110,ERR=13) T */
17057 
17058 /* Save the current graphics state, and set the clip path to */
17059 /*   the boundary of the window. */
17060 
17061 /*      WRITE (LUN,150,ERR=13) */
17062 /*      WRITE (LUN,160,ERR=13) WR */
17063 /*      WRITE (LUN,170,ERR=13) */
17064 /*  150 FORMAT ('gsave') */
17065 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
17066 /*  170 FORMAT ('clip newpath') */
17067 
17068 /* Compute the Cartesian coordinates of E and the components */
17069 /*   of a rotation R which maps E to the north pole (0,0,1). */
17070 /*   R is taken to be a rotation about the z-axis (into the */
17071 /*   yz-plane) followed by a rotation about the x-axis chosen */
17072 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
17073 /*   E is the north or south pole. */
17074 
17075 /*           ( R11  R12  0   ) */
17076 /*       R = ( R21  R22  R23 ) */
17077 /*           ( EX   EY   EZ  ) */
17078 
17079     t = cf * *elon;
17080     ct = cos(cf * *elat);
17081     ex = ct * cos(t);
17082     ey = ct * sin(t);
17083     ez = sin(cf * *elat);
17084     if (ct != 0.) {
17085         r11 = -ey / ct;
17086         r12 = ex / ct;
17087     } else {
17088         r11 = 0.;
17089         r12 = 1.;
17090     }
17091     r21 = -ez * r12;
17092     r22 = ez * r11;
17093     r23 = ct;
17094 
17095 /* Loop on nodes (Voronoi centers) N0. */
17096 /*   LPL indexes the last neighbor of N0. */
17097 
17098     i__1 = *n;
17099     for (n0 = 1; n0 <= i__1; ++n0) {
17100         lpl = lend[n0];
17101 
17102 /* Set KV2 to the first (and last) vertex index and compute */
17103 /*   its coordinates P2 in the rotated coordinate system. */
17104 
17105         kv2 = listc[lpl];
17106         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17107         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17108         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17109 
17110 /*   IN2 = TRUE iff KV2 is in the window. */
17111 
17112         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17113 
17114 /* Loop on neighbors N1 of N0.  For each triangulation edge */
17115 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
17116 
17117         lp = lpl;
17118 L1:
17119         lp = lptr[lp];
17120         kv1 = kv2;
17121         p1[0] = p2[0];
17122         p1[1] = p2[1];
17123         p1[2] = p2[2];
17124         in1 = in2;
17125         kv2 = listc[lp];
17126 
17127 /*   Compute the new values of P2 and IN2. */
17128 
17129         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17130         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17131         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17132         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17133 
17134 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
17135 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
17136 /*   outside (so that the edge is drawn only once). */
17137 
17138         if (! in1 || (in2 && kv2 <= kv1)) {
17139             goto L2;
17140         }
17141         if (p2[2] < 0.) {
17142 
17143 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17144 /*     intersection of edge KV1-KV2 with the equator so that */
17145 /*     the edge is clipped properly.  P2(3) is set to 0. */
17146 
17147             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17148             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17149             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17150             p2[0] /= t;
17151             p2[1] /= t;
17152         }
17153 
17154 /*   Add the edge to the path.  (TOL is converted to world */
17155 /*     coordinates.) */
17156 
17157         if (p2[2] < 0.) {
17158             p2[2] = 0.f;
17159         }
17160         d__1 = tol / sf;
17161         drwarc_(lun, p1, p2, &d__1, &nseg);
17162 
17163 /* Bottom of loops. */
17164 
17165 L2:
17166         if (lp != lpl) {
17167             goto L1;
17168         }
17169 /* L3: */
17170     }
17171 
17172 /* Paint the path and restore the saved graphics state (with */
17173 /*   no clip path). */
17174 
17175 /*      WRITE (LUN,130,ERR=13) */
17176 /*      WRITE (LUN,190,ERR=13) */
17177 /*  190 FORMAT ('grestore') */
17178     if (*numbr) {
17179 
17180 /* Nodes in the window are to be labeled with their indexes. */
17181 /*   Convert FSIZN from points to world coordinates, and */
17182 /*   output the commands to select a font and scale it. */
17183 
17184         t = fsizn / sf;
17185 /*        WRITE (LUN,200,ERR=13) T */
17186 /*  200   FORMAT ('/Helvetica findfont'/ */
17187 /*     .          F12.6,' scalefont setfont') */
17188 
17189 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17190 /*   the window. */
17191 
17192         i__1 = *n;
17193         for (n0 = 1; n0 <= i__1; ++n0) {
17194             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17195                 goto L4;
17196             }
17197             x0 = r11 * x[n0] + r12 * y[n0];
17198             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17199             if (x0 * x0 + y0 * y0 > wrs) {
17200                 goto L4;
17201             }
17202 
17203 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17204 /*     of the first character at (X0,Y0). */
17205 
17206 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17207 /*          WRITE (LUN,220,ERR=13) N0 */
17208 /*  210     FORMAT (2F12.6,' moveto') */
17209 /*  220     FORMAT ('(',I3,') show') */
17210 L4:
17211             ;
17212         }
17213     }
17214 
17215 /* Convert FSIZT from points to world coordinates, and output */
17216 /*   the commands to select a font and scale it. */
17217 
17218     t = fsizt / sf;
17219 /*      WRITE (LUN,200,ERR=13) T */
17220 
17221 /* Display TITLE centered above the plot: */
17222 
17223     y0 = wr + t * 3.;
17224 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17225 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17226 /*     .        ' moveto') */
17227 /*      WRITE (LUN,240,ERR=13) TITLE */
17228 /*  240 FORMAT (A80/'  show') */
17229     if (annot) {
17230 
17231 /* Display the window center and radius below the plot. */
17232 
17233         x0 = -wr;
17234         y0 = -wr - 50. / sf;
17235 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17236 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17237         y0 -= t * 2.;
17238 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17239 /*        WRITE (LUN,260,ERR=13) A */
17240 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17241 /*     .          ',  ELON = ',F8.2,') show') */
17242 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17243     }
17244 
17245 /* Paint the path and output the showpage command and */
17246 /*   end-of-file indicator. */
17247 
17248 /*      WRITE (LUN,270,ERR=13) */
17249 /*  270 FORMAT ('stroke'/ */
17250 /*     .        'showpage'/ */
17251 /*     .        '%%EOF') */
17252 
17253 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17254 /*   indicator (to eliminate a timeout error message): */
17255 /*   ASCII 4. */
17256 
17257 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17258 /*  280 FORMAT (A1) */
17259 
17260 /* No error encountered. */
17261 
17262     *ier = 0;
17263     return 0;
17264 
17265 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17266 
17267 L11:
17268     *ier = 1;
17269     return 0;
17270 
17271 /* Invalid input parameter ELAT, ELON, or A. */
17272 
17273 L12:
17274     *ier = 2;
17275     return 0;
17276 
17277 /* Error writing to unit LUN. */
17278 
17279 /* L13: */
17280     *ier = 3;
17281     return 0;
17282 } /* vrplot_ */
17283 
17284 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17285         double *rannum)
17286 {
17287     static double x;
17288 
17289 
17290 /*   This routine returns pseudo-random numbers uniformly */
17291 /* distributed in the interval (0,1).  int seeds IX, IY, */
17292 /* and IZ should be initialized to values in the range 1 to */
17293 /* 30,000 before the first call to RANDOM, and should not */
17294 /* be altered between subsequent calls (unless a sequence */
17295 /* of random numbers is to be repeated by reinitializing the */
17296 /* seeds). */
17297 
17298 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17299 /*             and Portable Pseudo-random Number Generator, */
17300 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17301 /*             pp. 188-190. */
17302 
17303     *ix = *ix * 171 % 30269;
17304     *iy = *iy * 172 % 30307;
17305     *iz = *iz * 170 % 30323;
17306     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17307             double) (*iz) / 30323.;
17308     *rannum = x - (int) x;
17309     return 0;
17310 } /* random_ */
17311 
17312 #undef TRUE_
17313 #undef FALSE_
17314 #undef abs
17315 
17316 /*################################################################################################
17317 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17318 ######   You must link the resulting object file with the libraries: #############################
17319 ####################    -lf2c -lm   (in that order)   ############################################
17320 ################################################################################################*/
17321 
17322 
17323 
17324 EMData* Util::mult_scalar(EMData* img, float scalar)
17325 {
17326         ENTERFUNC;
17327         /* Exception Handle */
17328         if (!img) {
17329                 throw NullPointerException("NULL input image");
17330         }
17331         /* ============  output = scalar*input  ================== */
17332 
17333         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17334         size_t size = (size_t)nx*ny*nz;
17335         EMData * img2 = img->copy_head();
17336         float *img_ptr  =img->get_data();
17337         float *img2_ptr = img2->get_data();
17338         for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17339         img2->update();
17340 
17341         if(img->is_complex()) {
17342                 img2->set_complex(true);
17343                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17344         }
17345         EXITFUNC;
17346         return img2;
17347 }
17348 
17349 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17350 {
17351         ENTERFUNC;
17352         /* Exception Handle */
17353         if (!img) {
17354                 throw NullPointerException("NULL input image");
17355         }
17356         /* ==============   output = img + scalar*img1   ================ */
17357 
17358         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17359         size_t size = (size_t)nx*ny*nz;
17360         EMData * img2 = img->copy_head();
17361         float *img_ptr  =img->get_data();
17362         float *img2_ptr = img2->get_data();
17363         float *img1_ptr = img1->get_data();
17364         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17365         img2->update();
17366         if(img->is_complex()) {
17367                 img2->set_complex(true);
17368                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17369         }
17370 
17371         EXITFUNC;
17372         return img2;
17373 }
17374 
17375 EMData* Util::addn_img(EMData* img, EMData* img1)
17376 {
17377         ENTERFUNC;
17378         /* Exception Handle */
17379         if (!img) {
17380                 throw NullPointerException("NULL input image");
17381         }
17382         /* ==============   output = img + img1   ================ */
17383 
17384         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17385         size_t size = (size_t)nx*ny*nz;
17386         EMData * img2 = img->copy_head();
17387         float *img_ptr  =img->get_data();
17388         float *img2_ptr = img2->get_data();
17389         float *img1_ptr = img1->get_data();
17390         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17391         img2->update();
17392         if(img->is_complex()) {
17393                 img2->set_complex(true);
17394                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17395         }
17396 
17397         EXITFUNC;
17398         return img2;
17399 }
17400 
17401 EMData* Util::subn_img(EMData* img, EMData* img1)
17402 {
17403         ENTERFUNC;
17404         /* Exception Handle */
17405         if (!img) {
17406                 throw NullPointerException("NULL input image");
17407         }
17408         /* ==============   output = img - img1   ================ */
17409 
17410         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17411         size_t size = (size_t)nx*ny*nz;
17412         EMData * img2 = img->copy_head();
17413         float *img_ptr  =img->get_data();
17414         float *img2_ptr = img2->get_data();
17415         float *img1_ptr = img1->get_data();
17416         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17417         img2->update();
17418         if(img->is_complex()) {
17419                 img2->set_complex(true);
17420                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17421         }
17422 
17423         EXITFUNC;
17424         return img2;
17425 }
17426 
17427 EMData* Util::muln_img(EMData* img, EMData* img1)
17428 {
17429         ENTERFUNC;
17430         /* Exception Handle */
17431         if (!img) {
17432                 throw NullPointerException("NULL input image");
17433         }
17434         /* ==============   output = img * img1   ================ */
17435 
17436         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17437         size_t size = (size_t)nx*ny*nz;
17438         EMData * img2 = img->copy_head();
17439         float *img_ptr  =img->get_data();
17440         float *img2_ptr = img2->get_data();
17441         float *img1_ptr = img1->get_data();
17442         if(img->is_complex()) {
17443                 for (size_t i=0; i<size; i+=2) {
17444                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17445                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17446                 }
17447                 img2->set_complex(true);
17448                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17449         } else {
17450                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17451                 img2->update();
17452         }
17453 
17454         EXITFUNC;
17455         return img2;
17456 }
17457 
17458 EMData* Util::divn_img(EMData* img, EMData* img1)
17459 {
17460         ENTERFUNC;
17461         /* Exception Handle */
17462         if (!img) {
17463                 throw NullPointerException("NULL input image");
17464         }
17465         /* ==============   output = img / img1   ================ */
17466 
17467         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17468         size_t size = (size_t)nx*ny*nz;
17469         EMData * img2 = img->copy_head();
17470         float *img_ptr  =img->get_data();
17471         float *img2_ptr = img2->get_data();
17472         float *img1_ptr = img1->get_data();
17473         if(img->is_complex()) {
17474                 float  sq2;
17475                 for (size_t i=0; i<size; i+=2) {
17476                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17477                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17478                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17479                 }
17480                 img2->set_complex(true);
17481                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17482         } else {
17483                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17484                 img2->update();
17485         }
17486 
17487         EXITFUNC;
17488         return img2;
17489 }
17490 
17491 EMData* Util::divn_filter(EMData* img, EMData* img1)
17492 {
17493         ENTERFUNC;
17494         /* Exception Handle */
17495         if (!img) {
17496                 throw NullPointerException("NULL input image");
17497         }
17498         /* ========= img /= img1 ===================== */
17499 
17500         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17501         size_t size = (size_t)nx*ny*nz;
17502         EMData * img2 = img->copy_head();
17503         float *img_ptr  =img->get_data();
17504         float *img1_ptr = img1->get_data();
17505         float *img2_ptr = img2->get_data();
17506         if(img->is_complex()) {
17507                 for (size_t i=0; i<size; i+=2) {
17508                         if(img1_ptr[i] > 1.e-10f) {
17509                         img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17510                         img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17511                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17512                 }
17513         } else  throw ImageFormatException("Only Fourier image allowed");
17514 
17515         img->update();
17516 
17517         EXITFUNC;
17518         return img2;
17519 }
17520 
17521 void Util::mul_scalar(EMData* img, float scalar)
17522 {
17523         ENTERFUNC;
17524         /* Exception Handle */
17525         if (!img) {
17526                 throw NullPointerException("NULL input image");
17527         }
17528         /* ============  output = scalar*input  ================== */
17529 
17530         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17531         size_t size = (size_t)nx*ny*nz;
17532         float *img_ptr  =img->get_data();
17533         for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17534         img->update();
17535 
17536         EXITFUNC;
17537 }
17538 
17539 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17540 {
17541         ENTERFUNC;
17542         /* Exception Handle */
17543         if (!img) {
17544                 throw NullPointerException("NULL input image");
17545         }
17546         /* ==============   img += scalar*img1   ================ */
17547 
17548         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17549         size_t size = (size_t)nx*ny*nz;
17550         float *img_ptr  =img->get_data();
17551         float *img1_ptr = img1->get_data();
17552         for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17553         img1->update();
17554 
17555         EXITFUNC;
17556 }
17557 
17558 void Util::add_img(EMData* img, EMData* img1)
17559 {
17560         ENTERFUNC;
17561         /* Exception Handle */
17562         if (!img || !img1) {
17563                 throw NullPointerException("NULL input image");
17564         }
17565         /* ========= img += img1 ===================== */
17566 
17567         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17568         size_t size = (size_t)nx*ny*nz;
17569         float *img_ptr  = img->get_data();
17570         float *img1_ptr = img1->get_data();
17571         for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17572         img->update();
17573 
17574         EXITFUNC;
17575 }
17576 
17577 void Util::add_img_abs(EMData* img, EMData* img1)
17578 {
17579         ENTERFUNC;
17580         /* Exception Handle */
17581         if (!img) {
17582                 throw NullPointerException("NULL input image");
17583         }
17584         /* ========= img += img1 ===================== */
17585 
17586         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17587         size_t size = (size_t)nx*ny*nz;
17588         float *img_ptr  = img->get_data();
17589         float *img1_ptr = img1->get_data();
17590         for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17591         img->update();
17592 
17593         EXITFUNC;
17594 }
17595 
17596 void Util::add_img2(EMData* img, EMData* img1)
17597 {
17598         ENTERFUNC;
17599         /* Exception Handle */
17600         if (!img) {
17601                 throw NullPointerException("NULL input image");
17602         }
17603         /* ========= img += img1**2 ===================== */
17604 
17605         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17606         size_t size = (size_t)nx*ny*nz;
17607         float *img_ptr  = img->get_data();
17608         float *img1_ptr = img1->get_data();
17609         if(img->is_complex()) {
17610                 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] ;
17611         } else {
17612                 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17613         }
17614         img->update();
17615 
17616         EXITFUNC;
17617 }
17618 
17619 void Util::sub_img(EMData* img, EMData* img1)
17620 {
17621         ENTERFUNC;
17622         /* Exception Handle */
17623         if (!img) {
17624                 throw NullPointerException("NULL input image");
17625         }
17626         /* ========= img -= img1 ===================== */
17627 
17628         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17629         size_t size = (size_t)nx*ny*nz;
17630         float *img_ptr  = img->get_data();
17631         float *img1_ptr = img1->get_data();
17632         for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17633         img->update();
17634 
17635         EXITFUNC;
17636 }
17637 
17638 void Util::mul_img(EMData* img, EMData* img1)
17639 {
17640         ENTERFUNC;
17641         /* Exception Handle */
17642         if (!img) {
17643                 throw NullPointerException("NULL input image");
17644         }
17645         /* ========= img *= img1 ===================== */
17646 
17647         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17648         size_t size = (size_t)nx*ny*nz;
17649         float *img_ptr  = img->get_data();
17650         float *img1_ptr = img1->get_data();
17651         if(img->is_complex()) {
17652                 for (size_t i=0; i<size; i+=2) {
17653                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17654                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17655                         img_ptr[i]   = tmp;
17656 
17657                 }
17658         } else {
17659                 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17660         }
17661         img->update();
17662 
17663         EXITFUNC;
17664 }
17665 
17666 void Util::div_img(EMData* img, EMData* img1)
17667 {
17668         ENTERFUNC;
17669         /* Exception Handle */
17670         if (!img) {
17671                 throw NullPointerException("NULL input image");
17672         }
17673         /* ========= img /= img1 ===================== */
17674 
17675         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17676         size_t size = (size_t)nx*ny*nz;
17677         float *img_ptr  = img->get_data();
17678         float *img1_ptr = img1->get_data();
17679         if(img->is_complex()) {
17680                 float  sq2;
17681                 for (size_t i=0; i<size; i+=2) {
17682                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17683                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17684                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17685                         img_ptr[i]   = tmp;
17686                 }
17687         } else {
17688                 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17689         }
17690         img->update();
17691 
17692         EXITFUNC;
17693 }
17694 
17695 void Util::div_filter(EMData* img, EMData* img1)
17696 {
17697         ENTERFUNC;
17698         /* Exception Handle */
17699         if (!img) {
17700                 throw NullPointerException("NULL input image");
17701         }
17702         /* ========= img /= img1 ===================== */
17703 
17704         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17705         size_t size = (size_t)nx*ny*nz;
17706         float *img_ptr  = img->get_data();
17707         float *img1_ptr = img1->get_data();
17708         if(img->is_complex()) {
17709                 for (size_t i=0; i<size; i+=2) {
17710                         if(img1_ptr[i] > 1.e-10f) {
17711                         img_ptr[i]   /= img1_ptr[i];
17712                         img_ptr[i+1] /= img1_ptr[i];
17713                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17714                 }
17715         } else throw ImageFormatException("Only Fourier image allowed");
17716 
17717         img->update();
17718 
17719         EXITFUNC;
17720 }
17721 
17722 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17723 
17724 EMData* Util::pack_complex_to_real(EMData* img)
17725 {
17726         ENTERFUNC;
17727         /* Exception Handle */
17728         if (!img) {
17729                 throw NullPointerException("NULL input image");
17730         }
17731         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17732                               output is img packed into real image with Friedel part added,   ================ */
17733 
17734         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17735         int nx = nxo - 2 + img->is_fftodd();
17736         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17737         int nyt, nzt;
17738         int nx2 = nx/2;
17739         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17740         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17741         int nx2p = nx2+nx%2;
17742         int ny2p = ny2+ny%2;
17743         int nz2p = nz2+nz%2;
17744         EMData& power = *(new EMData()); // output image
17745         power.set_size(nx, ny, nz);
17746         power.set_array_offsets(-nx2,-ny2,-nz2);
17747         //img->set_array_offsets(1,1,1);
17748         float *img_ptr  = img->get_data();
17749         for (int iz = 1; iz <= nz; iz++) {
17750                 int jz=iz-1;
17751                 if(jz>=nz2p) jz=jz-nzt;
17752                 for (int iy = 1; iy <= ny; iy++) {
17753                         int jy=iy-1;
17754                         if(jy>=ny2p) jy=jy-nyt;
17755                         for (int ix = 1; ix <= lsd2; ix++) {
17756                                 int jx=ix-1;
17757                                 if(jx>=nx2p) jx=jx-nx;
17758                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17759                         }
17760                 }
17761         }
17762 //  Create the Friedel related half
17763         int  nzb, nze, nyb, nye, nxb, nxe;
17764         nxb =-nx2+(nx+1)%2;
17765         nxe = nx2-(nx+1)%2;
17766         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17767         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17768         for (int iz = nzb; iz <= nze; iz++) {
17769                 for (int iy = nyb; iy <= nye; iy++) {
17770                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17771                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17772                         }
17773                 }
17774         }
17775         if(ny2 != 0)  {
17776                 if(nz2 != 0)  {
17777                         if(nz%2 == 0) {  //if nz even, fix the first slice
17778                                 for (int iy = nyb; iy <= nye; iy++) {
17779                                         for (int ix = nxb; ix <= -1; ix++) {
17780                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17781                                         }
17782                                 }
17783                                 if(ny%2 == 0) {  //if ny even, fix the first line
17784                                         for (int ix = nxb; ix <= -1; ix++) {
17785                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17786                                         }
17787                                 }
17788                         }
17789                 }
17790                 if(ny%2 == 0) {  //if ny even, fix the first column
17791                         for (int iz = nzb; iz <= nze; iz++) {
17792                                 for (int ix = nxb; ix <= -1; ix++) {
17793                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17794                                 }
17795                         }
17796                 }
17797 
17798         }
17799         power.update();
17800         power.set_array_offsets(0,0,0);
17801         return &power;
17802 }
17803 #undef  img_ptr
17804 
17805 float Util::ang_n(float peakp, string mode, int maxrin)
17806 {
17807     if (mode == "f" || mode == "F")
17808         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17809     else
17810         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17811 }
17812 
17813 
17814 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17815 {
17816     float* data = ring->get_data();
17817     float av=0.0;
17818     float sq=0.0;
17819     float nn=0.0;
17820     int nring = numr.size()/3;
17821     for( int i=0; i < nring; ++i )
17822     {
17823         int numr3i = numr[3*i+2];
17824         int numr2i = numr[3*i+1]-1;
17825         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17826         for( int j=0; j < numr3i; ++j )
17827         {
17828             int jc = numr2i+j;
17829             av += data[jc] * w;
17830             sq += data[jc] * data[jc] * w;
17831             nn += w;
17832         }
17833     }
17834 
17835     float avg = av/nn;
17836     float sgm = sqrt( (sq-av*av/nn)/nn );
17837     size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17838     for( size_t i=0; i < n; ++i )
17839     {
17840         data[i] -= avg;
17841         data[i] /= sgm;
17842     }
17843 
17844     ring->update();
17845 }
17846 
17847 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17848                 float xrng, float yrng, float step, string mode,
17849                 vector<int>numr, float cnx, float cny) {
17850 
17851     // Manually extract.
17852 /*    vector< EMAN::EMData* > crefim;
17853     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17854     crefim.reserve(crefim_len);
17855 
17856     for(std::size_t i=0;i<crefim_len;i++) {
17857         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17858         crefim.push_back(proxy());
17859     }
17860 */
17861 
17862         size_t crefim_len = crefim.size();
17863 
17864         int   ky = int(2*yrng/step+0.5)/2;
17865         int   kx = int(2*xrng/step+0.5)/2;
17866         int   iref, nref=0, mirror=0;
17867         float iy, ix, sx=0, sy=0;
17868         float peak = -1.0E23f;
17869         float ang=0.0f;
17870         for (int i = -ky; i <= ky; i++) {
17871                 iy = i * step ;
17872                 for (int j = -kx; j <= kx; j++) {
17873                         ix = j*step ;
17874                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17875 
17876                         Normalize_ring( cimage, numr );
17877 
17878                         Frngs(cimage, numr);
17879                         //  compare with all reference images
17880                         // for iref in xrange(len(crefim)):
17881                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17882                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17883                                 double qn = retvals["qn"];
17884                                 double qm = retvals["qm"];
17885                                 if(qn >= peak || qm >= peak) {
17886                                         sx = -ix;
17887                                         sy = -iy;
17888                                         nref = iref;
17889                                         if (qn >= qm) {
17890                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17891                                                 peak = static_cast<float>(qn);
17892                                                 mirror = 0;
17893                                         } else {
17894                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17895                                                 peak = static_cast<float>(qm);
17896                                                 mirror = 1;
17897                                         }
17898                                 }
17899                         }  delete cimage; cimage = 0;
17900                 }
17901         }
17902         float co, so, sxs, sys;
17903         co = static_cast<float>( cos(ang*pi/180.0) );
17904         so = static_cast<float>( -sin(ang*pi/180.0) );
17905         sxs = sx*co - sy*so;
17906         sys = sx*so + sy*co;
17907         vector<float> res;
17908         res.push_back(ang);
17909         res.push_back(sxs);
17910         res.push_back(sys);
17911         res.push_back(static_cast<float>(mirror));
17912         res.push_back(static_cast<float>(nref));
17913         res.push_back(peak);
17914         return res;
17915 }
17916 
17917 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17918                 float xrng, float yrng, float step, string mode,
17919                 vector<int>numr, float cnx, float cny) {
17920 
17921         size_t crefim_len = crefim.size();
17922 
17923         int   ky = int(2*yrng/step+0.5)/2;
17924         int   kx = int(2*xrng/step+0.5)/2;
17925         float iy, ix;
17926         vector<float> peak(crefim_len*5, -1.0e23f);
17927         for (int i = -ky; i <= ky; i++) {
17928                 iy = i * step ;
17929                 for (int j = -kx; j <= kx; j++) {
17930                         ix = j*step ;
17931                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17932                         Normalize_ring( cimage, numr );
17933                         Frngs(cimage, numr);
17934                         for (int iref = 0; iref < (int)crefim_len; iref++) {
17935                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17936                                 double qn = retvals["qn"];
17937                                 double qm = retvals["qm"];
17938                                 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17939                                         if (qn >= qm) {
17940                                                 peak[iref*5] = static_cast<float>(qn);
17941                                                 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17942                                                 peak[iref*5+2] = -ix;
17943                                                 peak[iref*5+3] = -iy;
17944                                                 peak[iref*5+4] = 0;
17945                                         } else {
17946                                                 peak[iref*5] = static_cast<float>(qm);
17947                                                 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17948                                                 peak[iref*5+2] = -ix;
17949                                                 peak[iref*5+3] = -iy;
17950                                                 peak[iref*5+4] = 1;
17951                                         }
17952                                 }
17953                         }  delete cimage; cimage = 0;
17954                 }
17955         }
17956         for (int iref = 0; iref < (int)crefim_len; iref++) {
17957                 float ang = peak[iref*5+1];
17958                 float sx = peak[iref*5+2];
17959                 float sy = peak[iref*5+3];
17960                 float co =  cos(ang*pi/180.0);
17961                 float so = -sin(ang*pi/180.0);
17962                 float sxs = sx*co - sy*so;
17963                 float sys = sx*so + sy*co;
17964                 peak[iref*5+2] = sxs;
17965                 peak[iref*5+3] = sys;
17966         }
17967         return peak;
17968 }
17969 
17970 struct peak_table {
17971         float value;
17972         int index;
17973         bool operator<(const peak_table& b) const { return value > b.value; }
17974 };
17975 
17976 vector<int> Util::assign_groups(const vector<float>& d, int nref, int nima) {
17977 
17978         int kt = nref;
17979         unsigned int maxasi = nima/nref;
17980         vector< vector<int> > id_list;
17981         id_list.resize(nref);
17982         int group, ima;
17983 
17984         peak_table* dd = new peak_table[nref*nima];
17985         for (int i=0; i<nref*nima; i++)  {
17986                 dd[i].value = d[i];
17987                 dd[i].index = i;
17988         }
17989         sort(dd, dd+nref*nima);
17990         int begin = 0;
17991 
17992         bool* del_row = new bool[nref];
17993         for (int i=0; i<nref; i++) del_row[i] = false;
17994         bool* del_column = new bool[nima];
17995         for (int i=0; i<nima; i++) del_column[i] = false;
17996         while (kt > 0) {
17997                 bool flag = true;
17998                 while (flag) {
17999                         int l = dd[begin].index;
18000                         group = l/nima;
18001                         ima = l%nima;
18002                         if (del_column[ima] || del_row[group]) begin++;
18003                         else flag = false;
18004                 }
18005 
18006                 id_list[group].push_back(ima);
18007                 if (kt > 1) {
18008                         if (id_list[group].size() < maxasi) group = -1;
18009                         else kt -= 1;
18010                 } else {
18011                         if (id_list[group].size() < maxasi+nima%nref) group = -1;
18012                         else kt -= 1;
18013                 }
18014                 del_column[ima] = true;
18015                 if (group != -1) {
18016                         del_row[group] = true;
18017                 }
18018         }
18019 
18020         vector<int> id_list_1; 
18021         for (int iref=0; iref<nref; iref++)
18022                 for (unsigned int im=0; im<maxasi; im++)
18023                         id_list_1.push_back(id_list[iref][im]);
18024         for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
18025                         id_list_1.push_back(id_list[group][im]);
18026         id_list_1.push_back(group);
18027 
18028         delete[] del_row;
18029         delete[] del_column;
18030         delete[] dd;
18031         return id_list_1;
18032 }
18033 
18034 int Util::nearest_ang(const vector<float>& vecref, float x, float y, float z) {
18035         float best_v = -1.0f;
18036         int best_i = -1.0;
18037         
18038         for (int i=0; i<vecref.size()/3; i++) {
18039                 float v = abs(vecref[i*3]*x+vecref[i*3+1]*y+vecref[i*3+2]*z);
18040                 if (v > best_v) {
18041                         best_v = v;
18042                         best_i = i;
18043                 }
18044         }
18045         return best_i;
18046 }
18047 
18048 vector<int> Util::assign_projangles(const vector<float>& projangles, const vector<float>& refangles) {
18049         int nref = refangles.size()/2;
18050         int nproj = projangles.size()/2;
18051         
18052         vector<float> vecref(nref*3, 0.0f);
18053         vector<int> asg(nproj, 0);
18054         for (int i=0; i<nref; i++)  
18055                 getvec(refangles[i*2], refangles[i*2+1], vecref[i*3], vecref[i*3+1], vecref[i*3+2]); 
18056         for (int i=0; i<nproj; i++) {
18057                 float x, y, z;
18058                 getvec(projangles[i*2], projangles[i*2+1], x, y, z);
18059                 asg[i] = nearest_ang(vecref, x, y, z);
18060         }
18061         
18062         return asg;
18063 }
18064 
18065 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18066                 float xrng, float yrng, float step, string mode,
18067                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18068 
18069     // Manually extract.
18070 /*    vector< EMAN::EMData* > crefim;
18071     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18072     crefim.reserve(crefim_len);
18073 
18074     for(std::size_t i=0;i<crefim_len;i++) {
18075         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18076         crefim.push_back(proxy());
18077     }
18078 */
18079 
18080         size_t crefim_len = crefim.size();
18081 
18082         int   ky = int(2*yrng/step+0.5)/2;
18083         int   kx = int(2*xrng/step+0.5)/2;
18084         int   iref, nref=0, mirror=0;
18085         float iy, ix, sx=0, sy=0;
18086         float peak = -1.0E23f;
18087         float ang=0.0f;
18088         for (int i = -ky; i <= ky; i++) {
18089                 iy = i * step ;
18090                 for (int j = -kx; j <= kx; j++) {
18091                         ix = j*step ;
18092                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18093 
18094                         Normalize_ring( cimage, numr );
18095 
18096                         Frngs(cimage, numr);
18097                         //  compare with all reference images
18098                         // for iref in xrange(len(crefim)):
18099                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18100                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18101                                 double qn = retvals["qn"];
18102                                 double qm = retvals["qm"];
18103                                 if(qn >= peak || qm >= peak) {
18104                                         sx = -ix;
18105                                         sy = -iy;
18106                                         nref = iref;
18107                                         if (qn >= qm) {
18108                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18109                                                 peak = static_cast<float>(qn);
18110                                                 mirror = 0;
18111                                         } else {
18112                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18113                                                 peak = static_cast<float>(qm);
18114                                                 mirror = 1;
18115                                         }
18116                                 }
18117                         }  delete cimage; cimage = 0;
18118                 }
18119         }
18120         float co, so, sxs, sys;
18121         co = static_cast<float>( cos(ang*pi/180.0) );
18122         so = static_cast<float>( -sin(ang*pi/180.0) );
18123         sxs = sx*co - sy*so;
18124         sys = sx*so + sy*co;
18125         vector<float> res;
18126         res.push_back(ang);
18127         res.push_back(sxs);
18128         res.push_back(sys);
18129         res.push_back(static_cast<float>(mirror));
18130         res.push_back(static_cast<float>(nref));
18131         res.push_back(peak);
18132         return res;
18133 }
18134 
18135 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18136                 float xrng, float yrng, float step, string mode,
18137                 vector< int >numr, float cnx, float cny) {
18138 
18139     // Manually extract.
18140 /*    vector< EMAN::EMData* > crefim;
18141     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18142     crefim.reserve(crefim_len);
18143 
18144     for(std::size_t i=0;i<crefim_len;i++) {
18145         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18146         crefim.push_back(proxy());
18147     }
18148 */
18149         size_t crefim_len = crefim.size();
18150 
18151         int   ky = int(2*yrng/step+0.5)/2;
18152         int   kx = int(2*xrng/step+0.5)/2;
18153         int   iref, nref=0;
18154         float iy, ix, sx=0, sy=0;
18155         float peak = -1.0E23f;
18156         float ang=0.0f;
18157         for (int i = -ky; i <= ky; i++) {
18158                 iy = i * step ;
18159                 for (int j = -kx; j <= kx; j++) {
18160                         ix = j*step ;
18161                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18162                         Frngs(cimage, numr);
18163                         //  compare with all reference images
18164                         // for iref in xrange(len(crefim)):
18165                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18166                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18167                                 double qn = retvals["qn"];
18168                                 if(qn >= peak) {
18169                                         sx = -ix;
18170                                         sy = -iy;
18171                                         nref = iref;
18172                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18173                                         peak = static_cast<float>(qn);
18174                                 }
18175                         }  delete cimage; cimage = 0;
18176                 }
18177         }
18178         float co, so, sxs, sys;
18179         co = static_cast<float>( cos(ang*pi/180.0) );
18180         so = static_cast<float>( -sin(ang*pi/180.0) );
18181         sxs = sx*co - sy*so;
18182         sys = sx*so + sy*co;
18183         vector<float> res;
18184         res.push_back(ang);
18185         res.push_back(sxs);
18186         res.push_back(sys);
18187         res.push_back(static_cast<float>(nref));
18188         res.push_back(peak);
18189         return res;
18190 }
18191 
18192 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18193                 float xrng, float yrng, float step, float ant, string mode,
18194                 vector<int>numr, float cnx, float cny) {
18195 
18196     // Manually extract.
18197 /*    vector< EMAN::EMData* > crefim;
18198     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18199     crefim.reserve(crefim_len);
18200 
18201     for(std::size_t i=0;i<crefim_len;i++) {
18202         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18203         crefim.push_back(proxy());
18204     }
18205 */
18206         size_t crefim_len = crefim.size();
18207         const float qv = static_cast<float>( pi/180.0 );
18208 
18209         Transform * t = image->get_attr("xform.projection");
18210         Dict d = t->get_params("spider");
18211         if(t) {delete t; t=0;}
18212         float phi = d["phi"];
18213         float theta = d["theta"];
18214         int   ky = int(2*yrng/step+0.5)/2;
18215         int   kx = int(2*xrng/step+0.5)/2;
18216         int   iref, nref=0, mirror=0;
18217         float iy, ix, sx=0, sy=0;
18218         float peak = -1.0E23f;
18219         float ang=0.0f;
18220         float imn1 = sin(theta*qv)*cos(phi*qv);
18221         float imn2 = sin(theta*qv)*sin(phi*qv);
18222         float imn3 = cos(theta*qv);
18223         vector<float> n1(crefim_len);
18224         vector<float> n2(crefim_len);
18225         vector<float> n3(crefim_len);
18226         for ( iref = 0; iref < (int)crefim_len; iref++) {
18227                         n1[iref] = crefim[iref]->get_attr("n1");
18228                         n2[iref] = crefim[iref]->get_attr("n2");
18229                         n3[iref] = crefim[iref]->get_attr("n3");
18230         }
18231         for (int i = -ky; i <= ky; i++) {
18232             iy = i * step ;
18233             for (int j = -kx; j <= kx; j++) {
18234                 ix = j*step;
18235                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18236 
18237                 Normalize_ring( cimage, numr );
18238 
18239                 Frngs(cimage, numr);
18240                 //  compare with all reference images
18241                 // for iref in xrange(len(crefim)):
18242                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18243                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18244                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18245                                 double qn = retvals["qn"];
18246                                 double qm = retvals["qm"];
18247                                 if(qn >= peak || qm >= peak) {
18248                                         sx = -ix;
18249                                         sy = -iy;
18250                                         nref = iref;
18251                                         if (qn >= qm) {
18252                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18253                                                 peak = static_cast<float>( qn );
18254                                                 mirror = 0;
18255                                         } else {
18256                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18257                                                 peak = static_cast<float>( qm );
18258                                                 mirror = 1;
18259                                         }
18260                                 }
18261                         }
18262                 }  delete cimage; cimage = 0;
18263             }
18264         }
18265         float co, so, sxs, sys;
18266         if(peak == -1.0E23) {
18267                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18268                 nref = -1;
18269         } else {
18270                 co =  cos(ang*qv);
18271                 so = -sin(ang*qv);
18272                 sxs = sx*co - sy*so;
18273                 sys = sx*so + sy*co;
18274         }
18275         vector<float> res;
18276         res.push_back(ang);
18277         res.push_back(sxs);
18278         res.push_back(sys);
18279         res.push_back(static_cast<float>(mirror));
18280         res.push_back(static_cast<float>(nref));
18281         res.push_back(peak);
18282         return res;
18283 }
18284 
18285 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18286                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18287                 vector<int>numr, float cnx, float cny) {
18288 
18289     // Manually extract.
18290 /*    vector< EMAN::EMData* > crefim;
18291     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18292     crefim.reserve(crefim_len);
18293 
18294     for(std::size_t i=0;i<crefim_len;i++) {
18295         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18296         crefim.push_back(proxy());
18297     }
18298 */
18299         size_t crefim_len = crefim.size();
18300         const float qv = static_cast<float>(pi/180.0);
18301 
18302         Transform* t = image->get_attr("xform.projection");
18303         Dict d = t->get_params("spider");
18304         if(t) {delete t; t=0;}
18305         float phi = d["phi"];
18306         float theta = d["theta"];
18307         float psi = d["psi"];
18308         int ky = int(2*yrng/step+0.5)/2;
18309         int kx = int(2*xrng/step+0.5)/2;
18310         int iref, nref = 0, mirror = 0;
18311         float iy, ix, sx = 0, sy = 0;
18312         float peak = -1.0E23f;
18313         float ang = 0.0f;
18314         float imn1 = sin(theta*qv)*cos(phi*qv);
18315         float imn2 = sin(theta*qv)*sin(phi*qv);
18316         float imn3 = cos(theta*qv);
18317         vector<float> n1(crefim_len);
18318         vector<float> n2(crefim_len);
18319         vector<float> n3(crefim_len);
18320         for (iref = 0; iref < (int)crefim_len; iref++) {
18321                         n1[iref] = crefim[iref]->get_attr("n1");
18322                         n2[iref] = crefim[iref]->get_attr("n2");
18323                         n3[iref] = crefim[iref]->get_attr("n3");
18324         }
18325         bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18326         if (!nomirror) {
18327                 phi = fmod(phi+540.0f, 360.0f);
18328                 theta = 180-theta;
18329                 psi = fmod(540.0f-psi, 360.0f);
18330         }
18331         for (int i = -ky; i <= ky; i++) {
18332             iy = i * step ;
18333             for (int j = -kx; j <= kx; j++) {
18334                 ix = j*step;
18335                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18336 
18337                 Normalize_ring(cimage, numr);
18338 
18339                 Frngs(cimage, numr);
18340                 //  compare with all reference images
18341                 // for iref in xrange(len(crefim)):
18342                 for (iref = 0; iref < (int)crefim_len; iref++) {
18343                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18344                                 if (nomirror) {
18345                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0, psi_max);
18346                                         double qn = retvals["qn"];
18347                                         if (qn >= peak) {
18348                                                 sx = -ix;
18349                                                 sy = -iy;
18350                                                 nref = iref;
18351                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18352                                                 peak = static_cast<float>(qn);
18353                                                 mirror = 0;
18354                                         }
18355                                 } else {
18356                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1, psi_max);
18357                                         double qn = retvals["qn"];
18358                                         if (qn >= peak) {
18359                                                 sx = -ix;
18360                                                 sy = -iy;
18361                                                 nref = iref;
18362                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18363                                                 peak = static_cast<float>(qn);
18364                                                 mirror = 1;
18365                                         }
18366                                 }
18367                         }
18368                 }  delete cimage; cimage = 0;
18369             }
18370         }
18371         float co, so, sxs, sys;
18372         if(peak == -1.0E23) {
18373                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18374                 nref = -1;
18375         } else {
18376                 co =  cos(ang*qv);
18377                 so = -sin(ang*qv);
18378                 sxs = sx*co - sy*so;
18379                 sys = sx*so + sy*co;
18380         }
18381         vector<float> res;
18382         res.push_back(ang);
18383         res.push_back(sxs);
18384         res.push_back(sys);
18385         res.push_back(static_cast<float>(mirror));
18386         res.push_back(static_cast<float>(nref));
18387         res.push_back(peak);
18388         return res;
18389 }
18390 
18391 
18392 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18393                 float xrng, float yrng, float step, float psi_max, string mode,
18394                 vector<int>numr, float cnx, float cny, int ynumber) {
18395         
18396         size_t crefim_len = crefim.size();
18397 
18398         int   iref, nref=0, mirror=0;
18399         float iy, ix, sx=0, sy=0;
18400         float peak = -1.0E23f;
18401         float ang=0.0f;
18402         int   kx = int(2*xrng/step+0.5)/2;
18403         //if ynumber==-1, use the old code which process x and y direction equally.
18404         //if ynumber is given, it should be even. We need to check whether it is zero
18405 
18406         int ky;
18407         float stepy;
18408         int kystart;
18409         
18410         if (ynumber == -1){
18411             ky = int(2*yrng/step+0.5)/2;
18412             stepy = step;
18413             kystart = -ky;
18414         }
18415         else if(ynumber == 0){
18416              ky = 0;
18417                  stepy = 0.0f;
18418                  kystart = ky;
18419         }
18420         else {
18421             ky = int(ynumber/2);                
18422                 stepy=2*yrng/ynumber;
18423                 kystart = -ky + 1;    
18424         }
18425         //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18426         for (int i = kystart; i <= ky; i++) {
18427                 iy = i * stepy ;
18428                 for (int j = -kx; j <= kx; j++) {
18429                         ix = j*step ;
18430                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18431 
18432                         Normalize_ring( cimage, numr );
18433 
18434                         Frngs(cimage, numr);
18435                         //  compare with all reference images
18436                         // for iref in xrange(len(crefim)):
18437                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18438                                 Dict retvals_0 = Crosrng_psi(crefim[iref], cimage, numr, 0, psi_max);
18439                                 Dict retvals_180 = Crosrng_psi(crefim[iref], cimage, numr, 180, psi_max);
18440                                 double qn_0 = retvals_0["qn"];
18441                                 double qn_180 = retvals_180["qn"];
18442                                 double qm_0 = retvals_0["qm"];
18443                                 double qm_180 = retvals_180["qm"];
18444                                 double qn;
18445                                 double qm;
18446                                 bool qn_is_zero = false;
18447                                 bool qm_is_zero = false;
18448                                 
18449                                 if (qn_0 >= qn_180){
18450                                         qn = qn_0;
18451                                         qn_is_zero = true;
18452                                 }
18453                                 else{
18454                                         qn = qn_180;
18455                                         qn_is_zero = false; 
18456                                 }
18457                                         
18458                                 if (qm_0 >= qm_180){
18459                                         qm = qm_0;
18460                                         qm_is_zero = true;
18461                                 }
18462                                 else{
18463                                         qm = qm_180;
18464                                         qm_is_zero = false; 
18465                                 }
18466                                         
18467                                 if(qn >= peak || qm >= peak) {
18468                                         sx = -ix;
18469                                         sy = -iy;
18470                                         nref = iref;
18471                                         if (qn >= qm) {
18472                                                 if (qn_is_zero){
18473                                                         ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18474                                                 }
18475                                                 else{
18476                                                         ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18477                                                 }
18478                                                 peak = static_cast<float>(qn);
18479                                                 mirror = 0;
18480                                         } else {
18481                                                 if (qm_is_zero){
18482                                                         ang = ang_n(retvals_0["tmt"], mode, numr[numr.size()-1]);
18483                                                 }
18484                                                 else{
18485                                                         ang = ang_n(retvals_180["tmt"], mode, numr[numr.size()-1]);
18486                                                 }
18487                                                 peak = static_cast<float>(qm);
18488                                                 mirror = 1;
18489                                         }
18490                                 }
18491                         }
18492                         delete cimage; cimage = 0;
18493                 }
18494         }
18495         float co, so, sxs, sys;
18496         co = static_cast<float>( cos(ang*pi/180.0) );
18497         so = static_cast<float>( -sin(ang*pi/180.0) );
18498         sxs = sx*co - sy*so;
18499         sys = sx*so + sy*co;
18500         vector<float> res;
18501         res.push_back(ang);
18502         res.push_back(sxs);
18503         res.push_back(sys);
18504         res.push_back(static_cast<float>(mirror));
18505         res.push_back(static_cast<float>(nref));
18506         res.push_back(peak);
18507         return res;
18508 }
18509 
18510 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18511                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18512                 vector<int>numr, float cnx, float cny, int ynumber, bool mirror_only, float yrnglocal) {
18513         //std::cout<<"multiref_polar_ali_helical_local_jia called"<<std::endl;
18514         size_t crefim_len = crefim.size();
18515 
18516         int   iref, nref=-1, mirror=0;
18517         float iy, ix, sx=0, sy=0;
18518         float peak = -1.0E23f;
18519         float ang=0.0f;
18520         const float qv = static_cast<float>( pi/180.0 );
18521         Transform * t = image->get_attr("xform.projection");
18522         Dict d = t->get_params("spider");
18523         if(t) {delete t; t=0;}
18524         float phi = d["phi"];
18525         float theta = d["theta"];
18526         float psi = d["psi"];
18527         float imn1 = sin(theta*qv)*cos(phi*qv);
18528         float imn2 = sin(theta*qv)*sin(phi*qv);
18529         float imn3 = cos(theta*qv);
18530         vector<float> n1(crefim_len);
18531         vector<float> n2(crefim_len);
18532         vector<float> n3(crefim_len);
18533         for ( iref = 0; iref < (int)crefim_len; iref++) {
18534                         n1[iref] = crefim[iref]->get_attr("n1");
18535                         n2[iref] = crefim[iref]->get_attr("n2");
18536                         n3[iref] = crefim[iref]->get_attr("n3");
18537         }
18538         float nbrinp;
18539         bool use_ref;
18540         int   kx = int(2*xrng/step+0.5)/2;
18541         
18542         //if ynumber==-1, use the old code which process x and y direction equally.
18543         if(ynumber==-1) {
18544                 int   ky = int(2*yrng/step+0.5)/2;
18545                 for (int i = -ky; i <= ky; i++) {
18546                         iy = i * step ;
18547                         for (int j = -kx; j <= kx; j++)  {
18548                                 ix = j*step ;
18549                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18550 
18551                                 Normalize_ring( cimage, numr );
18552 
18553                                 Frngs(cimage, numr);
18554                                 //  compare with all reference images
18555                                 // for iref in xrange(len(crefim)):
18556                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18557                                         
18558                                         use_ref = false;
18559                                         if (!mirror_only){
18560                                                 // inner product of iref's Eulerian angles with that of the data
18561                                                 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18562                                                 if (nbrinp >= ant){
18563                                                         use_ref = true;
18564                                                 }
18565                                         }
18566                                         else if (mirror_only) {
18567                                                 // inner product of the mirror of iref's Eulerian angles with that of the data
18568                                                 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
18569                                                 if (nbrinp >= ant){
18570                                                         use_ref = true;
18571                                                 }
18572                                         }
18573                                         
18574                                         
18575                                         if(use_ref) {
18576                                                 Dict retvals;
18577                                                 if (mirror_only == true){
18578                                                     if ((psi-90) < 90)  
18579                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18580                                                     else
18581                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
18582                                                 }       
18583                                                 else{ 
18584                                                     if ((psi-90) < 90)  
18585                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18586                                                     else
18587                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18588                                                 }   
18589                                                 double qn = retvals["qn"];
18590                                                 
18591                                                 if(qn >= peak) {
18592                                                         sx = -ix;
18593                                                         sy = -iy;
18594                                                         nref = iref;
18595                                                         if (!mirror_only) {
18596                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18597                                                                 peak = static_cast<float>(qn);
18598                                                                 mirror = 0;
18599                                                         } else {
18600                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18601                                                                 peak = static_cast<float>(qn);
18602                                                                 mirror = 1;
18603                                                         }
18604                                                 }
18605                                         }
18606                                 }  
18607                                 delete cimage; cimage = 0;
18608                         }
18609                    }
18610         }
18611         //if ynumber is given, it should be even. We need to check whether it is zero
18612         else if(ynumber==0) {
18613                 sy = 0.0f;
18614                 for (int j = -kx; j <= kx; j++) {
18615                         ix = j*step ;
18616                         EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18617 
18618                         Normalize_ring( cimage, numr );
18619 
18620                         Frngs(cimage, numr);
18621                         //  compare with all reference images
18622                         // for iref in xrange(len(crefim)):
18623                         for ( iref = 0; iref < (int)crefim_len; iref++)  {
18624                                 
18625                                 use_ref = false;
18626                                 if (!mirror_only){
18627                                         // inner product of iref's Eulerian angles with that of the data
18628                                         nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18629                                         if (nbrinp >= ant){
18630                                                 use_ref = true;
18631                                         }
18632                                 }
18633                                 else if (mirror_only) {
18634                                         // inner product of the mirror of iref's Eulerian angles with that of the data
18635                                         nbrinp = (-1.0f*n1[iref]*imn1) + (-1.0f*n2[iref]*imn2) + n3[iref]*imn3;
18636                                         if (nbrinp >= ant){
18637                                                 use_ref = true;
18638                                         }
18639                                 }
18640                                 
18641                                 if(use_ref) {
18642                                                 Dict retvals;
18643                                                 if (mirror_only == true){
18644                                                     if ((psi-90) < 90)  
18645                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18646                                                     else
18647                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
18648                                                 }       
18649                                                 else{ 
18650                                                     if ((psi-90) < 90)  
18651                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18652                                                     else
18653                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18654                                                 }   
18655                                                 double qn = retvals["qn"];
18656                                                 
18657                                                 if(qn >= peak) {
18658                                                         sx = -ix;
18659                                                         sy = -iy;
18660                                                         nref = iref;
18661                                                         if (!mirror_only) {
18662                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18663                                                                 peak = static_cast<float>(qn);
18664                                                                 mirror = 0;
18665                                                         } else {
18666                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18667                                                                 peak = static_cast<float>(qn);
18668                                                                 mirror = 1;
18669                                                         }
18670                                                 }
18671                                 }
18672                         } 
18673                         delete cimage; cimage = 0;
18674                 }                       
18675         } else {
18676                 int   ky = int(ynumber/2);              
18677                 float stepy=2*yrng/ynumber;
18678                 // when yrnglocal is not equal to -1.0, the search range is limited to +/- yrnglocal
18679                 // leave step size the same
18680                 if (yrnglocal >= 0.0){
18681                         ky = int(yrnglocal/stepy);
18682                 }
18683                 
18684                 //std::cout<<"yrnglocal="<<yrnglocal<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18685                 //cout<<"ky stepy: "<<ky<<" "<<stepy<<endl;
18686                 for (int i = -ky+1; i <= ky; i++) {
18687                         iy = i * stepy ;
18688                         for (int j = -kx; j <= kx; j++) {
18689                                 ix = j*step ;
18690                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18691 
18692                                 Normalize_ring( cimage, numr );
18693 
18694                                 Frngs(cimage, numr);
18695                                 //  compare with all reference images
18696                                 // for iref in xrange(len(crefim)):
18697                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18698                                         
18699                                         use_ref = false;
18700                                         if (!mirror_only){
18701                                                 // inner product of iref's Eulerian angles with that of the data
18702                                                 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18703                                                 if (nbrinp >= ant){
18704                                                         use_ref = true;
18705                                                 }
18706                                         }
18707                                         else if (mirror_only) {
18708                                                 // inner product of the mirror of iref's Eulerian angles with that of the data
18709                                                 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
18710                                                 if (nbrinp >= ant){
18711                                                         use_ref = true;
18712                                                 }
18713                                         }
18714                                         if(use_ref) {
18715                                                 Dict retvals;
18716                                                 if (mirror_only == true){
18717                                                     if ((psi-90) < 90)  
18718                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18719                                                     else
18720                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max); 
18721                                                 }       
18722                                                 else{ 
18723                                                     if ((psi-90) < 90)  
18724                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18725                                                     else
18726                                                          retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18727                                                 }   
18728                                                 double qn = retvals["qn"];
18729                                                 
18730                                                 if(qn >= peak) {
18731                                                         sx = -ix;
18732                                                         sy = -iy;
18733                                                         nref = iref;
18734                                                         if (!mirror_only) {
18735                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18736                                                                 peak = static_cast<float>(qn);
18737                                                                 mirror = 0;
18738                                                         } else {
18739                                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18740                                                                 peak = static_cast<float>(qn);
18741                                                                 mirror = 1;
18742                                                         }
18743                                                 }
18744                                         }
18745                                 }
18746                                 delete cimage; cimage = 0;
18747                         }
18748                 }
18749         }
18750         float co, so, sxs, sys;
18751         co = static_cast<float>( cos(ang*pi/180.0) );
18752         so = static_cast<float>( -sin(ang*pi/180.0) );
18753         sxs = sx*co - sy*so;
18754         sys = sx*so + sy*co;
18755         vector<float> res;
18756         res.push_back(ang);
18757         res.push_back(sxs);
18758         res.push_back(sys);
18759         res.push_back(static_cast<float>(mirror));
18760         res.push_back(static_cast<float>(nref));
18761         res.push_back(peak);
18762         return res;
18763 }
18764 
18765 
18766 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
18767                 float xrng, float yrng, float step, float psi_max, string mode,
18768                 vector<int>numr, float cnx, float cny, int ynumber) {
18769 
18770         size_t crefim_len = crefim.size();
18771 
18772         int   iref, nref=0, mirror=0;
18773         float iy, ix, sx=0, sy=0;
18774         float peak = -1.0E23f;
18775         float ang=0.0f;
18776         int   kx = int(2*xrng/step+0.5)/2;
18777         //if ynumber==-1, use the old code which process x and y direction equally.
18778         
18779         int ky;
18780         float stepy;
18781         int kystart;
18782         
18783         if (ynumber == -1){
18784             ky = int(2*yrng/step+0.5)/2;
18785             stepy = step;
18786             kystart = -ky;
18787         }
18788         else if(ynumber == 0){
18789              ky = 0;
18790                  stepy = 0.0f;
18791                  kystart = ky;
18792         }
18793         else {
18794             ky = int(ynumber/2);                
18795                 stepy=2*yrng/ynumber;
18796                 kystart = -ky + 1;    
18797         }
18798         
18799                 //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18800         for (int i = kystart; i <= ky; i++) {
18801                 iy = i * stepy ;
18802                 for (int j = -kx; j <= kx; j++) {
18803                         ix = j*step ;
18804                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18805 
18806                         Normalize_ring( cimage, numr );
18807 
18808                         Frngs(cimage, numr);
18809                         //  compare with all reference images
18810                         // for iref in xrange(len(crefim)):
18811                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18812                                 Dict retvals_0 = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18813                                 Dict retvals_180 = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18814                                 double qn_0 = retvals_0["qn"];
18815                                 double qn_180 = retvals_180["qn"];
18816                                 double qn;
18817                                 bool qn_is_zero = false;
18818                                 
18819                                 if (qn_0 >= qn_180){
18820                                         qn = qn_0;
18821                                         qn_is_zero = true;
18822                                 }
18823                                 else{
18824                                         qn = qn_180;
18825                                         qn_is_zero = false; 
18826                                 }
18827                                         
18828                                 if(qn >= peak) {
18829                                         sx = -ix;
18830                                         sy = -iy;
18831                                         nref = iref;
18832                                         
18833                                         if (qn_is_zero){
18834                                                 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18835                                         }
18836                                         else{
18837                                                 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18838                                         }
18839                                         peak = static_cast<float>(qn);
18840                                         mirror = 0;
18841                                          
18842                                 }
18843                         }
18844                         delete cimage; cimage = 0;
18845                 }
18846         }       
18847         float co, so, sxs, sys;
18848         co = static_cast<float>( cos(ang*pi/180.0) );
18849         so = static_cast<float>( -sin(ang*pi/180.0) );
18850         sxs = sx*co - sy*so;
18851         sys = sx*so + sy*co;
18852         vector<float> res;
18853         res.push_back(ang);
18854         res.push_back(sxs);
18855         res.push_back(sys);
18856         res.push_back(static_cast<float>(mirror));
18857         res.push_back(static_cast<float>(nref));
18858         res.push_back(peak);
18859         return res;
18860 }
18861 
18862 
18863 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
18864                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18865                 vector<int>numr, float cnx, float cny, int ynumber) {
18866 
18867         size_t crefim_len = crefim.size();
18868         const float qv = static_cast<float>( pi/180.0 );
18869         Transform * t = image->get_attr("xform.projection");
18870         Dict d = t->get_params("spider");
18871         if(t) {delete t; t=0;}
18872         float phi = d["phi"];
18873         float theta = d["theta"];
18874         float imn1 = sin(theta*qv)*cos(phi*qv);
18875         float imn2 = sin(theta*qv)*sin(phi*qv);
18876         float imn3 = cos(theta*qv);
18877         vector<float> n1(crefim_len);
18878         vector<float> n2(crefim_len);
18879         vector<float> n3(crefim_len);
18880         int   iref, nref=-1, mirror=0;
18881         float iy, ix, sx=0, sy=0;
18882         float peak = -1.0E23f;
18883         float ang=0.0f;
18884         int   kx = int(2*xrng/step+0.5)/2;
18885         
18886         for ( iref = 0; iref < (int)crefim_len; iref++) {
18887                 n1[iref] = crefim[iref]->get_attr("n1");
18888                 n2[iref] = crefim[iref]->get_attr("n2");
18889                 n3[iref] = crefim[iref]->get_attr("n3");
18890         }
18891         
18892         //if ynumber==-1, use the old code which process x and y direction equally.
18893         if(ynumber==-1) {
18894                 int   ky = int(2*yrng/step+0.5)/2;
18895                 for (int i = -ky; i <= ky; i++) {
18896                         iy = i * step ;
18897                         for (int j = -kx; j <= kx; j++)  {
18898                                 ix = j*step ;
18899                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18900 
18901                                 Normalize_ring( cimage, numr );
18902 
18903                                 Frngs(cimage, numr);
18904                                 //  compare with all reference images
18905                                 // for iref in xrange(len(crefim)):
18906                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18907                                         if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18908                                                 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18909                                                 double qn = retvals["qn"];
18910                                                 if( qn >= peak) {
18911                                                         sx = -ix;
18912                                                         sy = -iy;
18913                                                         nref = iref;
18914                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18915                                                         peak = static_cast<float>(qn);
18916                                                         mirror = 0;
18917                                                 }
18918                                         }
18919                                 }  
18920                                 delete cimage; cimage = 0;
18921                         }
18922                    }
18923         }
18924         //if ynumber is given, it should be even. We need to check whether it is zero
18925         else if(ynumber==0) {
18926                 sy = 0.0f;
18927                 for (int j = -kx; j <= kx; j++) {
18928                         ix = j*step ;
18929                         iy = 0.0f ;
18930                         EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18931 
18932                         Normalize_ring( cimage, numr );
18933 
18934                         Frngs(cimage, numr);
18935                         //  compare with all reference images
18936                         // for iref in xrange(len(crefim)):
18937                         for ( iref = 0; iref < (int)crefim_len; iref++)  {
18938                                 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18939                                         Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18940                                         double qn = retvals["qn"];
18941                                         if( qn >= peak ) {
18942                                                 sx = -ix;
18943                                                 nref = iref;
18944                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18945                                                 peak = static_cast<float>(qn);
18946                                                 mirror = 0;
18947                                         }
18948                                 }
18949                         } 
18950                         delete cimage; cimage = 0;
18951                 }                       
18952         } else {
18953                 int   ky = int(ynumber/2);              
18954                 float stepy=2*yrng/ynumber;
18955                 //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18956                 for (int i = -ky+1; i <= ky; i++) {
18957                         iy = i * stepy ;
18958                         for (int j = -kx; j <= kx; j++) {
18959                                 ix = j*step ;
18960                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18961 
18962                                 Normalize_ring( cimage, numr );
18963 
18964                                 Frngs(cimage, numr);
18965                                 //  compare with all reference images
18966                                 // for iref in xrange(len(crefim)):
18967                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18968                                         if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18969                                                 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18970                                                 double qn = retvals["qn"];
18971                                                 if( qn >= peak) {
18972                                                         sx = -ix;
18973                                                         sy = -iy;
18974                                                         nref = iref;
18975                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18976                                                         peak = static_cast<float>(qn);
18977                                                         mirror = 0;
18978                                                 }
18979                                         }
18980                                 }
18981                                 delete cimage; cimage = 0;
18982                         }
18983                 }
18984         }
18985         float co, so, sxs, sys;
18986         co = static_cast<float>( cos(ang*pi/180.0) );
18987         so = static_cast<float>( -sin(ang*pi/180.0) );
18988         sxs = sx*co - sy*so;
18989         sys = sx*so + sy*co;
18990         vector<float> res;
18991         res.push_back(ang);
18992         res.push_back(sxs);
18993         res.push_back(sys);
18994         res.push_back(static_cast<float>(mirror));
18995         res.push_back(static_cast<float>(nref));
18996         res.push_back(peak);
18997         return res;
18998 }
18999 
19000 
19001 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
19002                         float xrng, float yrng, float step, string mode,
19003                         vector< int >numr, float cnx, float cny,
19004                         EMData *peaks, EMData *peakm) {
19005 
19006         int   maxrin = numr[numr.size()-1];
19007 
19008         int   ky = int(2*yrng/step+0.5)/2;
19009         int   kx = int(2*xrng/step+0.5)/2;
19010 
19011         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19012         float *p_ccf1ds = peaks->get_data();
19013 
19014         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19015         float *p_ccf1dm = peakm->get_data();
19016 
19017         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19018                 p_ccf1ds[i] = -1.e20f;
19019                 p_ccf1dm[i] = -1.e20f;
19020         }
19021 
19022         for (int i = -ky; i <= ky; i++) {
19023                 float iy = i * step;
19024                 for (int j = -kx; j <= kx; j++) {
19025                         float ix = j*step;
19026                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19027                         Frngs(cimage, numr);
19028                         Crosrng_msg_vec(crefim, cimage, numr,
19029                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19030                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19031                         delete cimage; cimage = 0;
19032                 }
19033         }
19034         return;
19035 }
19036 
19037 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19038      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19039      EMData *peaks_compress, EMData *peakm_compress) {
19040 
19041         int   maxrin = numr[numr.size()-1];
19042 
19043         int   ky = int(2*yrng/step+0.5)/2;
19044         int   kx = int(2*xrng/step+0.5)/2;
19045 
19046         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19047         float *p_ccf1ds = peaks->get_data();
19048 
19049         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19050         float *p_ccf1dm = peakm->get_data();
19051 
19052         peaks_compress->set_size(maxrin, 1, 1);
19053         float *p_ccf1ds_compress = peaks_compress->get_data();
19054 
19055         peakm_compress->set_size(maxrin, 1, 1);
19056         float *p_ccf1dm_compress = peakm_compress->get_data();
19057 
19058         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19059                 p_ccf1ds[i] = -1.e20f;
19060                 p_ccf1dm[i] = -1.e20f;
19061         }
19062 
19063         for (int i = -ky; i <= ky; i++) {
19064                 float iy = i * step;
19065                 for (int j = -kx; j <= kx; j++) {
19066                         float ix = j*step;
19067                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19068                         Frngs(cimage, numr);
19069                         Crosrng_msg_vec(crefim, cimage, numr,
19070                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19071                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19072                         delete cimage; cimage = 0;
19073                 }
19074         }
19075         for (int x=0; x<maxrin; x++) {
19076                 float maxs = -1.0e22f;
19077                 float maxm = -1.0e22f;
19078                 for (int i=1; i<=2*ky+1; i++) {
19079                         for (int j=1; j<=2*kx+1; j++) {
19080                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19081                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19082                         }
19083                 }
19084                 p_ccf1ds_compress[x] = maxs;
19085                 p_ccf1dm_compress[x] = maxm;
19086         }
19087         return;
19088 }
19089 
19090 struct ccf_point
19091 {
19092     float value;
19093     int i;
19094     int j;
19095     int k;
19096     int mirror;
19097 };
19098 
19099 
19100 struct ccf_value
19101 {
19102     bool operator()( const ccf_point& a, const ccf_point& b )
19103     {
19104         return a.value > b.value;
19105     }
19106 };
19107 
19108 
19109 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19110                         float xrng, float yrng, float step, string mode,
19111                         vector< int >numr, float cnx, float cny, double T) {
19112 
19113         int   maxrin = numr[numr.size()-1];
19114 
19115         int   ky = int(2*yrng/step+0.5)/2;
19116         int   kx = int(2*xrng/step+0.5)/2;
19117 
19118         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19119         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19120         int vol = maxrin*(2*kx+1)*(2*ky+1);
19121         vector<ccf_point> ccf(2*vol);
19122         ccf_point temp;
19123 
19124         int index = 0;
19125         for (int i = -ky; i <= ky; i++) {
19126                 float iy = i * step;
19127                 for (int j = -kx; j <= kx; j++) {
19128                         float ix = j*step;
19129                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19130                         Frngs(cimage, numr);
19131                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19132                         for (int k=0; k<maxrin; k++) {
19133                                 temp.value = p_ccf1ds[k];
19134                                 temp.i = k;
19135                                 temp.j = j;
19136                                 temp.k = i;
19137                                 temp.mirror = 0;
19138                                 ccf[index] = temp;
19139                                 index++;
19140                                 temp.value = p_ccf1dm[k];
19141                                 temp.mirror = 1;
19142                                 ccf[index] = temp;
19143                                 index++;
19144                         }
19145                         delete cimage; cimage = 0;
19146                 }
19147         }
19148 
19149         delete p_ccf1ds;
19150         delete p_ccf1dm;
19151         std::sort(ccf.begin(), ccf.end(), ccf_value());
19152 
19153         double qt = (double)ccf[0].value;
19154         vector <double> p(2*vol), cp(2*vol);
19155 
19156         double sump = 0.0;
19157         for (int i=0; i<2*vol; i++) {
19158                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19159                 sump += p[i];
19160         }
19161         for (int i=0; i<2*vol; i++) {
19162                 p[i] /= sump;
19163         }
19164         for (int i=1; i<2*vol; i++) {
19165                 p[i] += p[i-1];
19166         }
19167         p[2*vol-1] = 2.0;
19168 
19169         float t = get_frand(0.0f, 1.0f);
19170         int select = 0;
19171         while (p[select] < t)   select += 1;
19172 
19173         vector<float> a(6);
19174         a[0] = ccf[select].value;
19175         a[1] = (float)ccf[select].i;
19176         a[2] = (float)ccf[select].j;
19177         a[3] = (float)ccf[select].k;
19178         a[4] = (float)ccf[select].mirror;
19179         a[5] = (float)select;
19180         return a;
19181 }
19182 
19183 
19184 /*
19185 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
19186                         float xrng, float yrng, float step, string mode,
19187                         vector< int >numr, float cnx, float cny,
19188                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
19189 
19190 // formerly known as apmq
19191     // Determine shift and rotation between image and many reference
19192     // images (crefim, weights have to be applied) quadratic
19193     // interpolation
19194 
19195 
19196     // Manually extract.
19197 *//*    vector< EMAN::EMData* > crefim;
19198     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
19199     crefim.reserve(crefim_len);
19200 
19201     for(std::size_t i=0;i<crefim_len;i++) {
19202         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
19203         crefim.push_back(proxy());
19204     }
19205 */
19206 /*
19207         int   maxrin = numr[numr.size()-1];
19208 
19209         size_t crefim_len = crefim.size();
19210 
19211         int   iref;
19212         int   ky = int(2*yrng/step+0.5)/2;
19213         int   kx = int(2*xrng/step+0.5)/2;
19214         int   tkx = 2*kx+3;
19215         int   tky = 2*ky+3;
19216 
19217         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
19218         float *p_ccf1ds = peaks->get_data();
19219 
19220 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
19221 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
19222         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
19223         float *p_ccf1dm = peakm->get_data();
19224 
19225         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
19226                 p_ccf1ds[i] = -1.e20f;
19227                 p_ccf1dm[i] = -1.e20f;
19228         }
19229 
19230         float  iy, ix;
19231         for (int i = -ky; i <= ky; i++) {
19232                 iy = i * step ;
19233                 for (int j = -kx; j <= kx; j++) {
19234                         ix = j*step ;
19235                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19236                         Frngs(cimage, numr);
19237                         //  compare with all reference images
19238                         // for iref in xrange(len(crefim)):
19239                         for ( iref = 0; iref < (int)crefim_len; iref++) {
19240                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
19241                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
19242                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
19243                         }
19244                         delete cimage; cimage = 0;
19245                 }
19246         }
19247         return;
19248 }
19249 */
19250 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19251 
19252         EMData *rot;
19253 
19254         const int nmax=3, mmax=3;
19255         char task[60], csave[60];
19256         long int lsave[4];
19257         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19258         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];
19259         long int SIXTY=60;
19260 
19261         //     We wish to have no output.
19262         iprint = -1;
19263 
19264         //c     We specify the tolerances in the stopping criteria.
19265         factr=1.0e1;
19266         pgtol=1.0e-5;
19267 
19268         //     We specify the dimension n of the sample problem and the number
19269         //        m of limited memory corrections stored.  (n and m should not
19270         //        exceed the limits nmax and mmax respectively.)
19271         n=3;
19272         m=3;
19273 
19274         //     We now provide nbd which defines the bounds on the variables:
19275         //                    l   specifies the lower bounds,
19276         //                    u   specifies the upper bounds.
19277         //                    x   specifies the initial guess
19278         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19279         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19280         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19281 
19282 
19283         //     We start the iteration by initializing task.
19284         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19285         strcpy(task,"START");
19286         for (int i=5;i<60;i++)  task[i]=' ';
19287 
19288         //     This is the call to the L-BFGS-B code.
19289         // (* call the L-BFGS-B routine with task='START' once before loop *)
19290         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19291         //int step = 1;
19292 
19293         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19294         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19295 
19296                 if (strncmp(task,"FG",2)==0) {
19297                 //   the minimization routine has returned to request the
19298                 //   function f and gradient g values at the current x
19299 
19300                 //        Compute function value f for the sample problem.
19301                 rot = new EMData();
19302                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19303                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19304                 //f = -f;
19305                 delete rot;
19306 
19307                 //        Compute gradient g for the sample problem.
19308                 float dt = 1.0e-3f;
19309                 rot = new EMData();
19310                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19311                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19312                 //f1 = -f1;
19313                 g[0] = (f1-f)/dt;
19314                 delete rot;
19315 
19316                 dt = 1.0e-2f;
19317                 rot = new EMData();
19318                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19319                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19320                 //f2 = -f2;
19321                 g[1] = (f2-f)/dt;
19322                 delete rot;
19323 
19324                 rot = new EMData();
19325                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19326                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19327                 //f3 = -f3;
19328                 g[2] = (f3-f)/dt;
19329                 delete rot;
19330                 }
19331 
19332                 //c          go back to the minimization routine.
19333                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19334                 //step++;
19335         }
19336 
19337         //printf("Total step is %d\n", step);
19338         vector<float> res;
19339         res.push_back(static_cast<float>(x[0]));
19340         res.push_back(static_cast<float>(x[1]));
19341         res.push_back(static_cast<float>(x[2]));
19342         //res.push_back(step);
19343         return res;
19344 }
19345 
19346 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19347 
19348         EMData *rot;
19349 
19350         const int nmax=3, mmax=3;
19351         char task[60], csave[60];
19352         long int lsave[4];
19353         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19354         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];
19355         long int SIXTY=60;
19356 
19357         //     We wish to have no output.
19358         iprint = -1;
19359 
19360         //c     We specify the tolerances in the stopping criteria.
19361         factr=1.0e1;
19362         pgtol=1.0e-5;
19363 
19364         //     We specify the dimension n of the sample problem and the number
19365         //        m of limited memory corrections stored.  (n and m should not
19366         //        exceed the limits nmax and mmax respectively.)
19367         n=3;
19368         m=3;
19369 
19370         //     We now provide nbd which defines the bounds on the variables:
19371         //                    l   specifies the lower bounds,
19372         //                    u   specifies the upper bounds.
19373         //                    x   specifies the initial guess
19374         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19375         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19376         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19377 
19378 
19379         //     We start the iteration by initializing task.
19380         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19381         strcpy(task,"START");
19382         for (int i=5;i<60;i++)  task[i]=' ';
19383 
19384         //     This is the call to the L-BFGS-B code.
19385         // (* call the L-BFGS-B routine with task='START' once before loop *)
19386         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19387         //int step = 1;
19388 
19389         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19390         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19391 
19392                 if (strncmp(task,"FG",2)==0) {
19393                 //   the minimization routine has returned to request the
19394                 //   function f and gradient g values at the current x
19395 
19396                 //        Compute function value f for the sample problem.
19397                 rot = new EMData();
19398                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19399                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19400                 //f = -f;
19401                 delete rot;
19402 
19403                 //        Compute gradient g for the sample problem.
19404                 float dt = 1.0e-3f;
19405                 rot = new EMData();
19406                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19407                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19408                 //f1 = -f1;
19409                 g[0] = (f1-f)/dt;
19410                 delete rot;
19411 
19412                 rot = new EMData();
19413                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19414                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19415                 //f2 = -f2;
19416                 g[1] = (f2-f)/dt;
19417                 delete rot;
19418 
19419                 rot = new EMData();
19420                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19421                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19422                 //f3 = -f3;
19423                 g[2] = (f3-f)/dt;
19424                 delete rot;
19425                 }
19426 
19427                 //c          go back to the minimization routine.
19428                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19429                 //step++;
19430         }
19431 
19432         //printf("Total step is %d\n", step);
19433         vector<float> res;
19434         res.push_back(static_cast<float>(x[0]));
19435         res.push_back(static_cast<float>(x[1]));
19436         res.push_back(static_cast<float>(x[2]));
19437         //res.push_back(step);
19438         return res;
19439 }
19440 
19441 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) {
19442 
19443         EMData *proj, *proj2;
19444 
19445         const int nmax=5, mmax=5;
19446         char task[60], csave[60];
19447         long int lsave[4];
19448         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19449         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];
19450         long int SIXTY=60;
19451 
19452         //     We wish to have no output.
19453         iprint = -1;
19454 
19455         //c     We specify the tolerances in the stopping criteria.
19456         factr=1.0e1;
19457         pgtol=1.0e-5;
19458 
19459         //     We specify the dimension n of the sample problem and the number
19460         //        m of limited memory corrections stored.  (n and m should not
19461         //        exceed the limits nmax and mmax respectively.)
19462         n=5;
19463         m=5;
19464 
19465         //     We now provide nbd which defines the bounds on the variables:
19466         //                    l   specifies the lower bounds,
19467         //                    u   specifies the upper bounds.
19468         //                    x   specifies the initial guess
19469         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
19470         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
19471         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
19472         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
19473         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
19474 
19475 
19476         //     We start the iteration by initializing task.
19477         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19478         strcpy(task,"START");
19479         for (int i=5;i<60;i++)  task[i]=' ';
19480 
19481         //     This is the call to the L-BFGS-B code.
19482         // (* call the L-BFGS-B routine with task='START' once before loop *)
19483         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19484         int step = 1;
19485 
19486         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19487         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19488 
19489                 if (strncmp(task,"FG",2)==0) {
19490                 //   the minimization routine has returned to request the
19491                 //   function f and gradient g values at the current x
19492 
19493                 //        Compute function value f for the sample problem.
19494                 proj = new EMData();
19495                 proj2 = new EMData();
19496                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19497                 proj->fft_shuffle();
19498                 proj->center_origin_fft();
19499                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19500                 proj->do_ift_inplace();
19501                 int M = proj->get_ysize()/2;
19502                 proj2 = proj->window_center(M);
19503                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19504                 //f = -f;
19505                 delete proj;
19506                 delete proj2;
19507 
19508                 //        Compute gradient g for the sample problem.
19509                 float dt = 1.0e-3f;
19510                 proj = new EMData();
19511                 proj2 = new EMData();
19512                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19513                 proj->fft_shuffle();
19514                 proj->center_origin_fft();
19515                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19516                 proj->do_ift_inplace();
19517                 proj2 = proj->window_center(M);
19518                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19519                 //ft = -ft;
19520                 delete proj;
19521                 delete proj2;
19522                 g[0] = (ft-f)/dt;
19523 
19524                 proj = new EMData();
19525                 proj2 = new EMData();
19526                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19527                 proj->fft_shuffle();
19528                 proj->center_origin_fft();
19529                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19530                 proj->do_ift_inplace();
19531                 proj2 = proj->window_center(M);
19532                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19533                 //ft = -ft;
19534                 delete proj;
19535                 delete proj2;
19536                 g[1] = (ft-f)/dt;
19537 
19538                 proj = new EMData();
19539                 proj2 = new EMData();
19540                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19541                 proj->fft_shuffle();
19542                 proj->center_origin_fft();
19543                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19544                 proj->do_ift_inplace();
19545                 proj2 = proj->window_center(M);
19546                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19547                 //ft = -ft;
19548                 delete proj;
19549                 delete proj2;
19550                 g[2] = (ft-f)/dt;
19551 
19552                 proj = new EMData();
19553                 proj2 = new EMData();
19554                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19555                 proj->fft_shuffle();
19556                 proj->center_origin_fft();
19557                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19558                 proj->do_ift_inplace();
19559                 proj2 = proj->window_center(M);
19560                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19561                 //ft = -ft;
19562                 delete proj;
19563                 delete proj2;
19564                 g[3] = (ft-f)/dt;
19565 
19566                 proj = new EMData();
19567                 proj2 = new EMData();
19568                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19569                 proj->fft_shuffle();
19570                 proj->center_origin_fft();
19571                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19572                 proj->do_ift_inplace();
19573                 proj2 = proj->window_center(M);
19574                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19575                 //ft = -ft;
19576                 delete proj;
19577                 delete proj2;
19578                 g[4] = (ft-f)/dt;
19579                 }
19580 
19581                 //c          go back to the minimization routine.
19582                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19583                 step++;
19584         }
19585 
19586         //printf("Total step is %d\n", step);
19587         vector<float> res;
19588         res.push_back(static_cast<float>(x[0]));
19589         res.push_back(static_cast<float>(x[1]));
19590         res.push_back(static_cast<float>(x[2]));
19591         res.push_back(static_cast<float>(x[3]));
19592         res.push_back(static_cast<float>(x[4]));
19593         //res.push_back(step);
19594         return res;
19595 }
19596 
19597 
19598 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19599 
19600         double  x[4];
19601         int n;
19602         int l = 3;
19603         int m = 200;
19604         double e = 1e-9;
19605         double step = 0.01;
19606         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19607 
19608         x[1] = ang;
19609         x[2] = sxs;
19610         x[3] = sys;
19611 
19612         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
19613         //printf("Took %d steps\n", n);
19614 
19615         vector<float> res;
19616         res.push_back(static_cast<float>(x[1]));
19617         res.push_back(static_cast<float>(x[2]));
19618         res.push_back(static_cast<float>(x[3]));
19619         res.push_back(static_cast<float>(n));
19620         return res;
19621 }
19622 
19623 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19624         
19625         const int nmax=args.size(), mmax=nmax;
19626         char task[60], csave[60];
19627         long int lsave[4];
19628         long int n, m, iprint, isave[44];
19629         long int* nbd = new long int[nmax];
19630         long int* iwa = new long int[3*nmax];
19631         double f, factr, pgtol;
19632         double* x = new double[nmax];
19633         double* l = new double[nmax];
19634         double* u = new double[nmax];
19635         double* g = new double[nmax];
19636         double dsave[29];
19637         double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19638         long int SIXTY=60;
19639 
19640         int num_ali = nmax/3+1;
19641         int nima = all_ali_params.size()/(num_ali*4);
19642         
19643         //     We wish to have no output.
19644         iprint = -1;
19645 
19646         //c     We specify the tolerances in the stopping criteria.
19647         factr=1.0e1;
19648         pgtol=1.0e-9;
19649 
19650         //     We specify the dimension n of the sample problem and the number
19651         //        m of limited memory corrections stored.  (n and m should not
19652         //        exceed the limits nmax and mmax respectively.)
19653         n=nmax;
19654         m=mmax;
19655 
19656         //     We now provide nbd which defines the bounds on the variables:
19657         //                    l   specifies the lower bounds,
19658         //                    u   specifies the upper bounds.
19659         //                    x   specifies the initial guess
19660         for (int i=0; i<nmax; i++) {
19661                 x[i] = args[i]; 
19662                 nbd[i] = 0;
19663         }
19664 
19665         //     We start the iteration by initializing task.
19666         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19667         strcpy(task,"START");
19668         for (int i=5;i<60;i++)  task[i]=' ';
19669 
19670         //     This is the call to the L-BFGS-B code.
19671         // (* call the L-BFGS-B routine with task='START' once before loop *)
19672         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19673         int step = 1;
19674 
19675         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19676         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19677 
19678                 if (strncmp(task,"FG",2)==0) {
19679                 //   the minimization routine has returned to request the
19680                 //   function f and gradient g values at the current x
19681 
19682                 //        Compute function value f for the sample problem.
19683                 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
19684 
19685                 //        Compute gradient g for the sample problem.
19686                 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
19687 
19688                 }
19689                 //c          go back to the minimization routine.
19690                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19691                 step++;
19692         }
19693 
19694         //printf("Total step is %d\n", step);
19695         vector<float> res;
19696         for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19697         res.push_back(static_cast<float>(f));
19698 
19699         delete[] nbd;
19700         delete[] iwa;
19701         delete[] x;
19702         delete[] l;
19703         delete[] u;
19704         delete[] g;
19705         delete[] wa;
19706 
19707         return res;
19708 
19709 }
19710 
19711 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d) {
19712 
19713         vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
19714         double sum_sqr_pixel_error = 0.0;
19715         for (int i=0; i<nima; i++)  sum_sqr_pixel_error += sqr_pixel_error[i];
19716         return sum_sqr_pixel_error/static_cast<float>(nima);
19717 }
19718 
19719 
19720 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d) {
19721 
19722         double* args = new double[num_ali*3];
19723         for (int i=0; i<3*num_ali-3; i++)   args[i] = x[i];
19724         args[3*num_ali-3] = 0.0;
19725         args[3*num_ali-2] = 0.0;
19726         args[3*num_ali-1] = 0.0;
19727         double* cosa = new double[num_ali];
19728         double* sina = new double[num_ali];
19729         for (int i=0; i<num_ali; i++) {
19730                 cosa[i] = cos(args[i*3]*M_PI/180.0);
19731                 sina[i] = sin(args[i*3]*M_PI/180.0);
19732         }
19733         double* sx = new double[num_ali];
19734         double* sy = new double[num_ali];
19735         
19736         vector<double> sqr_pixel_error(nima);
19737 
19738         for (int i=0; i<nima; i++) {
19739                 double sum_cosa = 0.0;
19740                 double sum_sina = 0.0;
19741                 for (int j=0; j<num_ali; j++) {
19742                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19743                                 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19744                                 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19745                                 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];
19746                                 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];
19747                         } else {
19748                                 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19749                                 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19750                                 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];
19751                                 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];
19752                         }
19753                 }
19754                 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19755                 sum_cosa /= P;
19756                 sum_sina /= P;
19757                 sqr_pixel_error[i] = d*d/4.0*(1.0-P/num_ali)+var(sx, num_ali)+var(sy, num_ali);
19758         }
19759         
19760         delete[] args;
19761         delete[] cosa;
19762         delete[] sina;
19763         delete[] sx;
19764         delete[] sy;
19765         
19766         return sqr_pixel_error;
19767 }
19768 
19769 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
19770 
19771         for (int i=0; i<num_ali*3-3; i++)    g[i] = 0.0;
19772 
19773         double* args = new double[num_ali*3];
19774         for (int i=0; i<3*num_ali-3; i++)   args[i] = x[i];
19775         args[3*num_ali-3] = 0.0;
19776         args[3*num_ali-2] = 0.0;
19777         args[3*num_ali-1] = 0.0;
19778         double* cosa = new double[num_ali];
19779         double* sina = new double[num_ali];
19780         for (int i=0; i<num_ali; i++) {
19781                 cosa[i] = cos(args[i*3]*M_PI/180.0);
19782                 sina[i] = sin(args[i*3]*M_PI/180.0);
19783         }
19784         double* sx = new double[num_ali];
19785         double* sy = new double[num_ali];
19786         
19787         vector<float> sqr_pixel_error(nima);
19788 
19789         for (int i=0; i<nima; i++) {
19790                 double sum_cosa = 0.0;
19791                 double sum_sina = 0.0;
19792                 for (int j=0; j<num_ali; j++) {
19793                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19794                                 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19795                                 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19796                                 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];
19797                                 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];
19798                         } else {
19799                                 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19800                                 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19801                                 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];
19802                                 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];
19803                         }
19804                 }
19805                 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19806                 sum_cosa /= P;
19807                 sum_sina /= P;
19808                 for (int j=0; j<num_ali-1; j++) {
19809                         double dx = 2.0*(sx[j]-mean(sx, num_ali));
19810                         double dy = 2.0*(sy[j]-mean(sy, num_ali));
19811                         if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19812                                 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
19813                                                     sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19814                                                     dx*(-ali_params[j*nima*4+i*4+1]*sina[j]-ali_params[j*nima*4+i*4+2]*cosa[j])+
19815                                                     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;
19816                                 g[j*3+1] += dx;
19817                                 g[j*3+2] += dy;
19818                         } else {
19819                                 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
19820                                                      sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19821                                                     dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19822                                                     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;
19823                                 g[j*3+1] += -dx;
19824                                 g[j*3+2] += dy;
19825                         }
19826                 }
19827         }
19828         
19829         for (int i=0; i<3*num_ali-3; i++)  g[i] /= (num_ali*nima);
19830         
19831         delete[] args;
19832         delete[] cosa;
19833         delete[] sina;
19834         delete[] sx;
19835         delete[] sy;
19836 }
19837 
19838 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19839 
19840         EMData *rot= new EMData();
19841         float ccc;
19842 
19843         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19844         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19845         delete rot;
19846         return ccc;
19847 }
19848 
19849 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19850 
19851         double  x[4];
19852         int n;
19853         int l = 3;
19854         int m = 200;
19855         double e = 1e-9;
19856         double step = 0.001;
19857         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19858 
19859         x[1] = ang;
19860         x[2] = sxs;
19861         x[3] = sys;
19862 
19863         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
19864         //printf("Took %d steps\n", n);
19865 
19866         vector<float> res;
19867         res.push_back(static_cast<float>(x[1]));
19868         res.push_back(static_cast<float>(x[2]));
19869         res.push_back(static_cast<float>(x[3]));
19870         res.push_back(static_cast<float>(n));
19871         return res;
19872 }
19873 
19874 
19875 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19876 
19877         EMData *rot= new EMData();
19878         float ccc;
19879 
19880         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19881         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19882         delete rot;
19883         return ccc;
19884 }
19885 
19886 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*(size_t)nx]
19887 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19888 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19889 {
19890         ENTERFUNC;
19891         /* Exception Handle */
19892         if (!img) {
19893                 throw NullPointerException("NULL input image");
19894         }
19895 
19896         int newx, newy, newz;
19897         bool  keep_going;
19898         cout << " entered   " <<endl;
19899         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19900         //int size = nx*ny*nz;
19901         EMData * img2 = new EMData();
19902         img2->set_size(nx,ny,nz);
19903         img2->to_zero();
19904         float *img_ptr  =img->get_data();
19905         float *img2_ptr = img2->get_data();
19906         int r2 = ro*ro;
19907         int r3 = r2*ro;
19908         int ri2 = ri*ri;
19909         int ri3 = ri2*ri;
19910 
19911         int n2 = nx/2;
19912 
19913         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
19914                 float z2 = static_cast<float>(k*k);
19915                 for (int j=-n2; j<=n2; j++) {
19916                         float y2 = z2 + j*j;
19917                         if(y2 <= r2) {
19918                                                                                         //cout << "  j  "<<j <<endl;
19919 
19920                                 for (int i=-n2; i<=n2; i++) {
19921                                         float x2 = y2 + i*i;
19922                                         if(x2 <= r3) {
19923                                                                                         //cout << "  i  "<<i <<endl;
19924                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19925                                                 if(x2 >= ri3) {
19926                                                         //  this is the outer shell, here points can only vanish
19927                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
19928                                                                 //cout << "  1  "<<ib <<endl;
19929                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
19930                                                                         img2_ptr(ib,jb,kb) = 0.0f;
19931                                                                         keep_going = true;
19932                                                                 //cout << "  try  "<<ib <<endl;
19933                                                                         while(keep_going) {
19934                                                                                 newx = Util::get_irand(-ro,ro);
19935                                                                                 newy = Util::get_irand(-ro,ro);
19936                                                                                 newz = Util::get_irand(-ro,ro);
19937                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
19938                                                                                         newx += n2; newy += n2; newz += n2;
19939                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
19940                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
19941                                                                                                 keep_going = false; }
19942                                                                                 }
19943                                                                         }
19944                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
19945                                                         }
19946                                                 }  else  {
19947                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
19948                                                         if(img_ptr(ib,jb,kb) == 1.0) {
19949                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
19950                                                                         //  find out the number of neighbors
19951                                                                         float  numn = -1.0f;  // we already know the central one is 1
19952                                                                         for (newz = -1; newz <= 1; newz++)
19953                                                                                 for (newy = -1; newy <= 1; newy++)
19954                                                                                         for (newx = -1; newx <= 1; newx++)
19955                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19956                                                                         img2_ptr(ib,jb,kb) = 0.0;
19957                                                                         if(numn == 26.0f) {
19958                                                                                 //  all neighbors exist, it has to vanish
19959                                                                                 keep_going = true;
19960                                                                                 while(keep_going) {
19961                                                                                         newx = Util::get_irand(-ro,ro);
19962                                                                                         newy = Util::get_irand(-ro,ro);
19963                                                                                         newz = Util::get_irand(-ro,ro);
19964                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
19965                                                                                                 newx += n2; newy += n2; newz += n2;
19966                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
19967                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
19968                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
19969                                                                                                                         newx += n2; newy += n2; newz += n2;
19970                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
19971                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
19972                                                                                                                                 keep_going = false; }
19973                                                                                                                 }
19974                                                                                                         }
19975                                                                                                 }
19976                                                                                         }
19977                                                                                 }
19978                                                                         }  else if(numn == 25.0f) {
19979                                                                                 // there is only one empty neighbor, move there
19980                                                                                 for (newz = -1; newz <= 1; newz++) {
19981                                                                                         for (newy = -1; newy <= 1; newy++) {
19982                                                                                                 for (newx = -1; newx <= 1; newx++) {
19983                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
19984                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19985                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19986                                                                                                                         }
19987                                                                                                         }
19988                                                                                                 }
19989                                                                                         }
19990                                                                                 }
19991                                                                         }  else {
19992                                                                                 //  more than one neighbor is zero, select randomly one and move there
19993                                                                                 keep_going = true;
19994                                                                                 while(keep_going) {
19995                                                                                         newx = Util::get_irand(-1,1);
19996                                                                                         newy = Util::get_irand(-1,1);
19997                                                                                         newz = Util::get_irand(-1,1);
19998                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
19999                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
20000                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
20001                                                                                                         keep_going = false;
20002                                                                                                 }
20003                                                                                         }
20004                                                                                 }
20005                                                                         }
20006                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
20007                                                         }
20008                                                 }
20009                                         }
20010                                 }
20011                         }
20012                 }
20013         }
20014         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
20015         img2->update();
20016 
20017         EXITFUNC;
20018         return img2;
20019 }
20020 #undef img_ptr
20021 #undef img2_ptr
20022 
20023 struct point3d_t
20024 {
20025         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20026 
20027         int x;
20028         int y;
20029         int z;
20030 };
20031 
20032 
20033 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20034 {
20035         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20036         int noff = 6;
20037 
20038         int nx = visited->get_xsize();
20039         int ny = visited->get_ysize();
20040         int nz = visited->get_zsize();
20041 
20042         vector< point3d_t > pts;
20043         pts.push_back( point3d_t(ix, iy, iz) );
20044         visited->set_value_at( ix, iy, iz, (float)grpid );
20045 
20046         int start = 0;
20047         int end = pts.size();
20048 
20049         while( end > start ) {
20050                 for(int i=start; i < end; ++i ) {
20051                         int ix = pts[i].x;
20052                         int iy = pts[i].y;
20053                         int iz = pts[i].z;
20054 
20055                         for( int j=0; j < noff; ++j ) {
20056                                 int jx = ix + offs[j][0];
20057                                 int jy = iy + offs[j][1];
20058                                 int jz = iz + offs[j][2];
20059 
20060                                 if( jx < 0 || jx >= nx ) continue;
20061                                 if( jy < 0 || jy >= ny ) continue;
20062                                 if( jz < 0 || jz >= nz ) continue;
20063 
20064 
20065                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20066                                     pts.push_back( point3d_t(jx, jy, jz) );
20067                                     visited->set_value_at( jx, jy, jz, (float)grpid );
20068                                 }
20069 
20070                         }
20071                 }
20072 
20073                 start = end;
20074                 end = pts.size();
20075         }
20076         return pts.size();
20077 }
20078 
20079 
20080 EMData* Util::get_biggest_cluster( EMData* mg )
20081 {
20082         int nx = mg->get_xsize();
20083         int ny = mg->get_ysize();
20084         int nz = mg->get_zsize();
20085 
20086         EMData* visited = new EMData();
20087         visited->set_size( nx, ny, nz );
20088         visited->to_zero();
20089         int grpid = 0;
20090         int maxgrp = 0;
20091         int maxsize = 0;
20092         for( int iz=0; iz < nz; ++iz ) {
20093                 for( int iy=0; iy < ny; ++iy ) {
20094                         for( int ix=0; ix < nx; ++ix ) {
20095                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20096 
20097                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
20098                                         // visited before, must be in other group.
20099                                         continue;
20100                                 }
20101 
20102                                 grpid++;
20103                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20104                                 if( grpsize > maxsize ) {
20105                                         maxsize = grpsize;
20106                                         maxgrp = grpid;
20107                                 }
20108                         }
20109                 }
20110         }
20111 
20112         Assert( maxgrp > 0 );
20113 
20114         int npoint = 0;
20115         EMData* result = new EMData();
20116         result->set_size( nx, ny, nz );
20117         result->to_zero();
20118 
20119         for( int iz=0; iz < nz; ++iz ) {
20120                 for( int iy=0; iy < ny; ++iy ) {
20121                         for( int ix=0; ix < nx; ++ix ) {
20122                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
20123                                         (*result)(ix,iy,iz) = 1.0;
20124                                         npoint++;
20125                                 }
20126                         }
20127                 }
20128         }
20129 
20130         Assert( npoint==maxsize );
20131         delete visited;
20132         return result;
20133 
20134 }
20135 
20136 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)
20137 {
20138         int    ix, iy, iz;
20139         int    i,  j, k;
20140         int    nr2, nl2;
20141         float  az, ak;
20142         float  scx, scy, scz;
20143         int    offset = 2 - nx%2;
20144         int    lsm = nx + offset;
20145         EMData* ctf_img1 = new EMData();
20146         ctf_img1->set_size(lsm, ny, nz);
20147         float freq = 1.0f/(2.0f*ps);
20148         scx = 2.0f/float(nx);
20149         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20150         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20151         nr2 = ny/2 ;
20152         nl2 = nz/2 ;
20153         float pihalf = M_PI/2.0f;
20154         for ( k=0; k<nz;k++) {
20155                 iz = k;  if(k>nl2) iz=k-nz;
20156                 float oz2 = iz*scz*iz*scz;
20157                 for ( j=0; j<ny;j++) {
20158                         iy = j;  if(j>nr2) iy=j - ny;
20159                         float oy = iy*scy;
20160                         float oy2 = oy*oy;
20161                         for ( i=0; i<lsm/2; i++) {
20162                                 ix=i;
20163                                 if( dza == 0.0f) {
20164                                         ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20165                                         (*ctf_img1) (i*2,j,k)   = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20166                                 } else {
20167                                         float ox = ix*scx;
20168                                         ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20169                                         az = atan2(oy, ox);
20170                                         float dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f-pihalf));
20171                                         (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20172                                 }
20173                                 //(*ctf_img1) (i*2+1,j,k) = 0.0f;  PAP  I assumed new EMData sets to zero
20174                         }
20175                 }
20176         }
20177         ctf_img1->update();
20178         ctf_img1->set_complex(true);
20179         ctf_img1->set_ri(true);
20180         //ctf_img1->attr_dict["is_complex"] = 1;
20181         //ctf_img1->attr_dict["is_ri"] = 1;
20182         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20183         return ctf_img1;
20184 }
20185 /*
20186 #define  cent(i)     out[i+N]
20187 #define  assign(i)   out[i]
20188 vector<float> Util::cluster_pairwise(EMData* d, int K) {
20189 
20190         int nx = d->get_xsize();
20191         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20192         vector<float> out(N+K+2);
20193         if(N*(N-1)/2 != nx) {
20194                 //print  "  incorrect dimension"
20195                 return out;}
20196         //  assign random objects as centers
20197         for(int i=0; i<N; i++) assign(i) = float(i);
20198         // shuffle
20199         for(int i=0; i<N; i++) {
20200                 int j = Util::get_irand(0,N-1);
20201                 float temp = assign(i);
20202                 assign(i) = assign(j);
20203                 assign(j) = temp;
20204         }
20205         for(int k=0; k<K; k++) cent(k) = float(assign(k));
20206         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
20207         //
20208         for(int i=0; i<N; i++) assign(i) = 0.0f;
20209         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20210         bool change = true;
20211         int it = -1;
20212         while(change && disp < dispold) {
20213                 change = false;
20214                 dispold = disp;
20215                 it++;
20216                 //cout<<"Iteration:  "<<it<<endl;
20217                 // dispersion is a sum of distance from objects to object center
20218                 disp = 0.0f;
20219                 for(int i=0; i<N; i++) {
20220                         qm = 1.0e23f;
20221                         for(int k=0; k<K; k++) {
20222                                 if(float(i) == cent(k)) {
20223                                         qm = 0.0f;
20224                                         na = (float)k;
20225                                 } else {
20226                                         float dt = (*d)(mono(i,int(cent(k))));
20227                                         if(dt < qm) {
20228                                                 qm = dt;
20229                                                 na = (float)k;
20230                                         }
20231                                 }
20232                         }
20233                         disp += qm;
20234                         if(na != assign(i)) {
20235                                 assign(i) = na;
20236                                 change = true;
20237                         }
20238                 }
20239         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
20240                 //print disp
20241                 //print  assign
20242                 // find centers
20243                 for(int k=0; k<K; k++) {
20244                         qm = 1.0e23f;
20245                         for(int i=0; i<N; i++) {
20246                                 if(assign(i) == float(k)) {
20247                                         float q = 0.0;
20248                                         for(int j=0; j<N; j++) {
20249                                                 if(assign(j) == float(k)) {
20250                                                                 //it cannot be the same object
20251                                                         if(i != j)  q += (*d)(mono(i,j));
20252                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
20253                                                 }
20254                                         }
20255                                         if(q < qm) {
20256                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
20257                                                 qm = q;
20258                                                 cent(k) = float(i);
20259                                         }
20260                                 }
20261                         }
20262                 }
20263         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
20264         }
20265         out[N+K] = disp;
20266         out[N+K+1] = float(it);
20267         return  out;
20268 }
20269 #undef  cent
20270 #undef  assign
20271 */
20272 #define  cent(i)     out[i+N]
20273 #define  assign(i)   out[i]
20274 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20275         int nx = d->get_xsize();
20276         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20277         vector<float> out(N+K+2);
20278         if(N*(N-1)/2 != nx) {
20279                 //print  "  incorrect dimension"
20280                 return out;}
20281         //  assign random objects as centers
20282         for(int i=0; i<N; i++) assign(i) = float(i);
20283         // shuffle
20284         for(int i=0; i<N; i++) {
20285                 int j = Util::get_irand(0,N-1);
20286                 float temp = assign(i);
20287                 assign(i) = assign(j);
20288                 assign(j) = temp;
20289         }
20290         for(int k=0; k<K; k++) cent(k) = float(assign(k));
20291         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
20292         //
20293         for(int i=0; i<N; i++) assign(i) = 0.0f;
20294         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20295         bool change = true;
20296         int it = -1;
20297         int ct = -1;
20298         while ((change && disp < dispold) || ct > 0) {
20299 
20300                 change = false;
20301                 dispold = disp;
20302                 it++;
20303 
20304                 // dispersion is a sum of distance from objects to object center
20305                 disp = 0.0f;
20306                 ct = 0;
20307                 for(int i=0; i<N; i++) {
20308                         qm = 1.0e23f;
20309                         for(int k=0; k<K; k++) {
20310                                 if(float(i) == cent(k)) {
20311                                         qm = 0.0f;
20312                                         na = (float)k;
20313                                 } else {
20314                                         float dt = (*d)(mono(i,int(cent(k))));
20315                                         if(dt < qm) {
20316                                                 qm = dt;
20317                                                 na = (float)k;
20318                                         }
20319                                 }
20320                         }
20321 
20322 
20323                         // Simulated annealing
20324                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20325                             na = (float)(Util::get_irand(0, K));
20326                             qm = (*d)(mono(i,int(na)));
20327                             ct++;
20328                         }
20329 
20330                         disp += qm;
20331 
20332                         if(na != assign(i)) {
20333                                 assign(i) = na;
20334                                 change = true;
20335                         }
20336                 }
20337 
20338                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
20339                 T = T*F;
20340 
20341         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
20342                 //print disp
20343                 //print  assign
20344                 // find centers
20345                 for(int k=0; k<K; k++) {
20346                         qm = 1.0e23f;
20347                         for(int i=0; i<N; i++) {
20348                                 if(assign(i) == float(k)) {
20349                                         float q = 0.0;
20350                                         for(int j=0; j<N; j++) {
20351                                                 if(assign(j) == float(k)) {
20352                                                                 //it cannot be the same object
20353                                                         if(i != j)  q += (*d)(mono(i,j));
20354                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
20355                                                 }
20356                                         }
20357                                         if(q < qm) {
20358                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
20359                                                 qm = q;
20360                                                 cent(k) = float(i);
20361                                         }
20362                                 }
20363                         }
20364                 }
20365         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
20366         }
20367         out[N+K] = disp;
20368         out[N+K+1] = float(it);
20369         return  out;
20370 }
20371 #undef  cent
20372 #undef  assign
20373 /*
20374 #define  groupping(i,k)   group[i + k*m]
20375 vector<float> Util::cluster_equalsize(EMData* d, int m) {
20376         int nx = d->get_xsize();
20377         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20378         int K = N/m;
20379         //cout<<"  K  "<<K<<endl;
20380         vector<float> group(N+1);
20381         if(N*(N-1)/2 != nx) {
20382                 //print  "  incorrect dimension"
20383                 return group;}
20384         bool active[N];
20385         for(int i=0; i<N; i++) active[i] = true;
20386 
20387         float dm, qd;
20388         int   ppi, ppj;
20389         for(int k=0; k<K; k++) {
20390                 // find two most similiar objects among active
20391                 cout<<"  k  "<<k<<endl;
20392                 dm = 1.0e23;
20393                 for(int i=1; i<N; i++) {
20394                         if(active[i]) {
20395                                 for(int j=0; j<i; j++) {
20396                                         if(active[j]) {
20397                                                 qd = (*d)(mono(i,j));
20398                                                 if(qd < dm) {
20399                                                         dm = qd;
20400                                                         ppi = i;
20401                                                         ppj = j;
20402                                                 }
20403                                         }
20404                                 }
20405                         }
20406                 }
20407                 groupping(0,k) = float(ppi);
20408                 groupping(1,k) = float(ppj);
20409                 active[ppi] = false;
20410                 active[ppj] = false;
20411 
20412                 // find progressively objects most similar to those in the current list
20413                 for(int l=2; l<m; l++) {
20414                         //cout<<"  l  "<<l<<endl;
20415                         dm = 1.0e23;
20416                         for(int i=0; i<N; i++) {
20417                                 if(active[i]) {
20418                                         qd = 0.0;
20419                                         for(int j=0; j<l; j++) { //j in groupping[k]:
20420                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
20421                                                 int jj = int(groupping(j,k));
20422                         //cout<<"   "<<jj<<endl;
20423                                                 qd += (*d)(mono(i,jj));
20424                                         }
20425                                         if(qd < dm) {
20426                                                 dm = qd;
20427                                                 ppi = i;
20428                                         }
20429                                 }
20430                         }
20431                         groupping(l,k) = float(ppi);
20432                         active[ppi] = false;
20433                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
20434                 }
20435                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
20436         }
20437         // there might be remaining objects when N is not divisible by m, simply put them in one group
20438         if(N%m != 0) {
20439                 int j = K*m;
20440                 K++;
20441                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
20442                 for(int i=0; i<N; i++) {
20443                         if(active[i]) {
20444                                 group[j] = float(i);
20445                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
20446                                 j++;
20447                         }
20448                 }
20449         }
20450 
20451         int  cent[K];
20452          // find centers
20453         for(int k=0; k<K; k++) {
20454                 float qm = 1.0e23f;
20455                 for(int i=0; i<N; i++) {
20456                         if(group[i] == float(k)) {
20457                                 qd = 0.0;
20458                                 for(int j=0; j<N; j++) {
20459                                         if(group[j] == float(k)) {
20460                                                 //it cannot be the same object
20461                                                 if(i != j)  qd += (*d)(mono(i,j));
20462                                         }
20463                                 }
20464                                 if(qd < qm) {
20465                                         qm = qd;
20466                                         cent[k] = i;
20467                                 }
20468                         }
20469                 }
20470         }
20471         // dispersion is a sum of distances from objects to object center
20472         float disp = 0.0f;
20473         for(int i=0; i<N; i++) {
20474                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
20475         }
20476         group[N] = disp;
20477         return  group;
20478 }
20479 #undef  groupping
20480 */
20481 
20482 vector<float> Util::cluster_equalsize(EMData* d) {
20483         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
20484         int nx = d->get_xsize();
20485         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20486         int K = N/2;
20487         vector<float> group(N);
20488         if(N*(N-1)/2 != nx) {
20489                 //print  "  incorrect dimension"
20490                 return group;}
20491         //bool active[N];       //this does not compile in VS2005. --Grant Tang
20492         bool * active = new bool[N];
20493         for(int i=0; i<N; i++) active[i] = true;
20494 
20495         float dm, qd;
20496         int   ppi = 0, ppj = 0;
20497         for(int k=0; k<K; k++) {
20498                 // find pairs of most similiar objects among active
20499                 //cout<<"  k  "<<k<<endl;
20500                 dm = 1.0e23f;
20501                 for(int i=1; i<N; i++) {
20502                         if(active[i]) {
20503                                 for(int j=0; j<i; j++) {
20504                                         if(active[j]) {
20505                                                 qd = (*d)(i*(i - 1)/2 + j);
20506                                                 if(qd < dm) {
20507                                                         dm = qd;
20508                                                         ppi = i;
20509                                                         ppj = j;
20510                                                 }
20511                                         }
20512                                 }
20513                         }
20514                 }
20515                 group[2*k] = float(ppi);
20516                 group[1+2*k] = float(ppj);
20517                 active[ppi] = false;
20518                 active[ppj] = false;
20519         }
20520 
20521         delete [] active;
20522         active = NULL;
20523         return  group;
20524 }
20525 /*
20526 #define son(i,j)=i*(i-1)/2+j
20527 vector<float> Util::cluster_equalsize(EMData* d) {
20528         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
20529         int nx = d->get_xsize();
20530         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20531         int K = N/2;
20532         vector<float> group(N);
20533         if(N*(N-1)/2 != nx) {
20534                 //print  "  incorrect dimension"
20535                 return group;}
20536         //bool active[N];
20537         int  active[N];
20538         for(int i=0; i<N; i++) active[i] = i;
20539 
20540         float dm, qd;
20541         int   ppi = 0, ppj = 0, ln = N;
20542         for(int k=0; k<K; k++) {
20543                 // find pairs of most similiar objects among active
20544                 //cout<<"  k:  "<<k<<endl;
20545                 dm = 1.0e23;
20546                 for(int i=1; i<ln; i++) {
20547                         for(int j=0; j<i; j++) {
20548                                 //qd = (*d)(mono(active[i],active[j]));
20549                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
20550                                 if(qd < dm) {
20551                                         dm = qd;
20552                                         ppi = i;
20553                                         ppj = j;
20554                                 }
20555                         }
20556                 }
20557                 group[2*k]   = float(active[ppi]);
20558                 group[1+2*k] = float(active[ppj]);
20559                 //  Shorten the list
20560                 if(ppi > ln-3 || ppj > ln - 3) {
20561                         if(ppi > ln-3 && ppj > ln - 3) {
20562                         } else if(ppi > ln-3) {
20563                                 if(ppi == ln -1) active[ppj] = active[ln-2];
20564                                 else             active[ppj] = active[ln-1];
20565                         } else { // ppj>ln-3
20566                                 if(ppj == ln -1) active[ppi] = active[ln-2];
20567                                 else             active[ppi] = active[ln-1];
20568                         }
20569                 } else {
20570                         active[ppi] = active[ln-1];
20571                         active[ppj] = active[ln-2];
20572                 }
20573                 ln = ln - 2;
20574         }
20575         return  group;
20576 }
20577 
20578 */
20579 #define data(i,j) group[i*ny+j]
20580 vector<float> Util::vareas(EMData* d) {
20581         const float step=0.001f;
20582         int ny = d->get_ysize();
20583         //  input emdata should have size 2xN, where N is number of points
20584         //  output vector should be 2xN, first element is the number of elements
20585         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
20586         vector<float> group(2*ny);
20587         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20588         int K = int(1.0f/step) +1;
20589         int hit = 0;
20590         for(int kx=0; kx<=K; kx++) {
20591                 float tx = kx*step;
20592                 for(int ky=0; ky<=K; ky++) {
20593                         float ty = ky*step;
20594                         float dm = 1.0e23f;
20595                         for(int i=0; i<ny; i++) {
20596                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20597                                 if( qd < dm) {
20598                                         dm = qd;
20599                                         hit = i;
20600                                 }
20601                         }
20602                         data(0,hit) += 1.0f;
20603                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
20604                 }
20605         }
20606         return  group;
20607 }
20608 #undef data
20609 
20610 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20611 
20612         int nx = vol->get_xsize();
20613         int ny = vol->get_ysize();
20614         int nz = vol->get_zsize();
20615         float *vol_data = vol->get_data();
20616         int new_nx, new_ny;
20617 
20618         if (nz == 1)
20619                 throw ImageDimensionException("Error: Input must be a 3-D object");
20620         if ((dim < 1) || (dim > 3))
20621                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20622         if (((dim == 1) && (index < 0 || index > nx-1)) ||
20623           ((dim == 1) && (index < 0 || index > nx-1)) ||
20624           ((dim == 1) && (index < 0 || index > nx-1)))
20625                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20626 
20627         if (dim == 1) {
20628                 new_nx = ny;
20629                 new_ny = nz;
20630         } else if (dim == 2) {
20631                 new_nx = nx;
20632                 new_ny = nz;
20633         } else {
20634                 new_nx = nx;
20635                 new_ny = ny;
20636         }
20637 
20638         EMData *slice = new EMData();
20639         slice->set_size(new_nx, new_ny, 1);
20640         float *slice_data = slice->get_data();
20641 
20642         if (dim == 1) {
20643                 for (int x=0; x<new_nx; x++)
20644                         for (int y=0; y<new_ny; y++)
20645                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20646         } else if (dim == 2) {
20647                 for (int x=0; x<new_nx; x++)
20648                         for (int y=0; y<new_ny; y++)
20649                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20650         } else {
20651                 for (int x=0; x<new_nx; x++)
20652                         for (int y=0; y<new_ny; y++)
20653                                 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20654         }
20655 
20656         return slice;
20657 }
20658 
20659 void Util::image_mutation(EMData *img, float mutation_rate) {
20660         int nx = img->get_xsize();
20661         float min = img->get_attr("minimum");
20662         float max = img->get_attr("maximum");
20663         float* img_data = img->get_data();
20664         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20665         return;
20666 }
20667 
20668 
20669 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20670 
20671         if (is_mirror != 0) {
20672                 for (int i=0; i<len_list; i++) {
20673                         int r = rand()%10000;
20674                         float f = r/10000.0f;
20675                         if (f < mutation_rate) list[i] = 1-list[i];
20676                 }
20677         } else {
20678                 map<int, vector<int> >  graycode;
20679                 map<vector<int>, int> rev_graycode;
20680                 vector <int> gray;
20681 
20682                 int K=1;
20683                 for (int i=0; i<L; i++) K*=2;
20684 
20685                 for (int k=0; k<K; k++) {
20686                         int shift = 0;
20687                         vector <int> gray;
20688                         for (int i=L-1; i>-1; i--) {
20689                                 int t = ((k>>i)%2-shift)%2;
20690                                 gray.push_back(t);
20691                                 shift += t-2;
20692                         }
20693                         graycode[k] = gray;
20694                         rev_graycode[gray] = k;
20695                 }
20696 
20697                 float gap = (K-1)/(max_val-min_val);
20698                 for (int i=0; i<len_list; i++) {
20699                         float val = list[i];
20700                         if (val < min_val) { val = min_val; }
20701                         else if  (val > max_val) { val = max_val; }
20702                         int k = int((val-min_val)*gap+0.5);
20703                         vector<int> gray = graycode[k];
20704                         bool changed = false;
20705                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20706                                 int r = rand()%10000;
20707                                 float f = r/10000.0f;
20708                                 if (f < mutation_rate) {
20709                                         *p = 1-*p;
20710                                         changed = true;
20711                                 }
20712                         }
20713                         if (changed) {
20714                                 k = rev_graycode[gray];
20715                                 list[i] = k/gap+min_val;
20716                         }
20717                 }
20718         }
20719 
20720 }
20721 
20722 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20723 
20724         if (is_mirror != 0) {
20725                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20726                         int r = rand()%10000;
20727                         float f = r/10000.0f;
20728                         if (f < mutation_rate) *q = 1-*q;
20729                 }
20730         } else {
20731                 map<int, vector<int> >  graycode;
20732                 map<vector<int>, int> rev_graycode;
20733                 vector <int> gray;
20734 
20735                 int K=1;
20736                 for (int i=0; i<L; i++) K*=2;
20737 
20738                 for (int k=0; k<K; k++) {
20739                         int shift = 0;
20740                         vector <int> gray;
20741                         for (int i=L-1; i>-1; i--) {
20742                                 int t = ((k>>i)%2-shift)%2;
20743                                 gray.push_back(t);
20744                                 shift += t-2;
20745                         }
20746                         graycode[k] = gray;
20747                         rev_graycode[gray] = k;
20748                 }
20749 
20750                 float gap = (K-1)/(max_val-min_val);
20751                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20752                         float val = *q;
20753                         if (val < min_val) { val = min_val; }
20754                         else if  (val > max_val) { val = max_val; }
20755                         int k = int((val-min_val)*gap+0.5);
20756                         vector<int> gray = graycode[k];
20757                         bool changed = false;
20758                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20759                                 int r = rand()%10000;
20760                                 float f = r/10000.0f;
20761                                 if (f < mutation_rate) {
20762                                         *p = 1-*p;
20763                                         changed = true;
20764                                 }
20765                         }
20766                         if (changed) {
20767                                 k = rev_graycode[gray];
20768                                 *q = k/gap+min_val;
20769                         }
20770                 }
20771         }
20772         return list;
20773 }
20774 
20775 
20776 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20777         //cout<<"sanitycheck called\n";
20778         int total_cost = *output;
20779         int num_matches = *(output+1);
20780 
20781         int cost=0;
20782         int* intx;
20783         int intx_size;
20784         int* intx_next(0);
20785         int intx_next_size = 0;
20786         int curclass;
20787         int curclass_size;
20788         //cout<<"cost by match: [";
20789         for(int i = 0; i < num_matches; i++){
20790                 curclass = *(output+2+ i*nParts);
20791                 // check feasibility
20792                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20793                 *(argParts + Indices[curclass]+1) = -5;
20794                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
20795                 curclass_size = *(dimClasses+curclass)-2;
20796                 intx = new int[curclass_size];
20797                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20798                 intx_size = curclass_size;
20799 
20800                 for (int j=1; j < nParts; j++){
20801                       curclass = *(output+2+ i*nParts+j);
20802                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20803                       *(argParts + Indices[j*K+curclass]+1)=-5;
20804                       // compute the intersection of intx and class curclass of partition j of the i-th match
20805                       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);
20806                       intx_next = new int[intx_next_size];
20807                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20808                       delete[] intx;
20809                       intx=intx_next;
20810                       intx_size= intx_next_size;
20811                 }
20812                 delete[] intx_next;
20813 
20814                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20815                 //cout <<intx_next_size<<",";
20816                 cost = cost + intx_next_size;
20817         }
20818         //cout<<"]\n";
20819         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20820 
20821         return 1;
20822 
20823 }
20824 
20825 
20826 // Given J, returns the J matches with the largest weight
20827 // matchlist has room for J matches
20828 // costlist has J elements to record cost of the J largest matches
20829 
20830 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20831         
20832         // some temp variables
20833         bool flag = 0;
20834         int nintx;
20835         int* dummy(0);
20836         //int* ret;
20837         int* curbranch = new int[nParts];
20838         
20839         //initialize costlist to all 0
20840         for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20841         
20842         
20843         for(int a=0; a<K; a++)
20844         {
20845         
20846                 // 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
20847                 if (*(argParts + Indices[a] + 1) < 1) continue;
20848                 if (*(dimClasses + a)-2 <= T) continue;
20849 
20850                 // 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
20851 
20852                 for( int i=1; i < nParts; i++){
20853                         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.
20854                         for(int j=0; j < K; j++){
20855                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20856                                 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);
20857                                 if (nintx > T) flag=1;
20858                                 else *(argParts + Indices[i*K+j] + 1) =-4;
20859                         }
20860                         if (flag==0) {break;}
20861                 }
20862 
20863                 // explore determines J matchs with the largest weight greater than T where class in partition 0 is class a
20864                 *curbranch = a;
20865 
20866                 if (flag > 0) // Each partition has one or more active class
20867                         Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20868                         *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20869                         
20870                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
20871                 for( int i=1; i < nParts; i++){
20872                         for(int j=0; j < K; j++){
20873                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20874 
20875                         }
20876                 }
20877         }
20878         
20879         delete[] curbranch;
20880 }
20881 
20882 // returns J largest matches
20883 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){
20884 
20885 // depth is the level which is going to be explored in the current iteration
20886         int* curintx2(0);
20887         int nintx = size_curintx;
20888         
20889         
20890         // 2. take the intx of next and cur. Prune if <= T
20891         if (depth >0){
20892                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20893                 if (nintx <= T) return; //prune!
20894         }
20895 
20896         // 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
20897         if (depth == (nParts-1)) {
20898                 
20899                 int replace = 0;
20900                 int ind_smallest = -1;
20901                 int smallest_cost = -1;
20902                 
20903                 for (int jit = 0; jit < J; jit++){
20904                         if (*(costlist+jit) < nintx){
20905                                 replace = 1;
20906                                 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20907                                 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20908                         }       
20909                 }
20910                 
20911                 if (replace > 0){
20912                         // replace the smallest cost in matchlist with the current stuff
20913                         *(costlist + ind_smallest) = nintx;
20914                         for (int xit = 0; xit < nParts; xit++)
20915                                 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20916                                 
20917                 }
20918                 
20919                 return; 
20920         }
20921         
20922 
20923         // 3. have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20924 
20925         if (depth > 0){
20926                 curintx2 = new int[nintx]; // put the intersection set in here
20927                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20928         }
20929 
20930         if (depth == 0){
20931                 // set curintx2 to curintx
20932                 curintx2 = new int[size_curintx];
20933                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20934         }
20935 
20936 
20937         // recursion (non-leaf case)
20938         depth=depth+1;
20939         // we now consider each of the classes in partition depth and recurse upon each of them
20940         for (int i=0; i < K; i++){
20941 
20942                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
20943                 size_next = (*(dimClasses + depth*K+i ))-2;
20944                 if (size_next <= T) continue;
20945                 *(curbranch+depth) = i;
20946                 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20947                         costlist, curbranch);
20948                 
20949         }
20950 
20951         delete[] curintx2;
20952 }
20953 
20954 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20955         //cout<<"initial_prune\n";
20956         // simple initial pruning. For class indClass of partition indPart:
20957         // 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
20958         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
20959 
20960         // 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
20961 
20962         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
20963         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
20964 
20965         int* dummy(0);
20966         int* cref;
20967         int cref_size;
20968         int* ccomp;
20969         int ccomp_size;
20970         int nintx;
20971         for (int i=0; i < nParts; i++){
20972                 for (int j =0; j < K; j++){
20973 
20974                         // consider class Parts[i][j]
20975                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
20976                         cref_size = dimClasses[i*K+cref[0]]-2;
20977 
20978 
20979                         if (cref_size <= T){
20980                                 cref[0] = -1;
20981                                 continue;
20982                         }
20983                         bool done = 0;
20984                         for (int a = 0; a < nParts; a++){
20985                                 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
20986                                 bool hasActive=0;
20987                                 for (unsigned int b=0; b < Parts[a].size(); b++){
20988                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
20989                                         // remember first element of each class is the index of the class
20990                                         ccomp = Parts[a][b];
20991                                         ccomp_size= dimClasses[a*K+ccomp[0]]-2;
20992                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20993 
20994 
20995                                         if (nintx <= T)
20996                                                 ccomp[1] = 0; // class Parts[a][b] is 'inactive' for cref
20997                                         else{
20998                                                 ccomp[1] = 1; // class Parts[a][b] is 'active' for cref
20999                                                 hasActive=1;
21000                                         }
21001                                 }
21002                                 // see if partition a has at least one active class.if not then we're done with cref
21003                                 if (hasActive < 1){
21004                                    done=1;
21005                                    break;
21006                                 }
21007 
21008                         }
21009 
21010                         if (done > 0){
21011                                 // remove class j from partition i
21012 
21013                                 cref[0] = -1; // mark for deletion later
21014                                 continue; // move on to class Parts[i][j+1]
21015                         }
21016 
21017                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
21018                         // 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.
21019 
21020                         // (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.
21021                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
21022                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
21023 
21024                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
21025                         //bool found = 1;
21026                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21027 
21028                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
21029                                 // Parts[i].erase(Parts[i].begin()+j);
21030                                 cref[0] = -1;
21031                         }
21032                 }
21033 
21034                 // Erase from Parts[i] all the classes that's being designated for erasure
21035 
21036                 for (int d = K-1; d > -1; d--){
21037                         if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21038                 }
21039 
21040         }
21041         //cout <<"number of classes left in each partition after initial prune\n";      
21042         // Print out how many classes are left in each partition
21043         //for (int i =0; i < nParts; i++)
21044         //      cout << Parts[i].size()<<", ";
21045         //cout << "\n";
21046 }
21047 
21048 
21049 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) {
21050 
21051 
21052         if (size_next <= T) return 0;
21053 
21054         // take the intx of next and cur
21055         int* curintx2(0);
21056         int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21057         if (nintx <= T) return 0;
21058 
21059         int old_depth=depth;
21060         if (depth == partref) depth = depth + 1; // we skip classes in partref
21061         if (depth == nParts &&  old_depth>0) return 1;
21062 
21063         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
21064 
21065         curintx2 = new int[nintx]; // put the intersection set in here
21066         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21067 
21068         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
21069 
21070         // we now consider each of the classes in partition (depth+1) in turn
21071         bool gt_thresh;
21072         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
21073 
21074         for (int i=0; i < num_classes; i++){
21075                 if (Parts[depth][i][1] < 1) continue; // class is not active so move on
21076                 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21077                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21078                 if (gt_thresh) { delete[] curintx2; return 1; }
21079         }
21080         delete[] curintx2;
21081         return 0;
21082 }
21083 
21084 
21085 
21086 
21087 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21088 int max_branching, float stmult, int branchfunc, int LIM) {
21089         
21090         
21091         // 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
21092         // 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
21093         // Make a vector of nParts vectors of K int* each
21094          int* Indices = new int[nParts*K];
21095          int ind_c = 0;
21096          for (int i=0; i < nParts; i++){
21097                  for(int j = 0; j < K; j++){
21098                          Indices[i*K + j] = ind_c;
21099                          ind_c = ind_c + dimClasses[i*K + j];
21100                  }
21101          }
21102 
21103         // do initial pruning on argParts and return the pruned partitions
21104 
21105         // Make a vector of nParts vectors of K int* each
21106         vector <vector <int*> > Parts(nParts,vector<int*>(K));
21107         ind_c = 0;
21108         int argParts_size=0;
21109         for (int i=0; i < nParts; i++){
21110                 for(int j = 0; j < K; j++){
21111                         Parts[i][j] = argParts + ind_c;
21112                         ind_c = ind_c + dimClasses[i*K + j];
21113                         argParts_size = argParts_size + dimClasses[i*K + j];
21114                 }
21115         }
21116 
21117         // in the following we call initial_prune with Parts which is a vector. This is not the most
21118         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
21119         // 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.....
21120 
21121         // comment out for testing
21122         Util::initial_prune(Parts, dimClasses, nParts, K, T);
21123         for(int i = 0; i < nParts; i++){
21124                 for(int j=0; j < K; j++){
21125                         argParts[Indices[i*K + j]+1] = -1;
21126                 }
21127         }
21128 
21129         int num_classes;
21130         int old_index;
21131         for(int i=0; i<nParts; i++){
21132                 num_classes = Parts[i].size();// number of classes in partition i after pruning
21133                 for (int j=0; j < num_classes; j++){
21134                         old_index = Parts[i][j][0];
21135                         //cout << "old_index: " << old_index<<"\n";
21136                         argParts[Indices[i*K + old_index]+1] = 1;
21137                 }
21138         }
21139 
21140 
21141         // if we're not doing mpi then keep going and call branchMPI and return the output
21142         //cout <<"begin partition matching\n";
21143         //int* dummy(0);
21144         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21145         
21146         //cout<<"total cost: "<<*output<<"\n";
21147         //cout<<"number of matches: "<<*(output+1)<<"\n";
21148         // 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
21149         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21150 
21151         delete[] Indices;
21152 
21153         // something is wrong with output of branchMPI!
21154         if (correct < 1){
21155                 cout << "something is wrong with output of branchMPI!\n";
21156                 vector<int> ret(1);
21157                 ret[0] = -1;
21158                 if (output != 0)  { delete[] output; output = 0; }
21159                 return ret;
21160         }
21161 
21162         // output is not nonsense, so now put it into a single dimension vector and return
21163         // 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
21164         // and the rest is the list of matches. output is one dimensional
21165 
21166         int output_size = 2 + output[1] * nParts;
21167         vector<int> ret(output_size);
21168         for (int i = 0; i < output_size; i++) {
21169                 ret[i]= output[i];
21170         }
21171         if (output != 0) { delete[] output; output = 0; }
21172         return ret;
21173 
21174 }
21175 
21176 
21177 int branch_all=0;
21178 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21179 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21180 
21181 //*************************************
21182 //testing search2
21183 if (1 == 0){
21184 cout <<"begin test search2\n";
21185 int* matchlist = new int[J*nParts];
21186 int* costlist = new int[J];
21187 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21188 Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
21189 
21190 for (int jit = 0; jit < J; jit++) {
21191   cout << *(costlist +jit)<<": ";
21192   for (int yit = 0; yit < nParts; yit++)
21193         cout << *(matchlist + jit*nParts + yit)<<",";
21194   cout <<"\n";  
21195 
21196 }
21197 cout <<"end test search2\n";
21198 int* output = new int[1];
21199 output[0] = 1;
21200 delete [] matchlist;
21201 delete [] costlist;
21202 return output;
21203 }
21204 //**************************************
21205 
21206         // Base Case: we're at a leaf, no more feasible matches possible
21207         if (curlevel > K -1){
21208                 int* output = new int[2];
21209                 output[0] = 0;
21210                 output[1] = 0;
21211                 return output;
21212         }
21213 
21214         // branch dynamically depending on results of search 2!
21215         
21216         int* matchlist = new int[J*nParts];
21217         int* costlist = new int[J];
21218         Util::search2(argParts, Indices, dimClasses, nParts, K,  T, matchlist, costlist, J);
21219         
21220         
21221         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
21222         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
21223 
21224         // if there are no feasible matches with cost gt T, then return 0
21225         for (int jit = 0; jit < J ; jit++){
21226         
21227                 if (costlist[jit] > T) break;
21228                 if (jit == J-1){
21229                         int* output = new int[2];
21230                         output[0] = 0;
21231                         output[1] = 0;
21232                         delete[] matchlist;
21233                         delete[] costlist;
21234                         return output;
21235                 }
21236         }
21237         
21238 
21239         
21240         // note that costlist and matchlist are NOT sorted by weight, and branch factor takes care of that...
21241         if (curlevel==0) branch_all = 0;
21242         
21243         int nBranches = -1;
21244 
21245         if (branchfunc == 0)
21246                 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
21247 
21248         if (branchfunc == 2)
21249                 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
21250 
21251         if (branchfunc == 3)
21252                 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
21253 
21254         if (branchfunc == 4)
21255                 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
21256 
21257         int* newcostlist= new int[nBranches];
21258         int* newmatchlist = new int[nBranches*nParts];
21259         for (int i=0; i<nBranches; i++){
21260                 newcostlist[i] = costlist[i];
21261                 for (int j=0; j< nParts; j++)
21262                         newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21263         }
21264 
21265         delete[] costlist;
21266         delete[] matchlist;
21267         
21268         //int* output = new int[2];//initialize to placeholder
21269         int* output = new int[2+K*nParts];//initialize to placeholder
21270         output[0] = 0;
21271         output[1] = 0;
21272         // some temporary variables
21273         int old_index;
21274         int totalcost;
21275         int nmatches;
21276         //int offset;
21277 
21278         for(int i=0; i < nBranches ; i++){
21279 
21280                 // consider the i-th match returned by findTopLargest
21281                 //if (newcostlist[i] <= T) continue;
21282 
21283                 // 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.
21284                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
21285 
21286                 for(int j=0; j < nParts; j++){
21287                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
21288                         old_index = newmatchlist[i*nParts + j];
21289                         argParts[Indices[j*K+old_index] + 1] = -2;
21290                 }
21291 
21292                 
21293                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21294                 J, max_branching, stmult,branchfunc, LIM);
21295                 
21296                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
21297                 totalcost = newcostlist[i] + ret[0];
21298 
21299                 //if (curlevel == 0) {
21300                 //      cout <<"totalcost*****************************************************************: "<<totalcost<<", costlist["<<i<<"]="<<newcostlist[i]<<", *ret="<<*ret<<", level: "<<curlevel<<"\n";
21301                         
21302                 //}
21303                 if (totalcost > output[0]) // option 1
21304                 {
21305                         nmatches = 1 + ret[1];
21306                         //delete[] output; // get rid of the old maxreturn
21307                         //output = new int[2+nmatches*nParts];
21308                         output[0] = totalcost;
21309                         output[1] = nmatches;
21310                         int nret = 2+(nmatches-1)*nParts;
21311                         for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21312                         for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21313                 }
21314 
21315 
21316                 delete[] ret;
21317 
21318                 // unmark the marked classes in preparation for the next iteration
21319 
21320                 for(int j=0; j < nParts; j++){
21321                         old_index = newmatchlist[i*nParts + j];
21322                         argParts[Indices[j*K+old_index] + 1] = 1;
21323                 }
21324 
21325         }
21326 
21327         delete[] newmatchlist;
21328         delete[] newcostlist;
21329         
21330         return output;
21331 }
21332 
21333 int* costlist_global;
21334 // make global costlist
21335 bool jiafunc(int i, int j){
21336         return (costlist_global[j] < costlist_global[i]) ;
21337 
21338 }
21339 // 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).
21340 // 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.
21341 // Branch on subsequent ones only if its infeasible with ALL the ones which we have previously decided to branch on.
21342 // 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.
21343 // 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.
21344 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21345         
21346         int ntot=0;
21347         for (int jit=0; jit < J; jit++){
21348                 if (*(costlist+jit) > T) ntot++;
21349         }
21350 
21351         int cur;
21352         // sort matchlist by cost
21353         int* indx = new int[J];
21354         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21355         vector<int> myindx (indx, indx+J);
21356         vector<int>::iterator it;
21357         costlist_global=costlist;
21358         sort(myindx.begin(), myindx.end(), jiafunc);
21359 
21360         // put matchlist in the order of mycost
21361         int* templist = new int[J];
21362         int* temp2list = new int[J*nParts];
21363         int next = 0;
21364         
21365         for (it=myindx.begin(); it!=myindx.end();++it){
21366                 cur = *(costlist + *it);
21367                 if (cur > T){
21368                         
21369                         templist[next] = cur;
21370                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21371                         next = next + 1;
21372                 }
21373         }
21374         
21375         for (int jit=0; jit < ntot; jit++){
21376                 *(costlist+jit)=*(templist + jit);
21377                 //cout <<*(costlist+jit)<<", ";
21378                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21379         }
21380         //cout <<"\n";
21381         
21382         delete [] indx;
21383         //compute the average 
21384         
21385         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21386         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21387         
21388         
21389         int B=1;
21390         int B_init=B;
21391         int infeasible=0;
21392         
21393         for (int i=B_init; i<ntot; i++){
21394                 if (i==ntot) continue;
21395                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21396                 // branch on
21397                 infeasible = 0;
21398                 if (LIM < 0) LIM = B;
21399                 for (int j=0; j<B; j++){
21400                         
21401                         for (int vit=0; vit<nParts; vit++){
21402                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21403                         }
21404                         if (infeasible >= LIM) break;
21405                 }
21406                 
21407                 if (infeasible >= LIM){
21408                         *(costlist+B)=*(templist+i);
21409                         for (int vit=0; vit < nParts; vit++)
21410                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21411                         B=B+1;  
21412                 }
21413         }
21414         
21415         delete [] templist;
21416         delete [] temp2list;
21417         //cout<<"**************************************** "<<B<<" ***************************\n";
21418         
21419         if (branch_all < max_branching){
21420                 if (B>1)
21421                         {branch_all = branch_all + B -1 ; }
21422         }
21423         else B=1;
21424         
21425         return B;
21426         
21427 
21428 }
21429 
21430 
21431 // 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.
21432 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21433         
21434         int ntot=0;
21435         for (int jit=0; jit < J; jit++){
21436                 if (*(costlist+jit) > T) ntot++;
21437         }
21438 
21439         int cur;
21440         // sort matchlist by cost
21441         int* indx = new int[J];
21442         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21443         vector<int> myindx (indx, indx+J);
21444         vector<int>::iterator it;
21445         costlist_global=costlist;
21446         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21447 
21448         // put matchlist in the order of mycost
21449         int* templist = new int[J];
21450         int* temp2list = new int[J*nParts];
21451         int next = 0;
21452         
21453         for (it=myindx.begin(); it!=myindx.end();++it){
21454                 cur = *(costlist + *it);
21455                 if (cur > T){
21456                         
21457                         templist[next] = cur;
21458                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21459                         next = next + 1;
21460                 }
21461         }
21462         
21463         for (int jit=0; jit < ntot; jit++){
21464                 *(costlist+jit)=*(templist + jit);
21465                 //cout <<*(costlist+jit)<<", ";
21466                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21467         }
21468         //cout <<"\n";
21469         
21470         delete [] indx;
21471         //compute the average 
21472         
21473         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21474         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21475         
21476         
21477         int B=1;
21478         int B_init=B;
21479         int infeasible=0;
21480         // if we're near the bottom of the tree then explore more... this is because the larger weights are not likely to change much,
21481         // whereas the smaller ones can have many permutations
21482         if (LIM < 0) LIM = ntot-1;
21483         for (int i=B_init; i<ntot; i++){
21484                 if (i==ntot) continue;
21485                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21486                 // branch on
21487                 infeasible = 0;
21488                 
21489                 for (int j=0; j<ntot; j++){
21490                         if (j == i) continue;
21491                         for (int vit=0; vit<nParts; vit++){
21492                                 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21493                         }
21494                         if (infeasible >= LIM) break;
21495                 }
21496                 
21497                 if (infeasible >= LIM){
21498                         *(costlist+B)=*(templist+i);
21499                         for (int vit=0; vit < nParts; vit++)
21500                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21501                         B=B+1;  
21502                 }
21503         }
21504         
21505         delete [] templist;
21506         delete [] temp2list;
21507         //cout<<"**************************************** "<<B<<" ***************************\n";
21508         
21509         
21510         if (branch_all < max_branching){
21511                 if (B>1)
21512                         {branch_all = branch_all + B-1;}
21513         }
21514         else B=1;
21515         
21516         return B;
21517         
21518 
21519 }
21520 
21521 // 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
21522 // match. Otherwise, we branch on similar weighted matches.
21523 // As before we always branch on the match with the largest cost so worst case we'll get greedy.
21524 // 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.
21525 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21526         int sum=0;
21527         float average =0;
21528         int ntot=0;
21529         for (int jit=0; jit < J; jit++){
21530                 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21531         }
21532         average = ((float)sum)/((float)ntot);
21533         int cur;
21534         // sort matchlist by cost
21535         int* indx = new int[J];
21536         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21537         vector<int> myindx (indx, indx+J);
21538         vector<int>::iterator it;
21539         costlist_global=costlist;
21540         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21541 
21542         // put matchlist in the order of mycost
21543         int* templist = new int[J];
21544         int* temp2list = new int[J*nParts];
21545         int next = 0;
21546         
21547         for (it=myindx.begin(); it!=myindx.end();++it){
21548                 cur = *(costlist + *it);
21549                 if (cur > T){
21550                         
21551                         templist[next] = cur;
21552                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21553                         next = next + 1;
21554                 }
21555         }
21556         
21557         for (int jit=0; jit < ntot; jit++){
21558                 *(costlist+jit)=*(templist + jit);
21559                 //cout <<*(costlist+jit)<<", ";
21560                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21561         }
21562         //cout <<"\n";
21563         
21564         delete [] indx;
21565         delete [] templist;
21566         delete [] temp2list;
21567         
21568         if (ntot == 1) return 1;
21569         
21570         // look at the average, standard dev etc. If standard dev very small, i.e., costs very similar, then branch on the similar
21571         // costs
21572         float sq_sum=0.0;
21573         //cout <<"costlist:";
21574         for (int i=0; i< ntot; i++){
21575                 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21576                 //cout <<*(costlist+i)<<", ";
21577         }       
21578         //cout <<"\n";
21579         
21580         float variance = sq_sum/ntot;
21581         float stdev = (float)pow((float)variance,(float)0.5);
21582         
21583         //cout <<"stdev: "<<int(stdev)<<"\n";
21584         
21585         int B=1;
21586         int largest = *costlist;
21587         //cout <<"largest: "<<largest<<"\n";
21588         for (int i=1; i<ntot; i++){
21589                 int cur = *(costlist+i);
21590                 if (largest-cur < (float)(stdev*stmult)) B++;
21591                 else break;
21592         
21593         }
21594         //cout <<"B: "<<B<<"\n";
21595         if (branch_all < max_branching){
21596                 if (B>1)
21597                         {branch_all = branch_all + B-1;}
21598         }
21599         else B=1;
21600         
21601         return B;
21602         
21603 
21604 }
21605 
21606 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21607         
21608         int ntot=0;
21609         for (int jit=0; jit < J; jit++){
21610                 if (*(costlist+jit) > T) ntot++;
21611         }
21612 
21613         int cur;
21614         // sort matchlist by cost
21615         int* indx = new int[J];
21616         for (int jit=0; jit < J; jit++) indx[jit]=jit;
21617         vector<int> myindx (indx, indx+J);
21618         vector<int>::iterator it;
21619         costlist_global=costlist;
21620         sort(myindx.begin(), myindx.begin()+J, jiafunc);
21621 
21622         // put matchlist in the order of mycost
21623         int* templist = new int[J];
21624         int* temp2list = new int[J*nParts];
21625         int next = 0;
21626         
21627         for (it=myindx.begin(); it!=myindx.end();++it){
21628                 cur = *(costlist + *it);
21629                 if (cur > T){
21630                         
21631                         templist[next] = cur;
21632                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21633                         next = next + 1;
21634                 }
21635         }
21636         
21637         for (int jit=0; jit < ntot; jit++){
21638                 *(costlist+jit)=*(templist + jit);
21639                 //cout <<*(costlist+jit)<<", ";
21640                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21641         }
21642         //cout <<"\n";
21643         
21644         for (int jit=1; jit < ntot; jit++){
21645         
21646              if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21647              
21648                      for (int vit=0; vit < nParts; vit++){
21649                              if ( matchlist[jit*nParts + vit] >  matchlist[vit])
21650                                  break;
21651                              if ( matchlist[jit*nParts + vit] ==  matchlist[vit])
21652                                  continue;
21653                              if ( matchlist[jit*nParts + vit] <  matchlist[vit])
21654                              {
21655                                  // swap
21656                                  for (int swp=0; swp < nParts; swp++){
21657                                        int tmp  = matchlist[swp];
21658                                        matchlist[swp]= matchlist[jit*nParts + swp];
21659                                        matchlist[jit*nParts + swp] = tmp;
21660                                  }
21661                                  break;
21662                              
21663                              }   
21664                      }
21665              }
21666         
21667         }
21668         
21669         
21670         delete [] indx;
21671         //compute the average 
21672         
21673         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
21674         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
21675         
21676         
21677         int B=1;
21678         int B_init=B;
21679         int infeasible=0;
21680         
21681         for (int i=B_init; i<ntot; i++){
21682                 if (i==ntot) continue;
21683                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
21684                 // branch on
21685                 infeasible = 0;
21686                 if (LIM < 0) LIM = B;
21687                 for (int j=0; j<B; j++){
21688                         
21689                         for (int vit=0; vit<nParts; vit++){
21690                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21691                         }
21692                         if (infeasible >= LIM) break;
21693                 }
21694                 
21695                 if (infeasible >= LIM){
21696                         *(costlist+B)=*(templist+i);
21697                         for (int vit=0; vit < nParts; vit++)
21698                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21699                         B=B+1;  
21700                 }
21701         }
21702         
21703         delete [] templist;
21704         delete [] temp2list;
21705         //cout<<"**************************************** "<<B<<" ***************************\n";
21706         
21707         if (branch_all < max_branching){
21708                 if (B>1)
21709                         {branch_all = branch_all + B -1 ; }
21710         }
21711         else B=1;
21712         
21713         return B;
21714         
21715 
21716 }

Generated on Thu May 3 10:06:29 2012 for EMAN2 by  doxygen 1.4.7