EMAN2
util_sparx.cpp
Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Pawel A.Penczek, 09/09/2006 (Pawel.A.Penczek@uth.tmc.edu)
00007  * Copyright (c) 2000-2006 The University of Texas - Houston Medical School
00008  *
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  *
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  *
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  *
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  *
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  *
00034  */
00035 #ifdef _WIN32
00036 #pragma warning(disable:4819)
00037 #include <malloc.h>
00038 #endif  //_WIN32
00039 
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <boost/shared_ptr.hpp>
00044 #include <cstdio>
00045 #include <cstdlib>
00046 #include <cassert>
00047 #include <sstream>
00048 #include "emdata.h"
00049 #include "util.h"
00050 #include "fundamentals.h"
00051 #include "lapackblas.h"
00052 #include "lbfgsb.h"
00053 using namespace EMAN;
00054 #include "steepest.h"
00055 #include "emassert.h"
00056 #include "randnum.h"
00057 #include "mcqd.h"
00058 
00059 #include <gsl/gsl_sf_bessel.h>
00060 #include <gsl/gsl_sf_bessel.h>
00061 #include <cmath>
00062 //#include <omp.h>
00063 using namespace std;
00064 using std::complex;
00065 
00066 /* Subroutine */ 
00067 int circum_(double *, double *, double *, double *, int *);
00068 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00069 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00070 
00071 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00072 //  flip true:  find statistics under the mask (mask >0.5)
00073 //  flip false: find statistics ourside the mask (mask <0.5)
00074 {
00075         ENTERFUNC;
00076         vector<float> stats;
00077         float *Volptr, *maskptr,MAX,MIN;
00078         long double Sum1,Sum2;
00079         long count;
00080 
00081         MAX = -FLT_MAX;
00082         MIN =  FLT_MAX;
00083         count = 0L;
00084         Sum1  = 0.0L;
00085         Sum2  = 0.0L;
00086 
00087         if (mask == NULL) {
00088                 //Vol->update_stat();
00089                 stats.push_back(Vol->get_attr("mean"));
00090                 stats.push_back(Vol->get_attr("sigma"));
00091                 stats.push_back(Vol->get_attr("minimum"));
00092                 stats.push_back(Vol->get_attr("maximum"));
00093                 return stats;
00094         }
00095 
00096         /* Check if the sizes of the mask and image are same */
00097 
00098         size_t nx = Vol->get_xsize();
00099         size_t ny = Vol->get_ysize();
00100         size_t nz = Vol->get_zsize();
00101 
00102         size_t mask_nx = mask->get_xsize();
00103         size_t mask_ny = mask->get_ysize();
00104         size_t mask_nz = mask->get_zsize();
00105 
00106         if  (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00107                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00108 
00109  /*       if (nx != mask_nx ||
00110             ny != mask_ny ||
00111             nz != mask_nz  ) {
00112            // should throw an exception here!!! (will clean it up later CY)
00113            fprintf(stderr, "The dimension of the image does not match the dimension of the mask!\n");
00114            fprintf(stderr, " nx = %d, mask_nx = %d\n", nx, mask_nx);
00115            fprintf(stderr, " ny = %d, mask_ny = %d\n", ny, mask_ny);
00116            fprintf(stderr, " nz = %d, mask_nz = %d\n", nz, mask_nz);
00117            exit(1);
00118         }
00119  */
00120         Volptr = Vol->get_data();
00121         maskptr = mask->get_data();
00122 
00123         for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00124                 if ((maskptr[i]>0.5f) == flip) {
00125                         Sum1 += Volptr[i];
00126                         Sum2 += Volptr[i]*double(Volptr[i]);
00127                         MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00128                         MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00129                         count++;
00130                 }
00131         }
00132 
00133         if (count == 0) {
00134                 LOGERR("Invalid mask");
00135                 throw ImageFormatException( "Invalid mask");
00136         }
00137 
00138         float avg = static_cast<float>(Sum1/count);
00139         float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00140 
00141         stats.push_back(avg);
00142         stats.push_back(sig);
00143         stats.push_back(MIN);
00144         stats.push_back(MAX);
00145 
00146         return stats;
00147 }
00148 
00149 
00150 vector<float> Util::helixshiftali(vector<EMData*> ctx, vector<vector<float> > pcoords, int nsegms, float maxincline, int kang, int search_rng, int nxc)
00151 {
00152         int cents, six, incline, kim, ixl, sib;
00153         float q0, qt, qu, tang, dst, xl, dxl, qm, bang;
00154         vector<float> result;
00155         cents = nsegms/2;
00156         qm = -1.0e23;  
00157         for ( six =-search_rng; six <= search_rng; six++ ) {
00158                 q0 = ctx[cents]->get_value_at(six+nxc);
00159                 for ( incline = 0; incline <= kang; incline++ ) {
00160                         qt = q0;
00161                         qu = q0;
00162                         if ( kang > 0 ) tang = tan(maxincline/kang*incline);
00163                         else tang = 0.0;
00164                         for ( kim = cents+1; kim < nsegms; kim++ ) {
00165                                 dst = sqrt((pcoords[cents][0] - pcoords[kim][0])*(pcoords[cents][0] - pcoords[kim][0])+
00166                                 (pcoords[cents][1] - pcoords[kim][1])*(pcoords[cents][1] - pcoords[kim][1]));
00167                                 xl = dst * tang + six + nxc;
00168                                 ixl = floor(xl);
00169                                 dxl = xl - ixl;
00170                                 qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);
00171                                 xl = -dst*tang+six+nxc;
00172                                 ixl = floor(xl);
00173                                 dxl = xl - ixl;
00174                                 qu += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);
00175                         }
00176                         for ( kim = 0; kim < cents; kim++ ) {
00177                                 dst = sqrt((pcoords[cents][0] - pcoords[kim][0])*(pcoords[cents][0] - pcoords[kim][0])+
00178                                 (pcoords[cents][1] - pcoords[kim][1])*(pcoords[cents][1] - pcoords[kim][1]));
00179                                 xl = -dst*tang+six+nxc;
00180                                 ixl = floor(xl);
00181                                 dxl = xl - ixl;
00182                                 qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);
00183                                 xl = dst * tang + six + nxc;
00184                                 ixl = floor(xl);
00185                                 dxl = xl - ixl;
00186                                 qu += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);
00187                         }
00188                         if ( qt > qm ) {
00189                                 qm = qt;
00190                                 sib = six;
00191                                 bang = tang;}
00192                         if ( qu > qm ) {
00193                                 qm = qu;
00194                                 sib = six;
00195                                 bang = -tang;}  
00196                 }
00197                 
00198         
00199         
00200         }
00201         //printf("\nqm sib bang=%f %f %f", qm, sib, bang);    
00202     result.push_back((float)sib); 
00203     result.push_back(bang);
00204     result.push_back(qm);
00205     return result;    
00206 }
00207 
00208 
00209 vector<float> Util::curhelixshiftali(vector<EMData*> ctx, vector<vector<float> > pcoords, int nsegms, int search_rng, int nx, int ny)
00210 {
00211         int cents, six, incline, kim, ixl, sib, circnum, circ, nxc, kang;
00212         float q0, qt, qu, tang, dst, xl, dxl, qm, bang, circd, circR, CircR, x1, x2;
00213         float dst0, dst1, dst2, xmax, xmin, maxincline, Rsinang, Rcosang, RSinang, RCosang;
00214         vector<float> result;
00215         
00216         nxc = nx/2;
00217         cents = nsegms/2;
00218         circnum = nxc;
00219         qm = -1.0e23; 
00220         dst1 = sqrt((pcoords[cents][0] - pcoords[0][0])*(pcoords[cents][0] - pcoords[0][0]) + (pcoords[cents][1] - pcoords[0][1])*(pcoords[cents][1] - pcoords[0][1]));
00221         dst2 = sqrt((pcoords[cents][0] - pcoords[nsegms-1][0])*(pcoords[cents][0] - pcoords[nsegms-1][0]) + (pcoords[cents][1] - pcoords[nsegms-1][1])*(pcoords[cents][1] - pcoords[nsegms-1][1]));
00222         
00223         for ( circ= 0; circ < circnum; circ++ ) {       
00224                 circd = 4*(circ+1.0);
00225                 circR = circd/2.0 + ((ny*nsegms)*(ny*nsegms))/(8.0*circd);
00226                 x1 = circR - sqrt(circR*circR-dst1*dst1);
00227                 x2 = circR - sqrt(circR*circR-dst2*dst2);
00228                 xmax = (x1>x2)?x1:x2;
00229                 if ( xmax == x1 ) dst0 = dst1;
00230                 else dst0 = dst2;
00231                                  
00232                 for ( six =-search_rng; six <= search_rng; six++ ) {
00233                         if (nx-(six+nxc+xmax)-2 <= 0) continue;
00234                         xmin = (nx-(six+nxc+xmax)-2<six+nxc-2)?(nx-(six+nxc+xmax)-2):(six+nxc-2);
00235                         maxincline = atan2(xmin,dst0);
00236                         kang = (int)(dst0*tan(maxincline)+0.5);
00237                         q0 = ctx[cents]->get_value_at(six+nxc);
00238                         for ( incline = -kang; incline <= kang; incline++ ) {
00239                                 qt = q0;
00240                                 //qu = q0;
00241                                 if ( kang > 0 ) {
00242                                         tang = tan(maxincline/kang*incline);
00243                                         Rsinang = circR*sin(maxincline/kang*incline);
00244                                         Rcosang = circR*cos(maxincline/kang*incline);}
00245                                 else {
00246                                         tang = 0.0;
00247                                         Rsinang = 0.0;
00248                                         Rcosang = 1.0;}
00249                                 for ( kim = cents+1; kim < nsegms; kim++ ) {
00250                                         dst = sqrt((pcoords[cents][0] - pcoords[kim][0])*(pcoords[cents][0] - pcoords[kim][0])+
00251                                         (pcoords[cents][1] - pcoords[kim][1])*(pcoords[cents][1] - pcoords[kim][1]));
00252                                         if ( tang >= 0 ) {                     //case 1 and 4.
00253                                                 xl = Rcosang - sqrt(Rcosang*Rcosang-2*dst*Rsinang-dst*dst)+six+nxc;
00254                                                 ixl = (int) xl;
00255                                                 dxl = xl - ixl;
00256                                                 qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);}
00257                                                 
00258                                         if ( tang < 0 ) {
00259                                                 if ( -dst*tang > circR - sqrt(circR*circR-dst*dst)) {                //## case 3
00260                                                         if ( Rcosang*Rcosang+2*dst*Rsinang-dst*dst <0 )
00261                                                         printf("Rcosang**2+2*dst*Rsinang-dst**2 less than 0 in curhelicalshiftali_MPI"); 
00262                                                         xl = -Rcosang + sqrt(Rcosang*Rcosang+2*dst*Rsinang-dst*dst)+six+nxc;
00263                                                         ixl = (int) xl;
00264                                                         dxl = xl - ixl;
00265                                                         if ( ixl < 0 ) printf("kim=%d, cents=%d nsegms=%d incline=%d ixl=%d", kim, cents,nsegms, incline,ixl); 
00266                                                         qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);}
00267                                                 else {                                           //## case 2
00268                                                         xl = Rcosang - sqrt(Rcosang*Rcosang+2*dst*Rsinang-dst*dst)+six+nxc;
00269                                                         ixl = (int) xl;
00270                                                         dxl = xl - ixl;
00271                                                         qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);}}
00272                                 }
00273                                 for ( kim = 0; kim < cents; kim++ ) {
00274                                         dst = sqrt((pcoords[cents][0] - pcoords[kim][0])*(pcoords[cents][0] - pcoords[kim][0])+
00275                                         (pcoords[cents][1] - pcoords[kim][1])*(pcoords[cents][1] - pcoords[kim][1]));
00276                                         if ( tang >= 0 ) {
00277                                                 if ( dst*tang > circR - sqrt(circR*circR-dst*dst)) {             
00278                                                         xl = -Rcosang + sqrt(Rcosang*Rcosang+2*dst*Rsinang-dst*dst)+six+nxc;
00279                                                         ixl = (int) xl;
00280                                                         dxl = xl - ixl;
00281                                                         qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);}
00282                                                 else {                      
00283                                                         xl = Rcosang - sqrt(Rcosang*Rcosang+2*dst*Rsinang-dst*dst)+six+nxc;
00284                                                         ixl = (int) xl;
00285                                                         dxl = xl - ixl;
00286                                                         qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);}}
00287                                         if ( tang < 0 ) {       
00288                                                 xl = Rcosang - sqrt(Rcosang*Rcosang-2*dst*Rsinang-dst*dst)+six+nxc;
00289                                                 ixl = (int) xl;
00290                                                 dxl = xl - ixl;
00291                                                 qt += (1.0-dxl)*ctx[kim]->get_value_at(ixl) + dxl*ctx[kim]->get_value_at(ixl+1);}
00292                                 }
00293                                 if ( qt > qm ) {
00294                                         qm = qt;
00295                                         sib = six;
00296                                         bang = tang;
00297                                         CircR = circR;
00298                                         RSinang = Rsinang;
00299                                         RCosang = Rcosang;}
00300                         }
00301                 
00302                 }
00303         }       
00304         //printf("\nqm sib bang=%f %f %f", qm, sib, bang);    
00305     result.push_back((float)sib); 
00306     result.push_back(bang);
00307     result.push_back(CircR);
00308     result.push_back(RSinang);
00309     result.push_back(RCosang);
00310     result.push_back(qm);
00311     return result;    
00312 }
00313 
00314 
00315 
00316 //----------------------------------------------------------------------------------------------------------
00317 
00318 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00319 {
00320         ENTERFUNC;
00321 
00322         if (!EMUtil::is_same_size(V1, V2)) {
00323                 LOGERR("images not same size");
00324                 throw ImageFormatException( "images not same size");
00325         }
00326 
00327         size_t nx = V1->get_xsize();
00328         size_t ny = V1->get_ysize();
00329         size_t nz = V1->get_zsize();
00330         size_t size = (size_t)nx*ny*nz;
00331 
00332         EMData *BD = new EMData();
00333         BD->set_size(nx, ny, nz);
00334 
00335         float *params = new float[2];
00336 
00337         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00338         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00339         int nvox = 0L;
00340 
00341         V1ptr = V1->get_data();
00342         V2ptr = V2->get_data();
00343         BDptr = BD->get_data();
00344 
00345 
00346         if(!mask){
00347                 EMData * Mask = new EMData();
00348                 Mask->set_size(nx,ny,nz);
00349                 Mask->to_one();
00350                 MASKptr = Mask->get_data();
00351         } else {
00352                 if (!EMUtil::is_same_size(V1, mask)) {
00353                         LOGERR("mask not same size");
00354                         throw ImageFormatException( "mask not same size");
00355                 }
00356 
00357                 MASKptr = mask->get_data();
00358         }
00359 
00360 
00361 
00362 //       calculation of S1,S2,S3,S3,nvox
00363 
00364         for (size_t i = 0L;i < size; i++) {
00365               if (MASKptr[i]>0.5f) {
00366                S1 += V1ptr[i]*V2ptr[i];
00367                S2 += V1ptr[i]*V1ptr[i];
00368                S3 += V2ptr[i];
00369                S4 += V1ptr[i];
00370                nvox ++;
00371               }
00372         }
00373 
00374         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00375                 A =1.0f ;
00376         } else {
00377                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00378         }
00379         B = static_cast<float> (A*S4  -  S3)/nvox;
00380 
00381         // calculation of the difference image
00382 
00383         for (size_t i = 0L;i < size; i++) {
00384              if (MASKptr[i]>0.5f) {
00385                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00386              }  else  {
00387                BDptr[i] = 0.f;
00388              }
00389         }
00390 
00391         BD->update();
00392 
00393         params[0] = A;
00394         params[1] = B;
00395 
00396         Dict BDnParams;
00397         BDnParams["imdiff"] = BD;
00398         BDnParams["A"] = params[0];
00399         BDnParams["B"] = params[1];
00400 
00401         EXITFUNC;
00402         return BDnParams;
00403  }
00404 
00405 //----------------------------------------------------------------------------------------------------------
00406 
00407 
00408 
00409 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00410 {
00411         ENTERFUNC;
00412         int Mid= (Size+1)/2;
00413 
00414         if (flag==0) { // This is the real function
00415                 EMData* ImBW = new EMData();
00416                 ImBW->set_size(Size,Size,1);
00417                 ImBW->to_zero();
00418 
00419                 float tempIm;
00420                 float x,y;
00421 
00422                 for (int ix=(1-Mid);  ix<Mid; ix++){
00423                         for (int iy=(1-Mid);  iy<Mid; iy++){
00424                                 x = (float)ix;
00425                                 y = (float)iy;
00426                         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)) );
00427                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00428                         }
00429                 }
00430                 ImBW->update();
00431                 ImBW->set_complex(false);
00432                 ImBW->set_ri(true);
00433 
00434                 return ImBW;
00435         }
00436         else if (flag==1) {  // This is the Fourier Transform
00437                 EMData* ImBWFFT = new EMData();
00438                 ImBWFFT ->set_size(2*Size,Size,1);
00439                 ImBWFFT ->to_zero();
00440 
00441                 float r,s;
00442 
00443                 for (int ir=(1-Mid);  ir<Mid; ir++){
00444                         for (int is=(1-Mid);  is<Mid; is++){
00445                                 r = (float)ir;
00446                                 s = (float)is;
00447                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00448                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00449                         }
00450                 }
00451                 ImBWFFT->update();
00452                 ImBWFFT->set_complex(true);
00453                 ImBWFFT->set_ri(true);
00454                 ImBWFFT->set_shuffled(true);
00455                 ImBWFFT->set_fftodd(true);
00456 
00457                 return ImBWFFT;
00458         }
00459         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00460                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00461                 float C=cos(alpha);
00462                 float S=sin(alpha);
00463                 float D= sqrt(S*S*b*b + C*C*a*a);
00464                 //float D2 = D*D;   PAP - to get rid of warning
00465 
00466                 float P = p * C *a*a/D ;
00467                 float Q = q * S *b*b/D ;
00468 
00469                 if (flag==2) {
00470                         EMData* pofalpha = new EMData();
00471                         pofalpha ->set_size(Size,1,1);
00472                         pofalpha ->to_zero();
00473 
00474                         float Norm0 =  D*(float)sqrt(2*pi);
00475                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00476                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00477                         float sD;
00478 
00479                         for (int is=(1-Mid);  is<Mid; is++){
00480                                 sD = is/D ;
00481                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00482                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00483                         }
00484                         pofalpha-> update();
00485                         pofalpha-> set_complex(false);
00486                         pofalpha-> set_ri(true);
00487 
00488                         return pofalpha;
00489                 }
00490                 if (flag==3) { // This is the projection in Fourier Space
00491                         float vD;
00492 
00493                         EMData* pofalphak = new EMData();
00494                         pofalphak ->set_size(2*Size,1,1);
00495                         pofalphak ->to_zero();
00496 
00497                         for (int iv=(1-Mid);  iv<Mid; iv++){
00498                                 vD = iv*D ;
00499                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00500                         }
00501                         pofalphak-> update();
00502                         pofalphak-> set_complex(false);
00503                         pofalphak-> set_ri(true);
00504 
00505                         return pofalphak;
00506                 }
00507         }
00508         else if (flag==4) {
00509                 cout <<" FH under construction";
00510                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00511                 EMData* TryFH= OutFT -> real2FH(4.0);
00512                 return TryFH;
00513         } else {
00514                 cout <<" flag must be 0,1,2,3, or 4";
00515         }
00516 
00517         EXITFUNC;
00518         return 0;
00519 }
00520 
00521 
00522 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00523 {
00524 
00525         float x0= x[0];
00526         float x1= x[1];
00527         float x2= x[2];
00528         float y0= y[0];
00529         float y1= y[1];
00530         float y2= y[2];
00531         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00532         float xn  = x[n];
00533         float xnm1= x[n-1];
00534         float xnm2= x[n-2];
00535         float yn  = y[n];
00536         float ynm1= y[n-1];
00537         float ynm2= y[n-2];
00538         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00539         float *y2d = new float[n];
00540         Util::spline(x,y,n,yp1,ypn,y2d);
00541         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00542         delete [] y2d;
00543         return;
00544 }
00545 
00546 
00547 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00548 {
00549         int i,k;
00550         float p, qn, sig, un, *u;
00551         u = new float[n-1];
00552 
00553         if (yp1 > .99e30){
00554                 y2[0]=u[0]=0.0;
00555         } else {
00556                 y2[0]=-.5f;
00557                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00558         }
00559 
00560         for (i=1; i < n-1; i++) {
00561                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00562                 p = sig*y2[i-1] + 2.0f;
00563                 y2[i]  = (sig-1.0f)/p;
00564                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00565                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00566         }
00567 
00568         if (ypn>.99e30){
00569                 qn=0; un=0;
00570         } else {
00571                 qn= .5f;
00572                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00573         }
00574         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00575         for (k=n-2; k>=0; k--){
00576                 y2[k]=y2[k]*y2[k+1]+u[k];
00577         }
00578         delete [] u;
00579 }
00580 
00581 
00582 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00583 {
00584         int klo, khi, k;
00585         float h, b, a;
00586 
00587 //      klo=0; // can try to put here
00588         for (int j=0; j<m;j++){
00589                 klo=0;
00590                 khi=n-1;
00591                 while (khi-klo >1) {
00592                         k=(khi+klo) >>1;
00593                         if  (xa[k]>xq[j]){ khi=k;}
00594                         else { klo=k;}
00595                 }
00596                 h=xa[khi]- xa[klo];
00597                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00598                 a =(xa[khi]-xq[j])/h;
00599                 b=(xq[j]-xa[klo])/h;
00600                 yq[j]=a*ya[klo] + b*ya[khi]
00601                         + ((a*a*a-a)*y2a[klo]
00602                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00603         }
00604 //      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]);
00605 }
00606 
00607 
00608 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00609                float *weightofkValsSorted, int Size, int *SizeReturned)
00610 {
00611         int iMax = (int) floor( (Size-1.0)/2 +.01);
00612         int CountMax = (iMax+2)*(iMax+1)/2;
00613         int Count=-1;
00614         float *kVals     = new float[CountMax];
00615         float *weightMat = new float[CountMax];
00616         int *PermMat     = new   int[CountMax];
00617         SizeReturned[0] = CountMax;
00618 
00619 //      printf("Aa \n");        fflush(stdout);
00620         for (int jkx=0; jkx< iMax+1; jkx++) {
00621                 for (int jky=0; jky< jkx+1; jky++) {
00622                         Count++;
00623                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00624                         weightMat[Count]=  1.0;
00625                         if (jkx!=0)  { weightMat[Count] *=2;}
00626                         if (jky!=0)  { weightMat[Count] *=2;}
00627                         if (jkx!=jky){ weightMat[Count] *=2;}
00628                         PermMat[Count]=Count+1;
00629                 }
00630         }
00631 
00632         int lkVals = Count+1;
00633 //      printf("Cc \n");fflush(stdout);
00634 
00635         sort_mat(&kVals[0],&kVals[Count],
00636              &PermMat[0],  &PermMat[Count]);  //PermMat is
00637                                 //also returned as well as kValsSorted
00638         fflush(stdout);
00639 
00640         int newInd;
00641 
00642         for (int iP=0; iP < lkVals ; iP++ ) {
00643                 newInd =  PermMat[iP];
00644                 PermMatTr[newInd-1] = iP+1;
00645         }
00646 
00647 //      printf("Ee \n"); fflush(stdout);
00648 
00649         int CountA=-1;
00650         int CountB=-1;
00651 
00652         while (CountB< (CountMax-1)) {
00653                 CountA++;
00654                 CountB++;
00655 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00656                 kValsSorted[CountA] = kVals[CountB] ;
00657                 if (CountB<(CountMax-1) ) {
00658                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00659                                 SizeReturned[0]--;
00660                                 for (int iP=0; iP < lkVals; iP++){
00661 //                                      printf("iP=%d \n", iP);fflush(stdout);
00662                                         if  (PermMatTr[iP]>CountA+1) {
00663                                                 PermMatTr[iP]--;
00664                                         }
00665                                 }
00666                                 CountB++;
00667                         }
00668                 }
00669         }
00670 
00671 
00672         for (int CountD=0; CountD < CountMax; CountD++) {
00673             newInd = PermMatTr[CountD];
00674             weightofkValsSorted[newInd-1] += weightMat[CountD];
00675         }
00676 
00677 }
00678 
00679 
00680 vector<float>
00681 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00682 {
00683         vector<float> angles;
00684         float psi = 0.0;
00685         if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00686                 t1 =  0.0f;
00687                 t2 = 90.0f;
00688         }
00689         if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00690                 p1 =   0.0f;
00691                 p2 = 359.9f;
00692         }
00693         bool skip = ((t1 < 90.0) && (90.0 == t2) && (0.0 == p1) && (p2 > 180.0));
00694         for (float theta = t1; theta <= t2; theta += delta) {
00695                 float detphi;
00696                 int lt;
00697                 if ((0.0 == theta)||(180.0 == theta)) {
00698                         detphi = 360.0f;
00699                         lt = 1;
00700                 } else {
00701                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00702                         lt = int((p2 - p1)/detphi)-1;
00703                         if (lt < 1) lt = 1;
00704                         detphi = (p2 - p1)/lt;
00705                 }
00706                 for (int i = 0; i < lt; i++) {
00707                         float phi = p1 + i*detphi;
00708                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00709                         angles.push_back(phi);
00710                         angles.push_back(theta);
00711                         angles.push_back(psi);
00712                 }
00713         }
00714         return angles;
00715 }
00716 
00717 
00718 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00719 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00720 {
00721 
00722 //  purpose: quadratic interpolation
00723 //
00724 //  parameters:       xx,yy treated as circularly closed.
00725 //                    fdata - image 1..nxdata, 1..nydata
00726 //
00727 //                    f3    fc       f0, f1, f2, f3 are the values
00728 //                     +             at the grid points.  x is the
00729 //                     + x           point at which the function
00730 //              f2++++f0++++f1       is to be estimated. (it need
00731 //                     +             not be in the first quadrant).
00732 //                     +             fc - the outer corner point
00733 //                    f4             nearest x.
00734 c
00735 //                                   f0 is the value of the fdata at
00736 //                                   fdata(i,j), it is the interior mesh
00737 //                                   point nearest  x.
00738 //                                   the coordinates of f0 are (x0,y0),
00739 //                                   the coordinates of f1 are (xb,y0),
00740 //                                   the coordinates of f2 are (xa,y0),
00741 //                                   the coordinates of f3 are (x0,yb),
00742 //                                   the coordinates of f4 are (x0,ya),
00743 //                                   the coordinates of fc are (xc,yc),
00744 c
00745 //                   o               hxa, hxb are the mesh spacings
00746 //                   +               in the x-direction to the left
00747 //                  hyb              and right of the center point.
00748 //                   +
00749 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00750 //                   +               in the y-direction.
00751 //                  hya
00752 //                   +               hxc equals either  hxb  or  hxa
00753 //                   o               depending on where the corner
00754 //                                   point is located.
00755 c
00756 //                                   construct the interpolant
00757 //                                   f = f0 + c1*(x-x0) +
00758 //                                       c2*(x-x0)*(x-x1) +
00759 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00760 //                                       + c5*(x-x0)*(y-y0)
00761 //
00762 //
00763 
00764     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00765     float quadri;
00766     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00767 
00768     x = xx;
00769     y = yy;
00770 
00771     // circular closure
00772         while ( x < 1.0 ) x += nxdata;
00773         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00774         while ( y < 1.0 ) y += nydata;
00775         while ( y >= (float)(nydata+1) )  y -= nydata;
00776 
00777 
00778     i   = (int) x;
00779     j   = (int) y;
00780 
00781     dx0 = x - i;
00782     dy0 = y - j;
00783 
00784     ip1 = i + 1;
00785     im1 = i - 1;
00786     jp1 = j + 1;
00787     jm1 = j - 1;
00788 
00789     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00790     if (im1 < 1)      im1 = im1 + nxdata;
00791     if (jp1 > nydata) jp1 = jp1 - nydata;
00792     if (jm1 < 1)      jm1 = jm1 + nydata;
00793 
00794     f0  = fdata(i,j);
00795     c1  = fdata(ip1,j) - f0;
00796     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00797     c3  = fdata(i,jp1) - f0;
00798     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00799 
00800     dxb = dx0 - 1;
00801     dyb = dy0 - 1;
00802 
00803     // hxc & hyc are either 1 or -1
00804     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00805     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00806 
00807     ic  = i + hxc;
00808     jc  = j + hyc;
00809 
00810     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00811     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00812 
00813     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00814             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00815 
00816     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00817 
00818     return quadri;
00819 }*/
00820 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00821 {
00822 //  purpose: quadratic interpolation
00823 //  Optimized for speed, circular closer removed, checking of ranges removed
00824         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00825         float  quadri;
00826         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00827 
00828         x = xx;
00829         y = yy;
00830 
00831         //     any xx and yy
00832         while ( x < 1.0 )                 x += nxdata;
00833         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00834         while ( y < 1.0 )                 y += nydata;
00835         while ( y >= (float)(nydata+1) )  y -= nydata;
00836 
00837         i   = (int) x;
00838         j   = (int) y;
00839 
00840         dx0 = x - i;
00841         dy0 = y - j;
00842 
00843         ip1 = i + 1;
00844         im1 = i - 1;
00845         jp1 = j + 1;
00846         jm1 = j - 1;
00847 
00848         if (ip1 > nxdata) ip1 -= nxdata;
00849         if (im1 < 1)      im1 += nxdata;
00850         if (jp1 > nydata) jp1 -= nydata;
00851         if (jm1 < 1)      jm1 += nydata;
00852 
00853         f0  = fdata(i,j);
00854         c1  = fdata(ip1,j) - f0;
00855         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00856         c3  = fdata(i,jp1) - f0;
00857         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00858 
00859         dxb = dx0 - 1;
00860         dyb = dy0 - 1;
00861 
00862         // hxc & hyc are either 1 or -1
00863         if (dx0 >= 0) hxc = 1; else hxc = -1;
00864         if (dy0 >= 0) hyc = 1; else hyc = -1;
00865 
00866         ic  = i + hxc;
00867         jc  = j + hyc;
00868 
00869         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00870         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00871 
00872         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00873                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00874 
00875 
00876         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00877 
00878         return quadri;
00879 }
00880 
00881 #undef fdata
00882 
00883 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00884 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00885 {
00886 //  purpose: quadratic interpolation
00887 //  Optimized for speed, circular closer removed, checking of ranges removed
00888         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00889         float  quadri;
00890         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00891 
00892         x = xx;
00893         y = yy;
00894 
00895         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00896         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00897               x = (float)xnew;
00898                   y = (float)ynew;
00899      }
00900 
00901 
00902         i   = (int) x;
00903         j   = (int) y;
00904 
00905         dx0 = x - i;
00906         dy0 = y - j;
00907 
00908         ip1 = i + 1;
00909         im1 = i - 1;
00910         jp1 = j + 1;
00911         jm1 = j - 1;
00912 
00913         if (ip1 > nxdata) ip1 -= nxdata;
00914         if (im1 < 1)      im1 += nxdata;
00915         if (jp1 > nydata) jp1 -= nydata;
00916         if (jm1 < 1)      jm1 += nydata;
00917 
00918         f0  = fdata(i,j);
00919         c1  = fdata(ip1,j) - f0;
00920         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00921         c3  = fdata(i,jp1) - f0;
00922         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00923 
00924         dxb = dx0 - 1;
00925         dyb = dy0 - 1;
00926 
00927         // hxc & hyc are either 1 or -1
00928         if (dx0 >= 0) hxc = 1; else hxc = -1;
00929         if (dy0 >= 0) hyc = 1; else hyc = -1;
00930 
00931         ic  = i + hxc;
00932         jc  = j + hyc;
00933 
00934         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00935         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00936 
00937         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00938                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00939 
00940 
00941         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00942 
00943         return quadri;
00944 }
00945 
00946 #undef fdata
00947 
00948 
00949 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00950         int K = kb.get_window_size();
00951         int kbmin = -K/2;
00952         int kbmax = -kbmin;
00953         int kbc = kbmax+1;
00954 
00955         float pixel =0.0f;
00956         float w=0.0f;
00957 
00958         delx = restrict1(delx, nx);
00959         int inxold = int(round(delx));
00960         if ( ny < 2 ) {  //1D
00961                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00962                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00963                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00964                 float tablex4 = kb.i0win_tab(delx-inxold);
00965                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00966                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00967                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00968 
00969                 int x1, x2, x3, x4, x5, x6, x7;
00970 
00971                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00972                         x1 = (inxold-3+nx)%nx;
00973                         x2 = (inxold-2+nx)%nx;
00974                         x3 = (inxold-1+nx)%nx;
00975                         x4 = (inxold  +nx)%nx;
00976                         x5 = (inxold+1+nx)%nx;
00977                         x6 = (inxold+2+nx)%nx;
00978                         x7 = (inxold+3+nx)%nx;
00979                 } else {
00980                         x1 = inxold-3;
00981                         x2 = inxold-2;
00982                         x3 = inxold-1;
00983                         x4 = inxold;
00984                         x5 = inxold+1;
00985                         x6 = inxold+2;
00986                         x7 = inxold+3;
00987                 }
00988 
00989                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00990                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00991                         data[x7]*tablex7 ;
00992 
00993                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00994         } else if ( nz < 2 ) {  // 2D
00995                 dely = restrict1(dely, ny);
00996                 int inyold = int(round(dely));
00997                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00998                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00999                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01000                 float tablex4 = kb.i0win_tab(delx-inxold);
01001                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01002                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01003                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01004 
01005                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01006                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01007                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01008                 float tabley4 = kb.i0win_tab(dely-inyold);
01009                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01010                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01011                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01012 
01013                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01014 
01015                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01016                         x1 = (inxold-3+nx)%nx;
01017                         x2 = (inxold-2+nx)%nx;
01018                         x3 = (inxold-1+nx)%nx;
01019                         x4 = (inxold  +nx)%nx;
01020                         x5 = (inxold+1+nx)%nx;
01021                         x6 = (inxold+2+nx)%nx;
01022                         x7 = (inxold+3+nx)%nx;
01023 
01024                         y1 = ((inyold-3+ny)%ny)*nx;
01025                         y2 = ((inyold-2+ny)%ny)*nx;
01026                         y3 = ((inyold-1+ny)%ny)*nx;
01027                         y4 = ((inyold  +ny)%ny)*nx;
01028                         y5 = ((inyold+1+ny)%ny)*nx;
01029                         y6 = ((inyold+2+ny)%ny)*nx;
01030                         y7 = ((inyold+3+ny)%ny)*nx;
01031                 } else {
01032                         x1 = inxold-3;
01033                         x2 = inxold-2;
01034                         x3 = inxold-1;
01035                         x4 = inxold;
01036                         x5 = inxold+1;
01037                         x6 = inxold+2;
01038                         x7 = inxold+3;
01039 
01040                         y1 = (inyold-3)*nx;
01041                         y2 = (inyold-2)*nx;
01042                         y3 = (inyold-1)*nx;
01043                         y4 = inyold*nx;
01044                         y5 = (inyold+1)*nx;
01045                         y6 = (inyold+2)*nx;
01046                         y7 = (inyold+3)*nx;
01047                 }
01048 
01049                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01050                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01051                              data[x7+y1]*tablex7 ) * tabley1 +
01052                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01053                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01054                              data[x7+y2]*tablex7 ) * tabley2 +
01055                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01056                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01057                              data[x7+y3]*tablex7 ) * tabley3 +
01058                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01059                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01060                              data[x7+y4]*tablex7 ) * tabley4 +
01061                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01062                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01063                              data[x7+y5]*tablex7 ) * tabley5 +
01064                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01065                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01066                              data[x7+y6]*tablex7 ) * tabley6 +
01067                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01068                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01069                              data[x7+y7]*tablex7 ) * tabley7;
01070 
01071                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01072                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01073         } else {  //  3D
01074                 dely = restrict1(dely, ny);
01075                 int inyold = int(Util::round(dely));
01076                 delz = restrict1(delz, nz);
01077                 int inzold = int(Util::round(delz));
01078 
01079                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01080                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01081                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01082                 float tablex4 = kb.i0win_tab(delx-inxold);
01083                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01084                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01085                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01086 
01087                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01088                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01089                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01090                 float tabley4 = kb.i0win_tab(dely-inyold);
01091                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01092                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01093                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01094 
01095                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01096                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01097                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01098                 float tablez4 = kb.i0win_tab(delz-inzold);
01099                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01100                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01101                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01102 
01103                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01104 
01105                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01106                         x1 = (inxold-3+nx)%nx;
01107                         x2 = (inxold-2+nx)%nx;
01108                         x3 = (inxold-1+nx)%nx;
01109                         x4 = (inxold  +nx)%nx;
01110                         x5 = (inxold+1+nx)%nx;
01111                         x6 = (inxold+2+nx)%nx;
01112                         x7 = (inxold+3+nx)%nx;
01113 
01114                         y1 = ((inyold-3+ny)%ny)*nx;
01115                         y2 = ((inyold-2+ny)%ny)*nx;
01116                         y3 = ((inyold-1+ny)%ny)*nx;
01117                         y4 = ((inyold  +ny)%ny)*nx;
01118                         y5 = ((inyold+1+ny)%ny)*nx;
01119                         y6 = ((inyold+2+ny)%ny)*nx;
01120                         y7 = ((inyold+3+ny)%ny)*nx;
01121 
01122                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01123                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01124                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01125                         z4 = ((inzold  +nz)%nz)*nx*ny;
01126                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01127                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01128                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01129                 } else {
01130                         x1 = inxold-3;
01131                         x2 = inxold-2;
01132                         x3 = inxold-1;
01133                         x4 = inxold;
01134                         x5 = inxold+1;
01135                         x6 = inxold+2;
01136                         x7 = inxold+3;
01137 
01138                         y1 = (inyold-3)*nx;
01139                         y2 = (inyold-2)*nx;
01140                         y3 = (inyold-1)*nx;
01141                         y4 = inyold*nx;
01142                         y5 = (inyold+1)*nx;
01143                         y6 = (inyold+2)*nx;
01144                         y7 = (inyold+3)*nx;
01145 
01146                         z1 = (inzold-3)*nx*ny;
01147                         z2 = (inzold-2)*nx*ny;
01148                         z3 = (inzold-1)*nx*ny;
01149                         z4 = inzold*nx*ny;
01150                         z5 = (inzold+1)*nx*ny;
01151                         z6 = (inzold+2)*nx*ny;
01152                         z7 = (inzold+3)*nx*ny;
01153                 }
01154 
01155                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01156                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01157                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01158                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01159                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01160                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01161                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01162                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01163                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01164                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01165                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01166                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01167                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01168                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01169                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01170                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01171                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01172                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01173                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01174                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01175                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01176                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01177                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01178                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01179                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01180                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01181                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01182                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01183                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01184                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01185                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01186                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01187                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01188                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01189                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01190                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01191                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01192                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01193                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01194                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01195                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01196                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01197                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01198                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01199                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01200                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01201                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01202                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01203                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01204                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01205                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01206                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01207                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01208                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01209                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01210                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01211                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01212                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01213                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01214                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01215                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01216                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01217                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01218                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01219                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01220                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01221                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01222                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01223                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01224                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01225                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01226                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01227                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01228                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01229                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01230                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01231                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01232                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01233                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01234                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01235                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01236                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01237                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01238                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01239                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01240                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01241                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01242                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01243                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01244                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01245                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01246                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01247                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01248                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01249                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01250                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01251                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01252                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01253                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01254                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01255                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01256                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01257                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01258                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01259                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01260                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01261                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01262                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01263                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01264                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01265                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01266                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01267                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01268                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01269                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01270                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01271                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01272                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01273                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01274                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01275                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01276                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01277                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01278                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01279                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01280                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01281                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01282                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01283                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01284                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01285                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01286                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01287                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01288                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01289                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01290                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01291                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01292                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01293                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01294                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01295                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01296                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01297                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01298                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01299                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01300                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01301                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01302 
01303                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01304                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01305                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01306         }
01307         return pixel/w;
01308 }
01309 
01310 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) {
01311         int K = kb.get_window_size();
01312         int kbmin = -K/2;
01313         int kbmax = -kbmin;
01314         int kbc = kbmax+1;
01315 
01316         float pixel =0.0f;
01317         float w=0.0f;
01318 
01319     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01320         delx = restrict1(delx, nx);
01321         int inxold = int(round(delx));
01322         if ( ny < 2 ) {  //1D
01323                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01324                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01325                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01326                 float tablex4 = kb.i0win_tab(delx-inxold);
01327                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01328                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01329                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01330 
01331                 int x1, x2, x3, x4, x5, x6, x7;
01332 
01333                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01334                         x1 = (inxold-3+nx)%nx;
01335                         x2 = (inxold-2+nx)%nx;
01336                         x3 = (inxold-1+nx)%nx;
01337                         x4 = (inxold  +nx)%nx;
01338                         x5 = (inxold+1+nx)%nx;
01339                         x6 = (inxold+2+nx)%nx;
01340                         x7 = (inxold+3+nx)%nx;
01341                 } else {
01342                         x1 = inxold-3;
01343                         x2 = inxold-2;
01344                         x3 = inxold-1;
01345                         x4 = inxold;
01346                         x5 = inxold+1;
01347                         x6 = inxold+2;
01348                         x7 = inxold+3;
01349                 }
01350 
01351                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01352                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01353                         data[x7]*tablex7 ;
01354 
01355                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01356         } else if ( nz < 2 ) {  // 2D
01357 
01358                 delx = argdelx;
01359                 // 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
01360                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01361                 delx = (float)xnew*2.0f;
01362                 dely = (float)ynew*2.0f;
01363                 }
01364 
01365                 int inxold = int(round(delx));
01366                 int inyold = int(round(dely));
01367 
01368                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01369                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01370                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01371                 float tablex4 = kb.i0win_tab(delx-inxold);
01372                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01373                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01374                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01375 
01376                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01377                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01378                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01379                 float tabley4 = kb.i0win_tab(dely-inyold);
01380                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01381                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01382                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01383 
01384                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01385 
01386                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01387                         x1 = (inxold-3+nx)%nx;
01388                         x2 = (inxold-2+nx)%nx;
01389                         x3 = (inxold-1+nx)%nx;
01390                         x4 = (inxold  +nx)%nx;
01391                         x5 = (inxold+1+nx)%nx;
01392                         x6 = (inxold+2+nx)%nx;
01393                         x7 = (inxold+3+nx)%nx;
01394 
01395                         y1 = ((inyold-3+ny)%ny)*nx;
01396                         y2 = ((inyold-2+ny)%ny)*nx;
01397                         y3 = ((inyold-1+ny)%ny)*nx;
01398                         y4 = ((inyold  +ny)%ny)*nx;
01399                         y5 = ((inyold+1+ny)%ny)*nx;
01400                         y6 = ((inyold+2+ny)%ny)*nx;
01401                         y7 = ((inyold+3+ny)%ny)*nx;
01402                 } else {
01403                         x1 = inxold-3;
01404                         x2 = inxold-2;
01405                         x3 = inxold-1;
01406                         x4 = inxold;
01407                         x5 = inxold+1;
01408                         x6 = inxold+2;
01409                         x7 = inxold+3;
01410 
01411                         y1 = (inyold-3)*nx;
01412                         y2 = (inyold-2)*nx;
01413                         y3 = (inyold-1)*nx;
01414                         y4 = inyold*nx;
01415                         y5 = (inyold+1)*nx;
01416                         y6 = (inyold+2)*nx;
01417                         y7 = (inyold+3)*nx;
01418                 }
01419 
01420                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01421                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01422                              data[x7+y1]*tablex7 ) * tabley1 +
01423                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01424                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01425                              data[x7+y2]*tablex7 ) * tabley2 +
01426                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01427                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01428                              data[x7+y3]*tablex7 ) * tabley3 +
01429                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01430                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01431                              data[x7+y4]*tablex7 ) * tabley4 +
01432                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01433                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01434                              data[x7+y5]*tablex7 ) * tabley5 +
01435                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01436                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01437                              data[x7+y6]*tablex7 ) * tabley6 +
01438                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01439                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01440                              data[x7+y7]*tablex7 ) * tabley7;
01441 
01442                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01443                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01444         } else {  //  3D
01445                 dely = restrict1(dely, ny);
01446                 int inyold = int(Util::round(dely));
01447                 delz = restrict1(delz, nz);
01448                 int inzold = int(Util::round(delz));
01449 
01450                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01451                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01452                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01453                 float tablex4 = kb.i0win_tab(delx-inxold);
01454                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01455                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01456                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01457 
01458                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01459                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01460                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01461                 float tabley4 = kb.i0win_tab(dely-inyold);
01462                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01463                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01464                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01465 
01466                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01467                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01468                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01469                 float tablez4 = kb.i0win_tab(delz-inzold);
01470                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01471                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01472                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01473 
01474                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01475 
01476                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01477                         x1 = (inxold-3+nx)%nx;
01478                         x2 = (inxold-2+nx)%nx;
01479                         x3 = (inxold-1+nx)%nx;
01480                         x4 = (inxold  +nx)%nx;
01481                         x5 = (inxold+1+nx)%nx;
01482                         x6 = (inxold+2+nx)%nx;
01483                         x7 = (inxold+3+nx)%nx;
01484 
01485                         y1 = ((inyold-3+ny)%ny)*nx;
01486                         y2 = ((inyold-2+ny)%ny)*nx;
01487                         y3 = ((inyold-1+ny)%ny)*nx;
01488                         y4 = ((inyold  +ny)%ny)*nx;
01489                         y5 = ((inyold+1+ny)%ny)*nx;
01490                         y6 = ((inyold+2+ny)%ny)*nx;
01491                         y7 = ((inyold+3+ny)%ny)*nx;
01492 
01493                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01494                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01495                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01496                         z4 = ((inzold  +nz)%nz)*nx*ny;
01497                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01498                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01499                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01500                 } else {
01501                         x1 = inxold-3;
01502                         x2 = inxold-2;
01503                         x3 = inxold-1;
01504                         x4 = inxold;
01505                         x5 = inxold+1;
01506                         x6 = inxold+2;
01507                         x7 = inxold+3;
01508 
01509                         y1 = (inyold-3)*nx;
01510                         y2 = (inyold-2)*nx;
01511                         y3 = (inyold-1)*nx;
01512                         y4 = inyold*nx;
01513                         y5 = (inyold+1)*nx;
01514                         y6 = (inyold+2)*nx;
01515                         y7 = (inyold+3)*nx;
01516 
01517                         z1 = (inzold-3)*nx*ny;
01518                         z2 = (inzold-2)*nx*ny;
01519                         z3 = (inzold-1)*nx*ny;
01520                         z4 = inzold*nx*ny;
01521                         z5 = (inzold+1)*nx*ny;
01522                         z6 = (inzold+2)*nx*ny;
01523                         z7 = (inzold+3)*nx*ny;
01524                 }
01525 
01526                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01527                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01528                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01529                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01530                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01531                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01532                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01533                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01534                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01535                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01536                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01537                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01538                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01539                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01540                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01541                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01542                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01543                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01544                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01545                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01546                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01547                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01548                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01549                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01550                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01551                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01552                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01553                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01554                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01555                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01556                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01557                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01558                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01559                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01560                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01561                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01562                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01563                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01564                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01565                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01566                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01567                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01568                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01569                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01570                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01571                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01572                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01573                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01574                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01575                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01576                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01577                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01578                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01579                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01580                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01581                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01582                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01583                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01584                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01585                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01586                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01587                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01588                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01589                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01590                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01591                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01592                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01593                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01594                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01595                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01596                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01597                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01598                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01599                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01600                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01601                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01602                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01603                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01604                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01605                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01606                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01607                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01608                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01609                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01610                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01611                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01612                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01613                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01614                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01615                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01616                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01617                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01618                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01619                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01620                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01621                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01622                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01623                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01624                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01625                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01626                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01627                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01628                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01629                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01630                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01631                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01632                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01633                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01634                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01635                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01636                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01637                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01638                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01639                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01640                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01641                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01642                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01643                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01644                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01645                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01646                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01647                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01648                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01649                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01650                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01651                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01652                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01653                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01654                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01655                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01656                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01657                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01658                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01659                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01660                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01661                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01662                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01663                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01664                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01665                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01666                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01667                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01668                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01669                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01670                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01671                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01672                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01673 
01674                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01675                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01676                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01677         }
01678         return pixel/w;
01679 }
01680 
01681 /*
01682 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01683 
01684         int nxreal = nx - 2;
01685         if (nxreal != ny)
01686                 throw ImageDimensionException("extractpoint requires ny == nx");
01687         int nhalf = nxreal/2;
01688         int kbsize = kb.get_window_size();
01689         int kbmin = -kbsize/2;
01690         int kbmax = -kbmin;
01691         bool flip = (nuxnew < 0.f);
01692         if (flip) {
01693                 nuxnew *= -1;
01694                 nuynew *= -1;
01695         }
01696         // put (xnew,ynew) on a grid.  The indices will be wrong for
01697         // the Fourier elements in the image, but the grid sizing will
01698         // be correct.
01699         int ixn = int(Util::round(nuxnew));
01700         int iyn = int(Util::round(nuynew));
01701         // set up some temporary weighting arrays
01702         float* wy0 = new float[kbmax - kbmin + 1];
01703         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01704         float* wx0 = new float[kbmax - kbmin + 1];
01705         float* wx = wx0 - kbmin;
01706         for (int i = kbmin; i <= kbmax; i++) {
01707                         int iyp = iyn + i;
01708                         wy[i] = kb.i0win_tab(nuynew - iyp);
01709                         int ixp = ixn + i;
01710                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01711         }
01712         // restrict loops to non-zero elements
01713         int iymin = 0;
01714         for (int iy = kbmin; iy <= -1; iy++) {
01715                 if (wy[iy] != 0.f) {
01716                         iymin = iy;
01717                         break;
01718                 }
01719         }
01720         int iymax = 0;
01721         for (int iy = kbmax; iy >= 1; iy--) {
01722                 if (wy[iy] != 0.f) {
01723                         iymax = iy;
01724                         break;
01725                 }
01726         }
01727         int ixmin = 0;
01728         for (int ix = kbmin; ix <= -1; ix++) {
01729                 if (wx[ix] != 0.f) {
01730                         ixmin = ix;
01731                         break;
01732                 }
01733         }
01734         int ixmax = 0;
01735         for (int ix = kbmax; ix >= 1; ix--) {
01736                 if (wx[ix] != 0.f) {
01737                         ixmax = ix;
01738                         break;
01739                 }
01740         }
01741         float wsum = 0.0f;
01742         for (int iy = iymin; iy <= iymax; iy++)
01743                 for (int ix = ixmin; ix <= ixmax; ix++)
01744                         wsum += wx[ix]*wy[iy];
01745 
01746         complex<float> result(0.f,0.f);
01747         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01748                 // (xin,yin) not within window border from the edge
01749                 for (int iy = iymin; iy <= iymax; iy++) {
01750                         int iyp = iyn + iy;
01751                         for (int ix = ixmin; ix <= ixmax; ix++) {
01752                                 int ixp = ixn + ix;
01753                                 float w = wx[ix]*wy[iy];
01754                                 complex<float> val = fimage->cmplx(ixp,iyp);
01755                                 result += val*w;
01756                         }
01757                 }
01758         } else {
01759                 // points that "stick out"
01760                 for (int iy = iymin; iy <= iymax; iy++) {
01761                         int iyp = iyn + iy;
01762                         for (int ix = ixmin; ix <= ixmax; ix++) {
01763                                 int ixp = ixn + ix;
01764                                 bool mirror = false;
01765                                 int ixt= ixp, iyt= iyp;
01766                                 if (ixt < 0) {
01767                                         ixt = -ixt;
01768                                         iyt = -iyt;
01769                                         mirror = !mirror;
01770                                 }
01771                                 if (ixt > nhalf) {
01772                                         ixt = nxreal - ixt;
01773                                         iyt = -iyt;
01774                                         mirror = !mirror;
01775                                 }
01776                                 if (iyt > nhalf-1)  iyt -= nxreal;
01777                                 if (iyt < -nhalf)   iyt += nxreal;
01778                                 float w = wx[ix]*wy[iy];
01779                                 complex<float> val = fimage->cmplx(ixt,iyt);
01780                                 if (mirror)  result += conj(val)*w;
01781                                 else         result += val*w;
01782                         }
01783                 }
01784         }
01785         if (flip)  result = conj(result)/wsum;
01786         else result /= wsum;
01787         delete [] wx0;
01788         delete [] wy0;
01789         return result;
01790 }*/
01791 
01792 /*
01793 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01794 
01795         int nxreal = nx - 2;
01796         if (nxreal != ny)
01797                 throw ImageDimensionException("extractpoint requires ny == nx");
01798         int nhalf = nxreal/2;
01799         bool flip = false;
01800         if (nuxnew < 0.f) {
01801                 nuxnew *= -1;
01802                 nuynew *= -1;
01803                 flip = true;
01804         }
01805         if (nuynew >= nhalf-0.5)  {
01806                 nuynew -= nxreal;
01807         } else if (nuynew < -nhalf-0.5) {
01808                 nuynew += nxreal;
01809         }
01810 
01811         // put (xnew,ynew) on a grid.  The indices will be wrong for
01812         // the Fourier elements in the image, but the grid sizing will
01813         // be correct.
01814         int ixn = int(Util::round(nuxnew));
01815         int iyn = int(Util::round(nuynew));
01816 
01817         // set up some temporary weighting arrays
01818         static float wy[7];
01819         static float wx[7];
01820 
01821         float iynn = nuynew - iyn;
01822         wy[0] = kb.i0win_tab(iynn+3);
01823         wy[1] = kb.i0win_tab(iynn+2);
01824         wy[2] = kb.i0win_tab(iynn+1);
01825         wy[3] = kb.i0win_tab(iynn);
01826         wy[4] = kb.i0win_tab(iynn-1);
01827         wy[5] = kb.i0win_tab(iynn-2);
01828         wy[6] = kb.i0win_tab(iynn-3);
01829 
01830         float ixnn = nuxnew - ixn;
01831         wx[0] = kb.i0win_tab(ixnn+3);
01832         wx[1] = kb.i0win_tab(ixnn+2);
01833         wx[2] = kb.i0win_tab(ixnn+1);
01834         wx[3] = kb.i0win_tab(ixnn);
01835         wx[4] = kb.i0win_tab(ixnn-1);
01836         wx[5] = kb.i0win_tab(ixnn-2);
01837         wx[6] = kb.i0win_tab(ixnn-3);
01838 
01839         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]);
01840 
01841         complex<float> result(0.f,0.f);
01842         for (int iy = 0; iy < 7; iy++) {
01843                 int iyp = iyn + iy - 3 ;
01844                 for (int ix = 0; ix < 7; ix++) {
01845                         int ixp = ixn + ix - 3;
01846                         float w = wx[ix]*wy[iy];
01847                         complex<float> val = fimage->cmplx(ixp,iyp);
01848                         result += val*w;
01849                 }
01850         }
01851 
01852         if (flip)  result = conj(result)/wsum;
01853         else result /= wsum;
01854 
01855         return result;
01856 }*/
01857 
01858 
01859 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01860 
01861         int nxreal = nx - 2;
01862         if (nxreal != ny)
01863                 throw ImageDimensionException("extractpoint requires ny == nx");
01864         int nhalf = nxreal/2;
01865         bool flip = (nuxnew < 0.f);
01866         if (flip) {
01867                 nuxnew *= -1;
01868                 nuynew *= -1;
01869         }
01870         if (nuynew >= nhalf-0.5)  {
01871                 nuynew -= nxreal;
01872         } else if (nuynew < -nhalf-0.5) {
01873                 nuynew += nxreal;
01874         }
01875 
01876         // put (xnew,ynew) on a grid.  The indices will be wrong for
01877         // the Fourier elements in the image, but the grid sizing will
01878         // be correct.
01879         int ixn = int(Util::round(nuxnew));
01880         int iyn = int(Util::round(nuynew));
01881 
01882         // set up some temporary weighting arrays
01883         static float wy[7];
01884         static float wx[7];
01885 
01886         float iynn = nuynew - iyn;
01887         wy[0] = kb.i0win_tab(iynn+3);
01888         wy[1] = kb.i0win_tab(iynn+2);
01889         wy[2] = kb.i0win_tab(iynn+1);
01890         wy[3] = kb.i0win_tab(iynn);
01891         wy[4] = kb.i0win_tab(iynn-1);
01892         wy[5] = kb.i0win_tab(iynn-2);
01893         wy[6] = kb.i0win_tab(iynn-3);
01894 
01895         float ixnn = nuxnew - ixn;
01896         wx[0] = kb.i0win_tab(ixnn+3);
01897         wx[1] = kb.i0win_tab(ixnn+2);
01898         wx[2] = kb.i0win_tab(ixnn+1);
01899         wx[3] = kb.i0win_tab(ixnn);
01900         wx[4] = kb.i0win_tab(ixnn-1);
01901         wx[5] = kb.i0win_tab(ixnn-2);
01902         wx[6] = kb.i0win_tab(ixnn-3);
01903 
01904         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]);
01905 
01906         complex<float> result(0.f,0.f);
01907         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01908                 // (xin,yin) not within window border from the edge
01909                 for (int iy = 0; iy < 7; iy++) {
01910                         int iyp = iyn + iy - 3 ;
01911                         for (int ix = 0; ix < 7; ix++) {
01912                                 int ixp = ixn + ix - 3;
01913                                 float w = wx[ix]*wy[iy];
01914                                 complex<float> val = fimage->cmplx(ixp,iyp);
01915                                 result += val*w;
01916                         }
01917                 }
01918         } else {
01919                 // points that "stick out"
01920                 for (int iy = 0; iy < 7; iy++) {
01921                         int iyp = iyn + iy - 3;
01922                         for (int ix = 0; ix < 7; ix++) {
01923                                 int ixp = ixn + ix - 3;
01924                                 bool mirror = false;
01925                                 int ixt = ixp, iyt = iyp;
01926                                 if (ixt < 0) {
01927                                         ixt = -ixt;
01928                                         iyt = -iyt;
01929                                         mirror = !mirror;
01930                                 }
01931                                 if (ixt > nhalf) {
01932                                         ixt = nxreal - ixt;
01933                                         iyt = -iyt;
01934                                         mirror = !mirror;
01935                                 }
01936                                 if (iyt > nhalf-1)  iyt -= nxreal;
01937                                 if (iyt < -nhalf)   iyt += nxreal;
01938                                 float w = wx[ix]*wy[iy];
01939                                 complex<float> val = fimage->cmplx(ixt,iyt);
01940                                 if (mirror)  result += conj(val)*w;
01941                                 else         result += val*w;
01942                         }
01943                 }
01944         }
01945         if (flip)  result = conj(result)/wsum;
01946         else result /= wsum;
01947         return result;
01948 }
01949 
01950 /*
01951 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01952 
01953         int nxreal = nx - 2;
01954         if (nxreal != ny)
01955                 throw ImageDimensionException("extractpoint requires ny == nx");
01956         int nhalf = nxreal/2;
01957         bool flip = (nuxnew < 0.f);
01958         if (flip) {
01959                 nuxnew *= -1;
01960                 nuynew *= -1;
01961         }
01962         // put (xnew,ynew) on a grid.  The indices will be wrong for
01963         // the Fourier elements in the image, but the grid sizing will
01964         // be correct.
01965         int ixn = int(Util::round(nuxnew));
01966         int iyn = int(Util::round(nuynew));
01967         // set up some temporary weighting arrays
01968         static float wy[7];
01969         static float wx[7];
01970 
01971         float iynn = nuynew - iyn;
01972         wy[0] = kb.i0win_tab(iynn+3);
01973         wy[1] = kb.i0win_tab(iynn+2);
01974         wy[2] = kb.i0win_tab(iynn+1);
01975         wy[3] = kb.i0win_tab(iynn);
01976         wy[4] = kb.i0win_tab(iynn-1);
01977         wy[5] = kb.i0win_tab(iynn-2);
01978         wy[6] = kb.i0win_tab(iynn-3);
01979 
01980         float ixnn = nuxnew - ixn;
01981         wx[0] = kb.i0win_tab(ixnn+3);
01982         wx[1] = kb.i0win_tab(ixnn+2);
01983         wx[2] = kb.i0win_tab(ixnn+1);
01984         wx[3] = kb.i0win_tab(ixnn);
01985         wx[4] = kb.i0win_tab(ixnn-1);
01986         wx[5] = kb.i0win_tab(ixnn-2);
01987         wx[6] = kb.i0win_tab(ixnn-3);
01988 
01989         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]);
01990 
01991         complex<float> result(0.f,0.f);
01992 
01993         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01994                 // (xin,yin) not within window border from the edge
01995                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01996                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01997                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01998                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01999                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
02000                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
02001                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
02002                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
02003                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
02004                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
02005                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
02006                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
02007                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
02008                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
02009                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
02010                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
02011                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
02012                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
02013                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
02014                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
02015                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
02016                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
02017                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
02018                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
02019                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
02020                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
02021                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
02022                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
02023                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
02024                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
02025                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
02026                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
02027                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
02028                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
02029                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
02030                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
02031                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
02032                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
02033                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
02034                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
02035                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
02036                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
02037                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
02038                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
02039                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
02040                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
02041                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
02042                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
02043                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
02044 
02045         } else {
02046                 // points that "stick out"
02047                 for (int iy = 0; iy < 7; iy++) {
02048                         int iyp = iyn + iy - 3;
02049                         for (int ix = 0; ix < 7; ix++) {
02050                                 int ixp = ixn + ix - 3;
02051                                 bool mirror = false;
02052                                 int ixt= ixp, iyt= iyp;
02053                                 if (ixt < 0) {
02054                                         ixt = -ixt;
02055                                         iyt = -iyt;
02056                                         mirror = !mirror;
02057                                 }
02058                                 if (ixt > nhalf) {
02059                                         ixt = nxreal - ixt;
02060                                         iyt = -iyt;
02061                                         mirror = !mirror;
02062                                 }
02063                                 if (iyt > nhalf-1)  iyt -= nxreal;
02064                                 if (iyt < -nhalf)   iyt += nxreal;
02065                                 float w = wx[ix]*wy[iy];
02066                                 complex<float> val = fimage->cmplx(ixt,iyt);
02067                                 if (mirror)  result += conj(val)*w;
02068                                 else         result += val*w;
02069                         }
02070                 }
02071         }
02072         if (flip)  result = conj(result)/wsum;
02073         else result /= wsum;
02074         return result;
02075 }*/
02076 
02077 
02078 float Util::triquad(float R, float S, float T, float* fdata)
02079 {
02080 
02081     const float C2 = 0.5f;    //1.0 / 2.0;
02082     const float C4 = 0.25f;   //1.0 / 4.0;
02083     const float C8 = 0.125f;  //1.0 / 8.0;
02084 
02085     float  RS   = R * S;
02086     float  ST   = S * T;
02087     float  RT   = R * T;
02088     float  RST  = R * ST;
02089 
02090     float  RSQ  = 1-R*R;
02091     float  SSQ  = 1-S*S;
02092     float  TSQ  = 1-T*T;
02093 
02094     float  RM1  = (1-R);
02095     float  SM1  = (1-S);
02096     float  TM1  = (1-T);
02097 
02098     float  RP1  = (1+R);
02099     float  SP1  = (1+S);
02100     float  TP1  = (1+T);
02101 
02102     float triquad =
02103     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
02104         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
02105         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
02106         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
02107         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
02108         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
02109         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
02110         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
02111         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
02112 //
02113         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
02114         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
02115         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
02116         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
02117                       RSQ  * SSQ  * TSQ * fdata[13] +
02118         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
02119         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
02120         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
02121         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
02122  //
02123         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
02124         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
02125         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
02126         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
02127         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
02128         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
02129         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
02130         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
02131         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
02132      return triquad;
02133 }
02134 
02135 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
02136                 : M(M_), fc(fc_), ntable(ntable_) {
02137         // Sinc-Blackman kernel
02138         build_sBtable();
02139 }
02140 
02141 void Util::sincBlackman::build_sBtable() {
02142         sBtable.resize(ntable+1);
02143         int ltab = int(round(float(ntable)/1.25f));
02144         int M2 = M/2;
02145         fltb = float(ltab)/M2;
02146         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
02147         float x = 1.0e-7f;
02148         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)));
02149         for (int i=1; i <= ltab; i++) {
02150                 x = float(i)/fltb;
02151                 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)));
02152                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
02153         }
02154 }
02155 
02156 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
02157                                          int N_, float vtable_, int ntable_)
02158                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
02159                   ntable(ntable_) {
02160         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
02161         if (0.f == v) v = float(K)/2;
02162         if (0.f == vtable) vtable = v;
02163         alphar = alpha*r;
02164         fac = static_cast<float>(twopi)*alphar*v;
02165         vadjust = 1.0f*v;
02166         facadj = static_cast<float>(twopi)*alphar*vadjust;
02167         build_I0table();
02168 }
02169 
02170 float Util::KaiserBessel::i0win(float x) const {
02171         float val0 = float(gsl_sf_bessel_I0(facadj));
02172         float absx = fabs(x);
02173         if (absx > vadjust) return 0.f;
02174         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02175         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02176         return res;
02177 }
02178 
02179 void Util::KaiserBessel::build_I0table() {
02180         i0table.resize(ntable+1); // i0table[0:ntable]
02181         int ltab = int(round(float(ntable)/1.25f));
02182         fltb = float(ltab)/(K/2);
02183         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02184         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02185         for (int i=0; i <= ltab; i++) {
02186                 float s = float(i)/fltb/N;
02187                 if (s < vadjust) {
02188                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02189                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02190                 } else {
02191                         i0table[i] = 0.f;
02192                 }
02193 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02194         }
02195 }
02196 
02197 float Util::KaiserBessel::I0table_maxerror() {
02198         float maxdiff = 0.f;
02199         for (int i = 1; i <= ntable; i++) {
02200                 float diff = fabs(i0table[i] - i0table[i-1]);
02201                 if (diff > maxdiff) maxdiff = diff;
02202         }
02203         return maxdiff;
02204 }
02205 
02206 float Util::KaiserBessel::sinhwin(float x) const {
02207         float val0 = sinh(fac)/fac;
02208         float absx = fabs(x);
02209         if (0.0 == x) {
02210                 float res = 1.0f;
02211                 return res;
02212         } else if (absx == alphar) {
02213                 return 1.0f/val0;
02214         } else if (absx < alphar) {
02215                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02216                 float facrt = fac*rt;
02217                 float res = (sinh(facrt)/facrt)/val0;
02218                 return res;
02219         } else {
02220                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02221                 float facrt = fac*rt;
02222                 float res = (sin(facrt)/facrt)/val0;
02223                 return res;
02224         }
02225 }
02226 
02227 float Util::FakeKaiserBessel::i0win(float x) const {
02228         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02229         float absx = fabs(x);
02230         if (absx > vadjust) return 0.f;
02231         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02232         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02233         return res;
02234 }
02235 
02236 void Util::FakeKaiserBessel::build_I0table() {
02237         i0table.resize(ntable+1); // i0table[0:ntable]
02238         int ltab = int(round(float(ntable)/1.1f));
02239         fltb = float(ltab)/(K/2);
02240         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02241         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02242         for (int i=0; i <= ltab; i++) {
02243                 float s = float(i)/fltb/N;
02244                 if (s < vadjust) {
02245                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02246                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02247                 } else {
02248                         i0table[i] = 0.f;
02249                 }
02250         }
02251 }
02252 
02253 float Util::FakeKaiserBessel::sinhwin(float x) const {
02254         float val0 = sinh(fac)/fac;
02255         float absx = fabs(x);
02256         if (0.0 == x) {
02257                 float res = 1.0f;
02258                 return res;
02259         } else if (absx == alphar) {
02260                 return 1.0f/val0;
02261         } else if (absx < alphar) {
02262                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02263                 float facrt = fac*rt;
02264                 float res = (sinh(facrt)/facrt)/val0;
02265                 return res;
02266         } else {
02267                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02268                 float facrt = fac*rt;
02269                 float res = (sin(facrt)/facrt)/val0;
02270                 return res;
02271         }
02272 }
02273 
02274 #if 0 // 1-st order KB window
02275 float Util::FakeKaiserBessel::sinhwin(float x) const {
02276         //float val0 = sinh(fac)/fac;
02277         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02278         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02279         float absx = fabs(x);
02280         if (0.0 == x) {
02281                 //float res = 1.0f;
02282                 float res = val0;
02283                 return res;
02284         } else if (absx == alphar) {
02285                 //return 1.0f/val0;
02286                 return prefix;
02287         } else if (absx < alphar) {
02288                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02289                 //float facrt = fac*rt;
02290                 float facrt = facadj*rt;
02291                 //float res = (sinh(facrt)/facrt)/val0;
02292                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02293                 return res;
02294         } else {
02295                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02296                 //float facrt = fac*rt;
02297                 float facrt = facadj*rt;
02298                 //float res = (sin(facrt)/facrt)/val0;
02299                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02300                 return res;
02301         }
02302 }
02303 #endif // 0
02304 
02305 
02306 
02307 #define  circ(i)         circ[i-1]
02308 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02309 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02310 
02311 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02312         int nsam = image->get_xsize();
02313         int nrow = image->get_ysize();
02314         int nring = numr.size()/3;
02315         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02316         EMData* out = new EMData();
02317         out->set_size(lcirc,1,1);
02318         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02319         float *xim  = image->get_data();
02320         float *circ = out->get_data();
02321 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02322    return out;
02323 }
02324 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02325           float *circ, int lcirc, int nring, char mode)
02326 {*/
02327 /*
02328 c
02329 c  purpose:
02330 c
02331 c  resmaple to polar coordinates
02332 c
02333 */
02334         //  dimension         xim(nsam,nrow),circ(lcirc)
02335         //  integer           numr(3,nring)
02336 
02337         double dfi, dpi;
02338         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02339         float  yq, xold, yold, fi, x, y;
02340 
02341         ns2 = nsam/2+1;
02342         nr2 = nrow/2+1;
02343         dpi = 2.0*atan(1.0);
02344 
02345         for (i=1;i<=nring;i++) {
02346                 // radius of the ring
02347                 inr = numr(1,i);
02348                 yq  = static_cast<float>(inr);
02349                 l   = numr(3,i);
02350                 if (mode == 'h' || mode == 'H')  lt = l/2;
02351                 else                             lt = l/4;
02352 
02353                 nsim           = lt-1;
02354                 dfi            = dpi/(nsim+1);
02355                 kcirc          = numr(2,i);
02356                 xold           = 0.0f;
02357                 yold           = static_cast<float>(inr);
02358                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02359                 xold           = static_cast<float>(inr);
02360                 yold           = 0.0f;
02361                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02362 
02363                 if (mode == 'f' || mode == 'F') {
02364                         xold              = 0.0f;
02365                         yold              = static_cast<float>(-inr);
02366                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02367                         xold              = static_cast<float>(-inr);
02368                         yold              = 0.0f;
02369                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02370                 }
02371 
02372                 for (j=1;j<=nsim;j++) {
02373                         fi               = static_cast<float>(dfi*j);
02374                         x                = sin(fi)*yq;
02375                         y                = cos(fi)*yq;
02376                         xold             = x;
02377                         yold             = y;
02378                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02379                         xold             =  y;
02380                         yold             = -x;
02381                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02382 
02383                         if (mode == 'f' || mode == 'F')  {
02384                                 xold                = -x;
02385                                 yold                = -y;
02386                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02387                                 xold                = -y;
02388                                 yold                =  x;
02389                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02390                         }
02391                 }
02392         }
02393         return  out;
02394 }
02395 
02396 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02397         int nsam = image->get_xsize();
02398         int nrow = image->get_ysize();
02399         int nring = numr.size()/3;
02400         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02401         EMData* out = new EMData();
02402         out->set_size(lcirc,1,1);
02403         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02404         float *xim  = image->get_data();
02405         float *circ = out->get_data();
02406         double dpi, dfi;
02407         int    it, jt, inr, l, nsim, kcirc, lt;
02408         float  xold, yold, fi, x, y;
02409 
02410         //     cns2 and cnr2 are predefined centers
02411         //     no need to set to zero, all elements are defined
02412         dpi = 2*atan(1.0);
02413         for (it=1; it<=nring; it++) {
02414                 // radius of the ring
02415                 inr = numr(1,it);
02416 
02417                 // "F" means a full circle interpolation
02418                 // "H" means a half circle interpolation
02419 
02420                 l = numr(3,it);
02421                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02422                 else                              lt = l / 4;
02423 
02424                 nsim  = lt - 1;
02425                 dfi   = dpi / (nsim+1);
02426                 kcirc = numr(2,it);
02427                 xold  = 0.0f+cns2;
02428                 yold  = inr+cnr2;
02429 
02430                 Assert( kcirc <= lcirc );
02431                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02432 
02433                 xold  = inr+cns2;
02434                 yold  = 0.0f+cnr2;
02435                 Assert( lt+kcirc <= lcirc );
02436                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02437 
02438                 if ( mode == 'f' || mode == 'F' ) {
02439                         xold = 0.0f+cns2;
02440                         yold = -inr+cnr2;
02441                         Assert( lt+lt+kcirc <= lcirc );
02442                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02443 
02444                         xold = -inr+cns2;
02445                         yold = 0.0f+cnr2;
02446                         Assert(lt+lt+lt+kcirc <= lcirc );
02447                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02448                 }
02449 
02450                 for (jt=1; jt<=nsim; jt++) {
02451                         fi   = static_cast<float>(dfi * jt);
02452                         x    = sin(fi) * inr;
02453                         y    = cos(fi) * inr;
02454 
02455                         xold = x+cns2;
02456                         yold = y+cnr2;
02457 
02458                         Assert( jt+kcirc <= lcirc );
02459                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02460 
02461                         xold = y+cns2;
02462                         yold = -x+cnr2;
02463 
02464                         Assert( jt+lt+kcirc <= lcirc );
02465                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02466 
02467                         if ( mode == 'f' || mode == 'F' ) {
02468                                 xold = -x+cns2;
02469                                 yold = -y+cnr2;
02470 
02471                                 Assert( jt+lt+lt+kcirc <= lcirc );
02472                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02473 
02474                                 xold = -y+cns2;
02475                                 yold = x+cnr2;
02476 
02477                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02478                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02479                         }
02480                 } // end for jt
02481         } //end for it
02482         return out;
02483 }
02484 
02485 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02486 {
02487 /*
02488 c  purpose: linear interpolation
02489   Optimized for speed, circular closer removed, checking of ranges removed
02490 */
02491     float bilinear;
02492     int   ixold, iyold;
02493 
02494 /*
02495         float xdif, ydif, xrem, yrem;
02496         ixold   = (int) floor(xold);
02497         iyold   = (int) floor(yold);
02498         ydif = yold - iyold;
02499         yrem = 1.0f - ydif;
02500 
02501         //  May want to insert if?
02502 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02503 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02504 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02505         xdif = xold - ixold;
02506         xrem = 1.0f- xdif;
02507 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02508 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02509 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02510         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02511                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02512 
02513     return bilinear;
02514 }
02515 */
02516         float xdif, ydif;
02517 
02518         ixold   = (int) xold;
02519         iyold   = (int) yold;
02520         ydif = yold - iyold;
02521 
02522         //  May want to insert it?
02523 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02524 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02525 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02526         xdif = xold - ixold;
02527         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02528                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02529                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02530 
02531         return bilinear;
02532 }
02533 
02534 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02535              int  *numr, float *circ, int , int  nring, char  mode) {
02536         double dpi, dfi;
02537         int    it, jt, inr, l, nsim, kcirc, lt;
02538         float  xold, yold, fi, x, y;
02539 
02540         //     cns2 and cnr2 are predefined centers
02541         //     no need to set to zero, all elements are defined
02542 
02543         dpi = 2*atan(1.0);
02544         for (it=1; it<=nring; it++) {
02545                 // radius of the ring
02546                 inr = numr(1,it);
02547 
02548                 l = numr(3,it);
02549                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02550                 else                              lt = l / 4;
02551 
02552                 nsim  = lt - 1;
02553                 dfi   = dpi / (nsim+1);
02554                 kcirc = numr(2,it);
02555 
02556 
02557                 xold  = 0.0f+cns2;
02558                 yold  = inr+cnr2;
02559 
02560                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02561 
02562                 xold  = inr+cns2;
02563                 yold  = 0.0f+cnr2;
02564                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02565 
02566                 if ( mode == 'f' || mode == 'F' ) {
02567                         xold = 0.0f+cns2;
02568                         yold = -inr+cnr2;
02569                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02570 
02571                         xold = -inr+cns2;
02572                         yold = 0.0f+cnr2;
02573                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02574                 }
02575 
02576                 for (jt=1; jt<=nsim; jt++) {
02577                         fi   = static_cast<float>(dfi * jt);
02578                         x    = sin(fi) * inr;
02579                         y    = cos(fi) * inr;
02580 
02581                         xold = x+cns2;
02582                         yold = y+cnr2;
02583                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02584 
02585                         xold = y+cns2;
02586                         yold = -x+cnr2;
02587                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02588 
02589                         if ( mode == 'f' || mode == 'F' ) {
02590                                 xold = -x+cns2;
02591                                 yold = -y+cnr2;
02592                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02593 
02594                                 xold = -y+cns2;
02595                                 yold = x+cnr2;
02596                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02597                         }
02598                 } // end for jt
02599         } //end for it
02600 }
02601 /*
02602 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02603              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02604 {
02605    double dpi, dfi;
02606    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02607    float  yq, fi, x, y;
02608 
02609    //     cns2 and cnr2 are predefined centers
02610    //     no need to set to zero, all elements are defined
02611 
02612    dpi = 2*atan(1.0);
02613    for (it=1; it<=nring; it++) {
02614       // radius of the ring
02615       inr = numr(1,it);
02616       yq  = inr;
02617 
02618       l = numr(3,it);
02619       if ( mode == 'h' || mode == 'H' ) {
02620          lt = l / 2;
02621       }
02622       else { // if ( mode == 'f' || mode == 'F' )
02623          lt = l / 4;
02624       }
02625 
02626       nsim  = lt - 1;
02627       dfi   = dpi / (nsim+1);
02628       kcirc = numr(2,it);
02629 
02630 
02631         xold = (int) (0.0+cns2);
02632         yold = (int) (inr+cnr2);
02633 
02634         circ(kcirc) = xim(xold, yold);
02635 
02636       xold = (int) (inr+cns2);
02637       yold = (int) (0.0+cnr2);
02638       circ(lt+kcirc) = xim(xold, yold);
02639 
02640       if ( mode == 'f' || mode == 'F' ) {
02641          xold  = (int) (0.0+cns2);
02642          yold = (int) (-inr+cnr2);
02643          circ(lt+lt+kcirc) = xim(xold, yold);
02644 
02645          xold  = (int) (-inr+cns2);
02646          yold = (int) (0.0+cnr2);
02647          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02648       }
02649 
02650       for (jt=1; jt<=nsim; jt++) {
02651          fi   = dfi * jt;
02652          x    = sin(fi) * yq;
02653          y    = cos(fi) * yq;
02654 
02655          xold  = (int) (x+cns2);
02656          yold = (int) (y+cnr2);
02657          circ(jt+kcirc) = xim(xold, yold);
02658 
02659          xold  = (int) (y+cns2);
02660          yold = (int) (-x+cnr2);
02661          circ(jt+lt+kcirc) = xim(xold, yold);
02662 
02663          if ( mode == 'f' || mode == 'F' ) {
02664             xold  = (int) (-x+cns2);
02665             yold = (int) (-y+cnr2);
02666             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02667 
02668             xold  = (int) (-y+cns2);
02669             yold = (int) (x+cnr2);
02670             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02671          }
02672       } // end for jt
02673    } //end for it
02674 }
02675 */
02676 //xim((int) floor(xold), (int) floor(yold))
02677 #undef  xim
02678 
02679 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02680 // input image is twice the size of the original image
02681         int nring = numr.size()/3;
02682         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02683         EMData* out = new EMData();
02684         out->set_size(lcirc,1,1);
02685         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02686         float *circ = out->get_data();
02687         float *fimage = image->get_data();
02688         int nx = image->get_xsize();
02689         int ny = image->get_ysize();
02690         int nz = image->get_zsize();
02691         double dpi, dfi;
02692         int    it, jt, inr, l, nsim, kcirc, lt;
02693         float  yq, xold, yold, fi, x, y;
02694 
02695         //     cns2 and cnr2 are predefined centers
02696         //     no need to set to zero, all elements are defined
02697 
02698         dpi = 2*atan(1.0);
02699         for (it=1;it<=nring;it++) {
02700                 // radius of the ring
02701                 inr = numr(1,it);
02702                 yq  = static_cast<float>(inr);
02703 
02704                 l = numr(3,it);
02705                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02706                 else                               lt = l / 4;
02707 
02708                 nsim  = lt - 1;
02709                 dfi   = dpi / (nsim+1);
02710                 kcirc = numr(2,it);
02711                 xold  = 0.0f;
02712                 yold  = static_cast<float>(inr);
02713                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02714 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02715 
02716                 xold  = static_cast<float>(inr);
02717                 yold  = 0.0f;
02718                 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02719 //      circ(lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02720 
02721         if ( mode == 'f' || mode == 'F' ) {
02722                 xold = 0.0f;
02723                 yold = static_cast<float>(-inr);
02724                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02725 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02726 
02727                 xold = static_cast<float>(-inr);
02728                 yold = 0.0f;
02729                 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);
02730 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02731         }
02732 
02733         for (jt=1;jt<=nsim;jt++) {
02734                 fi   = static_cast<float>(dfi * jt);
02735                 x    = sin(fi) * yq;
02736                 y    = cos(fi) * yq;
02737 
02738                 xold = x;
02739                 yold = y;
02740                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02741 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02742 
02743                 xold = y;
02744                 yold = -x;
02745                 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02746 //         circ(jt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02747 
02748         if ( mode == 'f' || mode == 'F' ) {
02749                 xold = -x;
02750                 yold = -y;
02751                 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);
02752 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02753 
02754                 xold = -y;
02755                 yold = x;
02756                 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);
02757 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02758         }
02759         } // end for jt
02760         } //end for it
02761         return  out;
02762 }
02763 
02764 /*
02765 
02766         A set of 1-D power-of-two FFTs
02767         Pawel & Chao 01/20/06
02768 
02769 fftr_q(xcmplx,nv)
02770   single precision
02771 
02772  dimension xcmplx(2,iabs(nv)/2);
02773  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02774  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02775 
02776 
02777 fftr_d(xcmplx,nv)
02778   double precision
02779 
02780  dimension xcmplx(2,iabs(nv)/2);
02781  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02782  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02783 
02784 
02785 
02786 */
02787 #define  tab1(i)      tab1[i-1]
02788 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02789 #define  br(i)        br[i-1]
02790 #define  bi(i)        bi[i-1]
02791 //-----------------------------------------
02792 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02793 {
02794         double rni,sgn,tr1,tr2,ti1,ti2;
02795         double cc,c,ss,s,t,x2,x3,x4,x5;
02796         int    b3,b4,b5,b6,b7,b56;
02797         int    n, k, l, j, i, ix0, ix1, status=0;
02798 
02799         const double tab1[] = {
02800                 9.58737990959775e-5,
02801                 1.91747597310703e-4,
02802                 3.83495187571395e-4,
02803                 7.66990318742704e-4,
02804                 1.53398018628476e-3,
02805                 3.06795676296598e-3,
02806                 6.13588464915449e-3,
02807                 1.22715382857199e-2,
02808                 2.45412285229123e-2,
02809                 4.90676743274181e-2,
02810                 9.80171403295604e-2,
02811                 1.95090322016128e-1,
02812                 3.82683432365090e-1,
02813                 7.07106781186546e-1,
02814                 1.00000000000000,
02815         };
02816 
02817         n=(int)pow(2.0f,ln);
02818 
02819         k=abs(ks);
02820         l=16-ln;
02821         b3=n*k;
02822         b6=b3;
02823         b7=k;
02824         if (ks > 0) {
02825                 sgn=1.0f;
02826         } else {
02827                 sgn=-1.0f;
02828                 rni=1.0f/(float)(n);
02829                 j=1;
02830                 for (i=1; i<=n; i++) {
02831                         br(j)=br(j)*rni;
02832                         bi(j)=bi(j)*rni;
02833                         j=j+k;
02834                 }
02835         }
02836 
02837 L12:
02838    b6=b6/2;
02839    b5=b6;
02840    b4=2*b6;
02841    b56=b5-b6;
02842 
02843 L14:
02844    tr1=br(b5+1);
02845    ti1=bi(b5+1);
02846    tr2=br(b56+1);
02847    ti2=bi(b56+1);
02848 
02849    br(b5+1)=tr2-tr1;
02850    bi(b5+1)=ti2-ti1;
02851    br(b56+1)=tr1+tr2;
02852    bi(b56+1)=ti1+ti2;
02853 
02854    b5=b5+b4;
02855    b56=b5-b6;
02856    if ( b5 <= b3 )  goto  L14;
02857    if ( b6 == b7 )  goto  L20;
02858 
02859    b4=b7;
02860    cc=2.0f*pow(tab1(l),2);
02861    c=1.0f-cc;
02862    l++;
02863    ss=sgn*tab1(l);
02864    s=ss;
02865 
02866 L16:
02867    b5=b6+b4;
02868    b4=2*b6;
02869    b56=b5-b6;
02870 
02871 L18:
02872    tr1=br(b5+1);
02873    ti1=bi(b5+1);
02874    tr2=br(b56+1);
02875    ti2=bi(b56+1);
02876    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02877    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02878    br(b56+1)=tr1+tr2;
02879    bi(b56+1)=ti1+ti2;
02880 
02881    b5=b5+b4;
02882    b56=b5-b6;
02883    if ( b5 <= b3 )  goto  L18;
02884    b4=b5-b6;
02885    b5=b4-b3;
02886    c=-c;
02887    b4=b6-b5;
02888    if ( b5 < b4 )  goto  L16;
02889    b4=b4+b7;
02890    if ( b4 >= b5 ) goto  L12;
02891 
02892    t=c-cc*c-ss*s;
02893    s=s+ss*c-cc*s;
02894    c=t;
02895    goto  L16;
02896 
02897 L20:
02898    ix0=b3/2;
02899    b3=b3-b7;
02900    b4=0;
02901    b5=0;
02902    b6=ix0;
02903    ix1=0;
02904    if (b6 == b7) goto EXIT;
02905 
02906 L22:
02907    b4=b3-b4;
02908    b5=b3-b5;
02909    x2=br(b4+1);
02910    x3=br(b5+1);
02911    x4=bi(b4+1);
02912    x5=bi(b5+1);
02913    br(b4+1)=x3;
02914    br(b5+1)=x2;
02915    bi(b4+1)=x5;
02916    bi(b5+1)=x4;
02917    if(b6 < b4)  goto  L22;
02918 
02919 L24:
02920    b4=b4+b7;
02921    b5=b6+b5;
02922    x2=br(b4+1);
02923    x3=br(b5+1);
02924    x4=bi(b4+1);
02925    x5=bi(b5+1);
02926    br(b4+1)=x3;
02927    br(b5+1)=x2;
02928    bi(b4+1)=x5;
02929    bi(b5+1)=x4;
02930    ix0=b6;
02931 
02932 L26:
02933    ix0=ix0/2;
02934    ix1=ix1-ix0;
02935    if( ix1 >= 0)  goto L26;
02936 
02937    ix0=2*ix0;
02938    b4=b4+b7;
02939    ix1=ix1+ix0;
02940    b5=ix1;
02941    if ( b5 >= b4)  goto  L22;
02942    if ( b4 < b6)   goto  L24;
02943 
02944 EXIT:
02945    status = 0;
02946 }
02947 
02948 // -----------------------------------------------------------------
02949 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02950 {
02951         //  dimension  br(1),bi(1)
02952 
02953         int b3,b4,b5,b6,b7,b56;
02954         int n, k, l, j, i, ix0, ix1;
02955         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02956         int status=0;
02957 
02958         const float tab1[] = {
02959                 9.58737990959775e-5f,
02960                 1.91747597310703e-4f,
02961                 3.83495187571395e-4f,
02962                 7.66990318742704e-4f,
02963                 1.53398018628476e-3f,
02964                 3.06795676296598e-3f,
02965                 6.13588464915449e-3f,
02966                 1.22715382857199e-2f,
02967                 2.45412285229123e-2f,
02968                 4.90676743274181e-2f,
02969                 9.80171403295604e-2f,
02970                 1.95090322016128e-1f,
02971                 3.82683432365090e-1f,
02972                 7.07106781186546e-1f,
02973                 1.00000000000000f,
02974         };
02975 
02976         n=(int)pow(2.0f,ln);
02977 
02978         k=abs(ks);
02979         l=16-ln;
02980         b3=n*k;
02981         b6=b3;
02982         b7=k;
02983         if( ks > 0 ) {
02984                 sgn=1.0f;
02985         } else {
02986                 sgn=-1.0f;
02987                 rni=1.0f/(float)n;
02988                 j=1;
02989                 for (i=1; i<=n; i++) {
02990                         br(j)=br(j)*rni;
02991                         bi(j)=bi(j)*rni;
02992                         j=j+k;
02993                 }
02994         }
02995 L12:
02996    b6=b6/2;
02997    b5=b6;
02998    b4=2*b6;
02999    b56=b5-b6;
03000 L14:
03001    tr1=br(b5+1);
03002    ti1=bi(b5+1);
03003 
03004    tr2=br(b56+1);
03005    ti2=bi(b56+1);
03006 
03007    br(b5+1)=tr2-tr1;
03008    bi(b5+1)=ti2-ti1;
03009    br(b56+1)=tr1+tr2;
03010    bi(b56+1)=ti1+ti2;
03011 
03012    b5=b5+b4;
03013    b56=b5-b6;
03014    if ( b5 <= b3 )  goto  L14;
03015    if ( b6 == b7 )  goto  L20;
03016 
03017    b4=b7;
03018    cc=2.0f*pow(tab1(l),2);
03019    c=1.0f-cc;
03020    l++;
03021    ss=sgn*tab1(l);
03022    s=ss;
03023 L16:
03024    b5=b6+b4;
03025    b4=2*b6;
03026    b56=b5-b6;
03027 L18:
03028    tr1=br(b5+1);
03029    ti1=bi(b5+1);
03030    tr2=br(b56+1);
03031    ti2=bi(b56+1);
03032    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
03033    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
03034    br(b56+1)=tr1+tr2;
03035    bi(b56+1)=ti1+ti2;
03036 
03037    b5=b5+b4;
03038    b56=b5-b6;
03039    if(b5 <= b3)  goto L18;
03040    b4=b5-b6;
03041    b5=b4-b3;
03042    c=-c;
03043    b4=b6-b5;
03044    if(b5 < b4)  goto  L16;
03045    b4=b4+b7;
03046    if(b4 >= b5) goto  L12;
03047 
03048    t=c-cc*c-ss*s;
03049    s=s+ss*c-cc*s;
03050    c=t;
03051    goto  L16;
03052 L20:
03053    ix0=b3/2;
03054    b3=b3-b7;
03055    b4=0;
03056    b5=0;
03057    b6=ix0;
03058    ix1=0;
03059    if ( b6 == b7) goto EXIT;
03060 L22:
03061    b4=b3-b4;
03062    b5=b3-b5;
03063    x2=br(b4+1);
03064    x3=br(b5+1);
03065    x4=bi(b4+1);
03066    x5=bi(b5+1);
03067    br(b4+1)=x3;
03068    br(b5+1)=x2;
03069    bi(b4+1)=x5;
03070    bi(b5+1)=x4;
03071    if (b6 < b4) goto  L22;
03072 L24:
03073    b4=b4+b7;
03074    b5=b6+b5;
03075    x2=br(b4+1);
03076    x3=br(b5+1);
03077    x4=bi(b4+1);
03078    x5=bi(b5+1);
03079    br(b4+1)=x3;
03080    br(b5+1)=x2;
03081    bi(b4+1)=x5;
03082    bi(b5+1)=x4;
03083    ix0=b6;
03084 L26:
03085    ix0=ix0/2;
03086    ix1=ix1-ix0;
03087    if(ix1 >= 0)  goto  L26;
03088 
03089    ix0=2*ix0;
03090    b4=b4+b7;
03091    ix1=ix1+ix0;
03092    b5=ix1;
03093    if (b5 >= b4)  goto  L22;
03094    if (b4 < b6)   goto  L24;
03095 EXIT:
03096    status = 0;
03097 }
03098 
03099 void  Util::fftr_q(float *xcmplx, int nv)
03100 {
03101    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
03102 
03103         int nu, inv, nu1, n, isub, n2, i1, i2, i;
03104         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
03105 
03106         const float tab1[] = {
03107                 9.58737990959775e-5f,
03108                 1.91747597310703e-4f,
03109                 3.83495187571395e-4f,
03110                 7.66990318742704e-4f,
03111                 1.53398018628476e-3f,
03112                 3.06795676296598e-3f,
03113                 6.13588464915449e-3f,
03114                 1.22715382857199e-2f,
03115                 2.45412285229123e-2f,
03116                 4.90676743274181e-2f,
03117                 9.80171403295604e-2f,
03118                 1.95090322016128e-1f,
03119                 3.82683432365090e-1f,
03120                 7.07106781186546e-1f,
03121                 1.00000000000000f,
03122         };
03123 
03124         nu=abs(nv);
03125         inv=nv/nu;
03126         nu1=nu-1;
03127         n=(int)pow(2.f,nu1);
03128         isub=16-nu1;
03129 
03130         ss=-tab1(isub);
03131         cc=-2.0f*pow(tab1(isub-1),2.f);
03132         c=1.0f;
03133         s=0.0f;
03134         n2=n/2;
03135         if ( inv > 0) {
03136                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03137                 tr=xcmplx(1,1);
03138                 ti=xcmplx(2,1);
03139                 xcmplx(1,1)=tr+ti;
03140                 xcmplx(2,1)=tr-ti;
03141                 for (i=1;i<=n2;i++) {
03142                         i1=i+1;
03143                         i2=n-i+1;
03144                         tr1=xcmplx(1,i1);
03145                         tr2=xcmplx(1,i2);
03146                         ti1=xcmplx(2,i1);
03147                         ti2=xcmplx(2,i2);
03148                         t=(cc*c-ss*s)+c;
03149                         s=(cc*s+ss*c)+s;
03150                         c=t;
03151                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03152                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03153                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03154                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03155                 }
03156         } else {
03157                 tr=xcmplx(1,1);
03158                 ti=xcmplx(2,1);
03159                 xcmplx(1,1)=0.5f*(tr+ti);
03160                 xcmplx(2,1)=0.5f*(tr-ti);
03161                 for (i=1; i<=n2; i++) {
03162                         i1=i+1;
03163                         i2=n-i+1;
03164                         tr1=xcmplx(1,i1);
03165                         tr2=xcmplx(1,i2);
03166                         ti1=xcmplx(2,i1);
03167                         ti2=xcmplx(2,i2);
03168                         t=(cc*c-ss*s)+c;
03169                         s=(cc*s+ss*c)+s;
03170                         c=t;
03171                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03172                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03173                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03174                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03175                 }
03176                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03177         }
03178 }
03179 
03180 // -------------------------------------------
03181 void  Util::fftr_d(double *xcmplx, int nv)
03182 {
03183         // double precision  x(2,1)
03184         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03185         double tr1,tr2,ti1,ti2,tr,ti;
03186         double cc,c,ss,s,t;
03187         const double tab1[] = {
03188                 9.58737990959775e-5,
03189                 1.91747597310703e-4,
03190                 3.83495187571395e-4,
03191                 7.66990318742704e-4,
03192                 1.53398018628476e-3,
03193                 3.06795676296598e-3,
03194                 6.13588464915449e-3,
03195                 1.22715382857199e-2,
03196                 2.45412285229123e-2,
03197                 4.90676743274181e-2,
03198                 9.80171403295604e-2,
03199                 1.95090322016128e-1,
03200                 3.82683432365090e-1,
03201                 7.07106781186546e-1,
03202                 1.00000000000000,
03203         };
03204 
03205         nu=abs(nv);
03206         inv=nv/nu;
03207         nu1=nu-1;
03208         n=(int)pow(2.0f,nu1);
03209         isub=16-nu1;
03210         ss=-tab1(isub);
03211         cc=-2.0*pow(tab1(isub-1),2);
03212         c=1.0f;
03213         s=0.0f;
03214         n2=n/2;
03215 
03216         if ( inv > 0 ) {
03217                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03218                 tr=xcmplx(1,1);
03219                 ti=xcmplx(2,1);
03220                 xcmplx(1,1)=tr+ti;
03221                 xcmplx(2,1)=tr-ti;
03222                 for (i=1;i<=n2;i++) {
03223                         i1=i+1;
03224                         i2=n-i+1;
03225                         tr1=xcmplx(1,i1);
03226                         tr2=xcmplx(1,i2);
03227                         ti1=xcmplx(2,i1);
03228                         ti2=xcmplx(2,i2);
03229                         t=(cc*c-ss*s)+c;
03230                         s=(cc*s+ss*c)+s;
03231                         c=t;
03232                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03233                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03234                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03235                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03236                 }
03237         } else {
03238                 tr=xcmplx(1,1);
03239                 ti=xcmplx(2,1);
03240                 xcmplx(1,1)=0.5*(tr+ti);
03241                 xcmplx(2,1)=0.5*(tr-ti);
03242                 for (i=1; i<=n2; i++) {
03243                         i1=i+1;
03244                         i2=n-i+1;
03245                         tr1=xcmplx(1,i1);
03246                         tr2=xcmplx(1,i2);
03247                         ti1=xcmplx(2,i1);
03248                         ti2=xcmplx(2,i2);
03249                         t=(cc*c-ss*s)+c;
03250                         s=(cc*s+ss*c)+s;
03251                         c=t;
03252                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03253                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03254                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03255                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03256                 }
03257                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03258         }
03259 }
03260 #undef  tab1
03261 #undef  xcmplx
03262 #undef  br
03263 #undef  bi
03264 
03265 
03266 void Util::Frngs(EMData* circp, vector<int> numr){
03267         int nring = numr.size()/3;
03268         float *circ = circp->get_data();
03269         int i, l;
03270         for (i=1; i<=nring;i++) {
03271 
03272 #ifdef _WIN32
03273                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03274 #else
03275                 l=(int)(log2(numr(3,i)));
03276 #endif  //_WIN32
03277 
03278                 fftr_q(&circ(numr(2,i)),l);
03279         }
03280 }
03281 
03282 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03283         int nring = numr.size()/3;
03284         float *circ = circp->get_data();
03285         int i, l;
03286         for (i=1; i<=nring;i++) {
03287 
03288 #ifdef _WIN32
03289                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03290 #else
03291                 l=(int)(log2(numr(3,i)));
03292 #endif  //_WIN32
03293 
03294                 fftr_q(&circ(numr(2,i)),-l);
03295         }
03296 }
03297 #undef  circ
03298 
03299 void Util::Applyws(EMData* circp, vector<int> numr, vector<float> wr)
03300 {       /*
03301           Apply weights to FTs of rings
03302         */
03303         const int nring = numr.size() / 3;
03304         const int maxrin = numr.back();
03305         float *circ = circp->get_data();
03306         for (int i = 0; i < nring; ++i) {
03307                 const int numr3i = numr[2+i*3];
03308                 const int numr2i = numr[1+i*3]-1;
03309                 const float w = wr[i];
03310                 circ[numr2i] *= w;
03311                 if (numr3i == maxrin)  circ[numr2i+1] *= w;
03312                 else                   circ[numr2i+1] *= 0.5*w;
03313                 for (int j = 2+numr2i; j < numr3i+numr2i; ++j)  circ[j] *= w;
03314         }
03315 }
03316 
03317 #define  b(i)            b[i-1]
03318 void Util::prb1d(double *b, int npoint, float *pos) {
03319         double  c2,c3;
03320         int     nhalf;
03321 
03322         nhalf = npoint/2 + 1;
03323         *pos  = 0.0;
03324 
03325         if (npoint == 7) {
03326                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03327                      - 6.*b(6) + 31.*b(7);
03328                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03329         }
03330         else if (npoint == 5) {
03331                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03332                    + 46.*b(5) ) / (-70.);
03333                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03334         }
03335         else if (npoint == 3) {
03336                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03337                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03338         }
03339         //else if (npoint == 9) {
03340         else  { // at least one has to be true!!
03341                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03342                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03343                      + 1092.*b(9) ) / (-4620.);
03344                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03345                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03346         }
03347         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03348 }
03349 #undef  b
03350 
03351 #define  circ1(i)        circ1[i-1]
03352 #define  circ2(i)        circ2[i-1]
03353 #define  t(i)            t[i-1]
03354 #define  q(i)            q[i-1]
03355 #define  b(i)            b[i-1]
03356 #define  t7(i)           t7[i-1]
03357 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03358         //  neg = 0 straight,  neg = 1 mirrored
03359         int nring = numr.size()/3;
03360         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03361         int maxrin = numr[numr.size()-1];
03362         double qn;   float  tot;
03363         float *circ1 = circ1p->get_data();
03364         float *circ2 = circ2p->get_data();
03365 /*
03366 c checks single position, neg is flag for checking mirrored position
03367 c
03368 c  input - fourier transforms of rings!
03369 c  first set is conjugated (mirrored) if neg
03370 c  circ1 already multiplied by weights!
03371 c       automatic arrays
03372         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03373         double precision  q(maxrin)
03374         double precision  t7(-3:3)
03375 */
03376         float *t;
03377         double t7[7], *q;
03378         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03379         float  pos;
03380 
03381 #ifdef _WIN32
03382         ip = -(int)(log((float)maxrin)/log(2.0f));
03383 #else
03384         ip = -(int) (log2(maxrin));
03385 #endif  //_WIN32
03386 
03387         q = (double*)calloc(maxrin, sizeof(double));
03388         t = (float*)calloc(maxrin, sizeof(float));
03389 
03390 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03391         for (i=1; i<=nring; i++) {
03392                 numr3i = numr(3,i);
03393                 numr2i = numr(2,i);
03394 
03395                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03396 
03397                 if (numr3i != maxrin) {
03398                          // test .ne. first for speed on some compilers
03399                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03400                         t(2)            = 0.0;
03401 
03402                         if (neg) {
03403                                 // first set is conjugated (mirrored)
03404                                 for (j=3;j<=numr3i;j=j+2) {
03405                                         jc = j+numr2i-1;
03406                                         t(j)   =  (circ1(jc))*circ2(jc)   - (circ1(jc+1))*circ2(jc+1);
03407                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03408                                 }
03409                         } else {
03410                                 for (j=3;j<=numr3i;j=j+2) {
03411                                         jc = j+numr2i-1;
03412                                         t(j)   =  (circ1(jc))*circ2(jc)   + (circ1(jc+1))*circ2(jc+1);
03413                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03414                                 }
03415                         }
03416                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03417                 } else {
03418                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03419                         if (neg) {
03420                                 // first set is conjugated (mirrored)
03421                                 for (j=3;j<=maxrin;j=j+2) {
03422                                         jc = j+numr2i-1;
03423                                         t(j)   =  (circ1(jc))*circ2(jc)   - (circ1(jc+1))*circ2(jc+1);
03424                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03425                                 }
03426                         } else {
03427                                 for (j=3;j<=maxrin;j=j+2) {
03428                                         jc = j+numr2i-1;
03429                                         t(j)   =  (circ1(jc))*circ2(jc)   + (circ1(jc+1))*circ2(jc+1);
03430                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03431                                 }
03432                         }
03433                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03434                 }
03435         }
03436 
03437         fftr_d(q,ip);
03438 
03439         qn = -1.0e20;
03440         for (j=1;j<=maxrin;j++) {
03441            if (q(j) >= qn) {
03442                   qn = q(j); jtot = j;
03443            }
03444         }
03445         for (k=-3; k<=3; k++) {
03446                 j = (jtot+k+maxrin-1)%maxrin + 1;
03447                 t7(k+4) = q(j);
03448         }
03449 
03450         prb1d(t7,7,&pos);
03451 
03452         tot = (float)jtot + pos;
03453 
03454         if (q) free(q);
03455         if (t) free(t);
03456 
03457         Dict retvals;
03458         retvals["qn"]  = qn;
03459         retvals["tot"] = tot;
03460         return  retvals;
03461 }
03462 
03463 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03464    //  neg = 0 straight,  neg = 1 mirrored
03465         int nring = numr.size()/3;
03466         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03467         int maxrin = numr[numr.size()-1];
03468         double qn;   float  tot;
03469         float *circ1 = circ1p->get_data();
03470         float *circ2 = circ2p->get_data();
03471 /*
03472 c checks single position, neg is flag for checking mirrored position
03473 c
03474 c  input - fourier transforms of rings!
03475 c  first set is conjugated (mirrored) if neg
03476 c  multiplication by weights!
03477 c       automatic arrays
03478         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03479         double precision  q(maxrin)
03480         double precision  t7(-3:3)
03481 */
03482         float *t;
03483         double t7[7], *q;
03484         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03485         float  pos;
03486 
03487 #ifdef _WIN32
03488         ip = -(int)(log((float)maxrin)/log(2.0f));
03489 #else
03490         ip = -(int) (log2(maxrin));
03491 #endif  //_WIN32
03492 
03493         q = (double*)calloc(maxrin, sizeof(double));
03494         t = (float*)calloc(maxrin, sizeof(float));
03495 
03496 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03497         for (i=1;i<=nring;i++) {
03498                 numr3i = numr(3,i);
03499                 numr2i = numr(2,i);
03500 
03501                 t(1) = circ1(numr2i) * circ2(numr2i);
03502 
03503                 if (numr3i != maxrin) {
03504                         // test .ne. first for speed on some compilers
03505                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03506                         t(2)      = 0.0;
03507 
03508                         if (neg) {
03509                                 // first set is conjugated (mirrored)
03510                                 for (j=3; j<=numr3i; j=j+2) {
03511                                         jc = j+numr2i-1;
03512                                         t(j)   =  (circ1(jc))*circ2(jc)   - (circ1(jc+1))*circ2(jc+1);
03513                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03514                                 }
03515                         } else {
03516                                 for (j=3; j<=numr3i; j=j+2) {
03517                                         jc     = j+numr2i-1;
03518                                         t(j)   =  (circ1(jc))*circ2(jc)   + (circ1(jc+1))*circ2(jc+1);
03519                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03520                                 }
03521                         }
03522                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03523                 } else {
03524                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03525                         if (neg) {
03526                                 // first set is conjugated (mirrored)
03527                                 for (j=3; j<=maxrin; j=j+2) {
03528                                         jc = j+numr2i-1;
03529                                         t(j)   =  (circ1(jc))*circ2(jc)   - (circ1(jc+1))*circ2(jc+1);
03530                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03531                                 }
03532                         } else {
03533                                 for (j=3; j<=maxrin; j=j+2) {
03534                                 jc = j+numr2i-1;
03535                                 t(j)   =  (circ1(jc))*circ2(jc)   + (circ1(jc+1))*circ2(jc+1);
03536                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03537                                 }
03538                         }
03539                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03540                 }
03541         }
03542 
03543         fftr_d(q,ip);
03544 
03545         qn = -1.0e20;
03546         for (j=1;j<=maxrin;j++) {
03547                 //cout << j << "  " << q(j) << endl;
03548                 if (q(j) >= qn) {
03549                         qn = q(j);
03550                         jtot = j;
03551                 }
03552         }
03553 
03554         for (k=-3; k<=3; k++) {
03555                 j = (jtot+k+maxrin-1)%maxrin + 1;
03556                 t7(k+4) = q(j);
03557         }
03558 
03559         prb1d(t7,7,&pos);
03560 
03561         tot = (float)jtot + pos;
03562 
03563         //if (q) free(q);
03564         if (t) free(t);
03565 
03566         Dict retvals;
03567         //tot = 1;
03568         //qn = q(1);
03569         retvals["qn"] = qn;
03570         retvals["tot"] = tot;
03571 
03572         if (q) free(q);
03573 
03574         return  retvals;
03575 }
03576 
03577 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03578         int nring = numr.size()/3;
03579         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03580         int maxrin = numr[numr.size()-1];
03581         double qn; float tot; double qm; float tmt;
03582         float *circ1 = circ1p->get_data();
03583         float *circ2 = circ2p->get_data();
03584 /*
03585 c
03586 c  checks both straight & mirrored positions
03587 c
03588 c  input - fourier transforms of rings!!
03589 c  circ1 already multiplied by weights!
03590 c
03591 */
03592 
03593         // dimension             circ1(lcirc),circ2(lcirc)
03594 
03595         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03596         double *t, *q, t7[7];
03597 
03598         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03599         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03600 
03601         qn  = 0.0f;
03602         qm  = 0.0f;
03603         tot = 0.0f;
03604         tmt = 0.0f;
03605 #ifdef _WIN32
03606         ip = -(int)(log((float)maxrin)/log(2.0f));
03607 #else
03608         ip = -(int)(log2(maxrin));
03609 #endif  //_WIN32
03610   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03611 
03612         //  c - straight  = circ1 * conjg(circ2)
03613         //  zero q array
03614 
03615         q = (double*)calloc(maxrin,sizeof(double));
03616 
03617         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03618         //   zero t array
03619         t = (double*)calloc(maxrin,sizeof(double));
03620 
03621    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03622         for (i=1; i<=nring; i++) {
03623 
03624                 numr3i = numr(3,i);   // Number of samples of this ring
03625                 numr2i = numr(2,i);   // The beginning point of this ring
03626 
03627                 t1   = circ1(numr2i) * circ2(numr2i);
03628                 q(1) += t1;
03629                 t(1) += t1;
03630 
03631                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03632                 if (numr3i == maxrin)  {
03633                         q(2) += t1;
03634                         t(2) += t1;
03635                 } else {
03636                         q(numr3i+1) += t1;
03637                         t(numr3i+1) += t1;
03638                 }
03639 
03640                 for (j=3; j<=numr3i; j += 2) {
03641                         jc     = j+numr2i-1;
03642 
03643 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03644 //                                ----- -----    ----- -----
03645 //                                 t1     t2      t3    t4
03646 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03647 //                                    ----- -----    ----- -----
03648 //                                     t1    t2       t3    t4
03649 
03650                         c1     = circ1(jc);
03651                         c2     = circ1(jc+1);
03652                         d1     = circ2(jc);
03653                         d2     = circ2(jc+1);
03654 
03655                         t1     = c1 * d1;
03656                         t2     = c2 * d2;
03657                         t3     = c1 * d2;
03658                         t4     = c2 * d1;
03659 
03660                         q(j)   += t1 + t2;
03661                         q(j+1) += -t3 + t4;
03662                         t(j)   += t1 - t2;
03663                         t(j+1) += -t3 - t4;
03664                 }
03665         }
03666         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03667         fftr_d(q,ip);
03668 
03669         qn  = -1.0e20;
03670         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03671                 if (q(j) >= qn) {
03672                         qn  = q(j);
03673                         jtot = j;
03674                 }
03675         }
03676 
03677         for (k=-3; k<=3; k++) {
03678                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03679                 t7(k+4) = q(j);
03680         }
03681 
03682         // interpolate
03683         prb1d(t7,7,&pos);
03684         tot = (float)(jtot)+pos;
03685         // Do not interpolate
03686         //tot = (float)(jtot);
03687 
03688         // mirrored
03689         fftr_d(t,ip);
03690 
03691         // find angle
03692         qm = -1.0e20;
03693         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03694                 if ( t(j) >= qm ) {
03695                         qm   = t(j);
03696                         jtot = j;
03697                 }
03698         }
03699 
03700         for (k=-3; k<=3; k++)  {
03701                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03702                 t7(k+4) = t(j);
03703         }
03704 
03705         // interpolate
03706 
03707         prb1d(t7,7,&pos);
03708         tmt = float(jtot) + pos;
03709         // Do not interpolate
03710         //tmt = float(jtot);
03711 
03712         free(t);
03713         free(q);
03714 
03715         Dict retvals;
03716         retvals["qn"] = qn;
03717         retvals["tot"] = tot;
03718         retvals["qm"] = qm;
03719         retvals["tmt"] = tmt;
03720         return retvals;
03721 }
03722 
03723 static std::vector<int> shuffled_range(int first, int last)
03724 {
03725         std::vector<int> v(last - first + 1);
03726         for (unsigned i = 0; i < v.size(); ++i) v[i] = first + i;
03727         for (unsigned i = 0; i < v.size(); ++i) {
03728                 unsigned r = Util::get_irand(0,v.size()-1);
03729                 std::swap( v[r], v[i] );
03730         }
03731         return v;
03732 }
03733 
03734 static Dict Crosrng_rand_ms(EMData* circ1p, EMData* circ2p, vector<int> numr, float previous_max)
03735 {
03736         int nring = numr.size()/3;
03737         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03738         int maxrin = numr[numr.size()-1];
03739         float *circ1 = circ1p->get_data();
03740         float *circ2 = circ2p->get_data();
03741 /*
03742 c
03743 c  checks both straight & mirrored positions
03744 c
03745 c  input - fourier transforms of rings!!
03746 c  circ1 already multiplied by weights!
03747 c
03748 */
03749 
03750         // dimension             circ1(lcirc),circ2(lcirc)
03751 
03752         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03753         double *t, *q, t7[7];
03754 
03755         int   jc, numr3i, numr2i, i, j, k;
03756         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03757 
03758 #ifdef _WIN32
03759         const int ip = -(int)(log((float)maxrin)/log(2.0f));
03760 #else
03761         const int ip = -(int)(log2(maxrin));
03762 #endif  //_WIN32
03763   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03764 
03765         //  c - straight  = circ1 * conjg(circ2)
03766         //  zero q array
03767 
03768         q = (double*)calloc(maxrin,sizeof(double));
03769 
03770         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03771         //   zero t array
03772         t = (double*)calloc(maxrin,sizeof(double));
03773 
03774    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03775         for (i=1; i<=nring; i++) {
03776 
03777                 numr3i = numr(3,i);   // Number of samples of this ring
03778                 numr2i = numr(2,i);   // The beginning point of this ring
03779 
03780                 t1   = circ1(numr2i) * circ2(numr2i);
03781                 q(1) += t1;
03782                 t(1) += t1;
03783 
03784                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03785                 if (numr3i == maxrin)  {
03786                         q(2) += t1;
03787                         t(2) += t1;
03788                 } else {
03789                         q(numr3i+1) += t1;
03790                         t(numr3i+1) += t1;
03791                 }
03792 
03793                 for (j=3; j<=numr3i; j += 2) {
03794                         jc     = j+numr2i-1;
03795 
03796 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03797 //                                ----- -----    ----- -----
03798 //                                 t1     t2      t3    t4
03799 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03800 //                                    ----- -----    ----- -----
03801 //                                     t1    t2       t3    t4
03802 
03803                         c1     = circ1(jc);
03804                         c2     = circ1(jc+1);
03805                         d1     = circ2(jc);
03806                         d2     = circ2(jc+1);
03807 
03808                         t1     = c1 * d1;
03809                         t2     = c2 * d2;
03810                         t3     = c1 * d2;
03811                         t4     = c2 * d1;
03812 
03813                         q(j)   += t1 + t2;
03814                         q(j+1) += -t3 + t4;
03815                         t(j)   += t1 - t2;
03816                         t(j+1) += -t3 - t4;
03817                 }
03818         }
03819         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03820         Util::fftr_d(q,ip);
03821         // mirrored
03822         Util::fftr_d(t,ip);
03823 
03824         std::vector<int> angles = shuffled_range(0, 2*maxrin-1);  // [0..maxrin) - not-mirrored, [maxrin..2*maxrin) - mirrored
03825         bool mirrored = false;
03826         double qn = -1;
03827         int jtot = -1;
03828         for (int pos_j=0; pos_j < 2*maxrin; pos_j++) {//cout <<"  "<<j<<"   "<<q(j-1) <<endl;
03829                 j = angles[pos_j] + 1;
03830                 if (j <= maxrin) {
03831                         // not mirrored
03832                         if (q(j) > qn) {
03833                                 mirrored = false;
03834                                 qn  = q(j);
03835                                 jtot = j;
03836                                 if (qn > previous_max) {
03837                                         break;
03838                                 }
03839                         }
03840                 } else {
03841                         // mirrored
03842                         j -= maxrin;
03843                         if ( t(j) > qn ) {
03844                                 mirrored = true;
03845                                 qn   = t(j);
03846                                 jtot = j;
03847                                 if (qn > previous_max) {
03848                                         break;
03849                                 }
03850                         }
03851                 }
03852         }
03853 
03854         // interpolation
03855         if (! mirrored) {
03856                 for (k=-3; k<=3; k++) {
03857                         j = ((jtot+k+maxrin-1)%maxrin)+1;
03858                         t7(k+4) = q(j);
03859                 }
03860         } else {
03861                 for (k=-3; k<=3; k++)  {
03862                         j = ((jtot+k+maxrin-1)%maxrin) + 1;
03863                         t7(k+4) = t(j);
03864                 }
03865         }
03866 
03867         // interpolate
03868         Util::prb1d(t7,7,&pos);
03869         const float tot = (float)(jtot)+pos;
03870         // Do not interpolate
03871         //      const float tot = (float)(jtot);
03872 
03873         free(t);
03874         free(q);
03875 
03876         Dict retvals;
03877         retvals["qn"] = qn;
03878         retvals["tot"] = tot;
03879         retvals["mirror"] = (mirrored) ? (1) : (0);
03880         return retvals;
03881 }
03882 
03883 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03884         int nring = numr.size()/3;
03885         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03886         int maxrin = numr[numr.size()-1];
03887         double qn; float tot; double qm; float tmt;
03888         float *circ1 = circ1p->get_data();
03889         float *circ2 = circ2p->get_data();
03890 /*
03891 c
03892 c  checks both straight & mirrored positions
03893 c
03894 c  input - fourier transforms of rings!!
03895 c  circ1 already multiplied by weights!
03896 c
03897 */
03898 
03899         // dimension             circ1(lcirc),circ2(lcirc)
03900 
03901         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03902         double *t, *q;
03903 
03904         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03905         float t1, t2, t3, t4, c1, c2, d1, d2;
03906 
03907         qn  = 0.0f;
03908         qm  = 0.0f;
03909         tot = 0.0f;
03910         tmt = 0.0f;
03911 #ifdef _WIN32
03912         ip = -(int)(log((float)maxrin)/log(2.0f));
03913 #else
03914         ip = -(int)(log2(maxrin));
03915 #endif  //_WIN32
03916   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03917 
03918         //  c - straight  = circ1 * conjg(circ2)
03919         //  zero q array
03920 
03921         q = (double*)calloc(maxrin,sizeof(double));
03922 
03923         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03924         //   zero t array
03925         t = (double*)calloc(maxrin,sizeof(double));
03926 
03927    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03928         for (i=1; i<=nring; i++) {
03929 
03930                 numr3i = numr(3,i);   // Number of samples of this ring
03931                 numr2i = numr(2,i);   // The beginning point of this ring
03932 
03933                 t1   = circ1(numr2i) * circ2(numr2i);
03934                 q(1) += t1;
03935                 t(1) += t1;
03936 
03937                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03938                 if (numr3i == maxrin)  {
03939                         q(2) += t1;
03940                         t(2) += t1;
03941                 } else {
03942                         q(numr3i+1) += t1;
03943                         t(numr3i+1) += t1;
03944                 }
03945 
03946                 for (j=3; j<=numr3i; j += 2) {
03947                         jc     = j+numr2i-1;
03948 
03949 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03950 //                                ----- -----    ----- -----
03951 //                                 t1     t2      t3    t4
03952 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03953 //                                    ----- -----    ----- -----
03954 //                                     t1    t2       t3    t4
03955 
03956                         c1     = circ1(jc);
03957                         c2     = circ1(jc+1);
03958                         d1     = circ2(jc);
03959                         d2     = circ2(jc+1);
03960 
03961                         t1     = c1 * d1;
03962                         t2     = c2 * d2;
03963                         t3     = c1 * d2;
03964                         t4     = c2 * d1;
03965 
03966                         q(j)   += t1 + t2;
03967                         q(j+1) += -t3 + t4;
03968                         t(j)   += t1 - t2;
03969                         t(j+1) += -t3 - t4;
03970                 }
03971         }
03972         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03973         fftr_d(q,ip);
03974 
03975         qn  = -1.0e20;
03976 
03977         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03978         int jstep = static_cast<int>(delta/360.0*maxrin);
03979         if (jstep < 1) { jstep = 1; }
03980 
03981         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03982                 if (q(j) >= qn) {
03983                         qn  = q(j);
03984                         jtot = j;
03985                 }
03986         }
03987 
03988         //for (k=-3; k<=3; k++) {
03989         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03990         //      t7(k+4) = q(j);
03991         //}
03992 
03993         // interpolate
03994         //prb1d(t7,7,&pos);
03995         //tot = (float)(jtot)+pos;
03996         // Do not interpolate
03997         tot = (float)(jtot);
03998 
03999         // mirrored
04000         fftr_d(t,ip);
04001 
04002         // find angle
04003         qm = -1.0e20;
04004         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
04005                 if ( t(j) >= qm ) {
04006                         qm   = t(j);
04007                         jtot = j;
04008                 }
04009         }
04010 
04011         //for (k=-3; k<=3; k++)  {
04012         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
04013         //      t7(k+4) = t(j);
04014         //}
04015 
04016         // interpolate
04017 
04018         //prb1d(t7,7,&pos);
04019         //tmt = float(jtot) + pos;
04020         // Do not interpolate
04021         tmt = float(jtot);
04022 
04023         free(t);
04024         free(q);
04025 
04026         Dict retvals;
04027         retvals["qn"] = qn;
04028         retvals["tot"] = tot;
04029         retvals["qm"] = qm;
04030         retvals["tmt"] = tmt;
04031         return retvals;
04032 }
04033 
04034 
04035 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
04036 // flag 0 - straight, 1 - mirror
04037 
04038         int nring = numr.size()/3;
04039         int maxrin = numr[numr.size()-1];
04040         double qn; float tot;
04041         float *circ1 = circ1p->get_data();
04042         float *circ2 = circ2p->get_data();
04043 
04044         double *q;
04045 
04046         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04047         float c1, c2, d1, d2;
04048 
04049         qn  = 0.0f;
04050         tot = 0.0f;
04051 #ifdef _WIN32
04052         ip = -(int)(log((float)maxrin)/log(2.0f));
04053 #else
04054         ip = -(int)(log2(maxrin));
04055 #endif  //_WIN32
04056 
04057         //  c - straight  = circ1 * conjg(circ2)
04058         //  zero q array
04059 
04060         q = (double*)calloc(maxrin,sizeof(double));
04061         int neg = 1-2*flag;
04062    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04063 
04064         for (i=1; i<=nring; i++) {
04065 
04066                 numr3i = numr(3,i);   // Number of samples of this ring
04067                 numr2i = numr(2,i);   // The beginning point of this ring
04068 
04069                 q(1) += circ1(numr2i) * circ2(numr2i);
04070 
04071                 float t1 = circ1(numr2i+1) * circ2(numr2i+1);
04072                 if (numr3i == maxrin)   q(2) += t1;
04073                 else  q(numr3i+1) += t1;
04074 
04075                 for (j=3; j<=numr3i; j += 2) {
04076                         jc     = j+numr2i-1;
04077 
04078         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04079         //                                ----- -----    ----- -----
04080         //                                 t1     t2      t3    t4
04081 
04082                         c1     = circ1(jc);
04083                         c2     = circ1(jc+1);
04084                         d1     = circ2(jc);
04085                         d2     = circ2(jc+1);
04086 
04087                         q(j)   +=  c1 * d1 + c2 * d2*neg;
04088                         q(j+1) += -c1 * d2 + c2 * d1*neg;
04089                 }
04090         }
04091 
04092         fftr_d(q,ip);
04093 
04094         qn  = -1.0e20;
04095         // psi = 0 should correspond to psi_pos = 1 (meaning no change in in-plane rotation)
04096         int psi_pos = int(psi/360.0*maxrin+0.5) + 1;
04097         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
04098 
04099         for (k=-psi_range; k<=psi_range; k++) {
04100                 j = ( k + psi_pos + maxrin - 1)%maxrin+1;
04101                 if (q(j) >= qn) {
04102                         qn  = q(j);
04103                         jtot = j;
04104                 }
04105         }
04106 
04107         tot = (float)(jtot);
04108         free(q);
04109 
04110         Dict retvals;
04111         retvals["qn"] = qn;
04112         retvals["tot"] = tot;
04113         return retvals;
04114 }
04115 
04116 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
04117 // Computes both straight and mirrored
04118 
04119         int nring = numr.size()/3;
04120         int maxrin = numr[numr.size()-1];
04121         double qn; float tot; double qm; float tmt;
04122         float *circ1 = circ1p->get_data();
04123         float *circ2 = circ2p->get_data();
04124 
04125         double *t, *q;
04126 
04127         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04128         float t1, t2, t3, t4, c1, c2, d1, d2;
04129 
04130         qn  = 0.0f;
04131         qm  = 0.0f;
04132         tot = 0.0f;
04133         tmt = 0.0f;
04134 #ifdef _WIN32
04135         ip = -(int)(log((float)maxrin)/log(2.0f));
04136 #else
04137         ip = -(int)(log2(maxrin));
04138 #endif  //_WIN32
04139 
04140         //  c - straight  = circ1 * conjg(circ2)
04141         //  zero q array
04142 
04143         q = (double*)calloc(maxrin,sizeof(double));
04144         
04145         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04146         //   zero t array
04147         t = (double*)calloc(maxrin,sizeof(double));
04148         
04149    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04150         
04151         for (i=1; i<=nring; i++) {
04152 
04153                 numr3i = numr(3,i);   // Number of samples of this ring
04154                 numr2i = numr(2,i);   // The beginning point of this ring
04155 
04156                 t1   = circ1(numr2i) * circ2(numr2i);
04157                 q(1) += t1;
04158                 t(1) += t1;
04159                 
04160                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
04161                 if (numr3i == maxrin)  {
04162                         q(2) += t1;
04163                         t(2) += t1;
04164                 } else {
04165                         q(numr3i+1) += t1;
04166                         t(numr3i+1) += t1;
04167                 }
04168 
04169                 for (j=3; j<=numr3i; j += 2) {
04170                         jc     = j+numr2i-1;
04171 
04172         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04173         //                                ----- -----    ----- -----
04174         //                                 t1     t2      t3    t4
04175 
04176                         c1     = circ1(jc);
04177                         c2     = circ1(jc+1);
04178                         d1     = circ2(jc);
04179                         d2     = circ2(jc+1);
04180 
04181                         t1     = c1 * d1;
04182                         t3     = c1 * d2;
04183                         t2     = c2 * d2;
04184                         t4     = c2 * d1;
04185 
04186                         q(j)   +=  t1 + t2;
04187                         q(j+1) += -t3 + t4;
04188                         t(j)   +=  t1 - t2;
04189                         t(j+1) += -t3 - t4;
04190                 }
04191         }
04192 
04193         fftr_d(q,ip);
04194 
04195         qn  = -1.0e20;
04196         // psi = 0 should correspond to psi_pos = 1
04197         int psi_pos = int(psi/360.0*maxrin+0.5) + 1;
04198         const int psi_range = int(psi_max/360.0*maxrin + 0.5);
04199 
04200         for (k=-psi_range; k<=psi_range; k++) {
04201                 j = (k+psi_pos+maxrin-1)%maxrin+1;
04202                 if (q(j) >= qn) {
04203                         qn  = q(j);
04204                         jtot = j;
04205                 }
04206         }
04207 
04208         tot = (float)(jtot);
04209         free(q);
04210 
04211     // mirrored
04212         fftr_d(t,ip);
04213 
04214         qm  = -1.0e20;
04215 
04216         for (k=-psi_range; k<=psi_range; k++) {
04217                 j = (k+psi_pos+maxrin-1)%maxrin+1;
04218                 if (t(j) >= qm) {
04219                         qm  = t(j);
04220                         jtot = j;
04221                 }
04222         }
04223 
04224         tmt = (float)(jtot);
04225         free(t);
04226 
04227         Dict retvals;
04228         retvals["qn"] = qn;
04229         retvals["tot"] = tot;
04230         retvals["qm"] = qm;
04231         retvals["tmt"] = tmt;
04232         return retvals;
04233 }
04234 
04235 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04236         int nring = numr.size()/3;
04237         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04238         int maxrin = numr[numr.size()-1];
04239         double qn; float tot;
04240         float *circ1 = circ1p->get_data();
04241         float *circ2 = circ2p->get_data();
04242 /*
04243 c
04244 c  checks only straight position
04245 c
04246 c  input - fourier transforms of rings!!
04247 c  circ1 already multiplied by weights!
04248 c
04249 */
04250 
04251         // dimension             circ1(lcirc),circ2(lcirc)
04252 
04253         // q(maxrin), t7(-3:3)  //maxrin+2 removed
04254         double *q, t7[7];
04255 
04256         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04257         float c1, c2, d1, d2, pos;
04258 
04259         qn  = 0.0;
04260         tot = 0.0;
04261 #ifdef _WIN32
04262         ip = -(int)(log((float)maxrin)/log(2.0f));
04263 #else
04264    ip = -(int)(log2(maxrin));
04265 #endif  //_WIN32
04266         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04267 
04268         //  c - straight  = circ1 * conjg(circ2)
04269         //  zero q array
04270 
04271         q = (double*)calloc(maxrin,sizeof(double));
04272 
04273                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04274         for (i=1; i<=nring; i++) {
04275 
04276                 numr3i = numr(3,i);   // Number of samples of this ring
04277                 numr2i = numr(2,i);   // The beginning point of this ring
04278 
04279                 q(1) += circ1(numr2i) * circ2(numr2i);
04280 
04281                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04282                 else                 q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04283 
04284                 for (j=3; j<=numr3i; j += 2) {
04285                         jc     = j+numr2i-1;
04286 
04287 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04288 //                                ----- -----    ----- -----
04289 //                                 t1     t2      t3    t4
04290 
04291                         c1     = circ1(jc);
04292                         c2     = circ1(jc+1);
04293                         d1     = circ2(jc);
04294                         d2     = circ2(jc+1);
04295 
04296                         q(j)   +=  c1 * d1 + c2 * d2;
04297                         q(j+1) += -c1 * d2 + c2 * d1;
04298                 }
04299         }
04300 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04301         fftr_d(q,ip);
04302 
04303         qn  = -1.0e20;
04304         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04305                 if (q(j) >= qn) {
04306                         qn  = q(j);
04307                         jtot = j;
04308                 }
04309         }
04310 
04311         for (k=-3; k<=3; k++)  {
04312                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04313                 t7(k+4) = q(j);
04314         }
04315 
04316         // interpolate
04317         prb1d(t7,7,&pos);
04318         tot = (float)(jtot)+pos;
04319         // Do not interpolate
04320         //*tot = (float)(jtot);
04321 
04322         free(q);
04323 
04324         Dict retvals;
04325         retvals["qn"] = qn;
04326         retvals["tot"] = tot;
04327         return retvals;
04328 }
04329 
04330 #define  dout(i,j)        dout[i+maxrin*j]
04331 #define  circ1b(i)        circ1b[i-1]
04332 #define  circ2b(i)        circ2b[i-1]
04333 
04334 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04335 
04336    // dimension         circ1(lcirc),circ2(lcirc)
04337 
04338         int   ip, jc, numr3i, numr2i, i, j;
04339         float t1, t2, t3, t4, c1, c2, d1, d2;
04340 
04341         int nring = numr.size()/3;
04342         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04343         int maxrin = numr[numr.size()-1];
04344 
04345         float* circ1b = circ1->get_data();
04346         float* circ2b = circ2->get_data();
04347 
04348         // t(maxrin), q(maxrin)  // removed +2
04349         double *t, *q;
04350 
04351         q = (double*)calloc(maxrin,sizeof(double));
04352         t = (double*)calloc(maxrin,sizeof(double));
04353 
04354 #ifdef _WIN32
04355         ip = -(int)(log((float)maxrin)/log(2.0f));
04356 #else
04357         ip = -(int)(log2(maxrin));
04358 #endif  //_WIN32
04359 
04360         //  q - straight  = circ1 * conjg(circ2)
04361 
04362         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04363 
04364         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04365 
04366         for (i=1; i<=nring; i++) {
04367 
04368                 numr3i = numr(3,i);
04369                 numr2i = numr(2,i);
04370 
04371                 t1   = circ1b(numr2i) * circ2b(numr2i);
04372                 q(1) += t1;
04373                 t(1) += t1;
04374 
04375                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04376                 if (numr3i == maxrin)  {
04377                         q(2) += t1;
04378                         t(2) += t1;
04379                 } else {
04380                         q(numr3i+1) += t1;
04381                         t(numr3i+1) += t1;
04382                 }
04383 
04384                 for (j=3; j<=numr3i; j=j+2) {
04385                         jc     = j+numr2i-1;
04386 
04387                         c1     = circ1b(jc);
04388                         c2     = circ1b(jc+1);
04389                         d1     = circ2b(jc);
04390                         d2     = circ2b(jc+1);
04391 
04392                         t1     = c1 * d1;
04393                         t3     = c1 * d2;
04394                         t2     = c2 * d2;
04395                         t4     = c2 * d1;
04396 
04397                         q(j)   +=  t1 + t2;
04398                         q(j+1) += -t3 + t4;
04399                         t(j)   +=  t1 - t2;
04400                         t(j+1) += -t3 - t4;
04401                 }
04402         }
04403 
04404         // straight
04405         fftr_d(q,ip);
04406 
04407         // mirrored
04408         fftr_d(t,ip);
04409 
04410         EMData* out = new EMData();
04411         out->set_size(maxrin,2,1);
04412         float *dout = out->get_data();
04413         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04414         //out->set_size(maxrin,1,1);
04415         //float *dout = out->get_data();
04416         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04417         free(t);
04418         free(q);
04419         return out;
04420 }
04421 
04422 
04423 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04424 
04425         int maxrin = numr[numr.size()-1];
04426 
04427         vector<float> r(2*maxrin);
04428 
04429         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04430 
04431         return r;
04432 }
04433 
04434 #define  dout(i,j)        dout[i+maxrin*j]
04435 #define  circ1b(i)        circ1b[i-1]
04436 #define  circ2b(i)        circ2b[i-1]
04437 
04438 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04439 
04440    // dimension         circ1(lcirc),circ2(lcirc)
04441 
04442         int   ip, jc, numr3i, numr2i, i, j;
04443         float t1, t2, t3, t4, c1, c2, d1, d2;
04444 
04445         int nring = numr.size()/3;
04446         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04447         int maxrin = numr[numr.size()-1];
04448 
04449         float* circ1b = circ1->get_data();
04450         float* circ2b = circ2->get_data();
04451 
04452 #ifdef _WIN32
04453         ip = -(int)(log((float)maxrin)/log(2.0f));
04454 #else
04455         ip = -(int)(log2(maxrin));
04456 #endif  //_WIN32
04457         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04458 
04459         //  q - straight  = circ1 * conjg(circ2)
04460 
04461         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04462 
04463         for (i=1; i<=nring; i++) {
04464 
04465                 numr3i = numr(3,i);
04466                 numr2i = numr(2,i);
04467 
04468                 t1   = circ1b(numr2i) * circ2b(numr2i);
04469                 q(1) += t1;
04470                 t(1) += t1;
04471 
04472                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04473                 if (numr3i == maxrin)  {
04474                         q(2) += t1;
04475                         t(2) += t1;
04476                 } else {
04477                         q(numr3i+1) += t1;
04478                         t(numr3i+1) += t1;
04479                 }
04480 
04481                 for (j=3; j<=numr3i; j=j+2) {
04482                         jc     = j+numr2i-1;
04483 
04484                         c1     = circ1b(jc);
04485                         c2     = circ1b(jc+1);
04486                         d1     = circ2b(jc);
04487                         d2     = circ2b(jc+1);
04488 
04489                         t1     = c1 * d1;
04490                         t3     = c1 * d2;
04491                         t2     = c2 * d2;
04492                         t4     = c2 * d1;
04493 
04494                         q(j)   +=  t1 + t2;
04495                         q(j+1) += -t3 + t4;
04496                         t(j)   +=  t1 - t2;
04497                         t(j+1) += -t3 - t4;
04498                 }
04499         }
04500         // straight
04501         fftr_q(q,ip);
04502         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04503 
04504         // mirrored
04505         fftr_q(t,ip);
04506 }
04507 
04508 
04509 
04510 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04511 {
04512 
04513         int   ip, jc, numr3i, numr2i, i, j;
04514         float c1, c2, d1, d2;
04515 
04516         int nring = numr.size()/3;
04517         int maxrin = numr[numr.size()-1];
04518 
04519         float* circ1b = circ1->get_data();
04520         float* circ2b = circ2->get_data();
04521 
04522         double *q;
04523 
04524         q = (double*)calloc(maxrin,sizeof(double));
04525 
04526 #ifdef _WIN32
04527         ip = -(int)(log((float)maxrin)/log(2.0f));
04528 #else
04529         ip = -(int)(log2(maxrin));
04530 #endif  //_WIN32
04531 
04532          //  q - straight  = circ1 * conjg(circ2)
04533 
04534         for (i=1;i<=nring;i++) {
04535 
04536                 numr3i = numr(3,i);
04537                 numr2i = numr(2,i);
04538 
04539                 q(1) += circ1b(numr2i) * circ2b(numr2i);
04540 
04541                 if (numr3i == maxrin)   q(2) += circ1b(numr2i+1) * circ2b(numr2i+1);
04542                 else             q(numr3i+1) += circ1b(numr2i+1) * circ2b(numr2i+1);
04543 
04544                 for (j=3;j<=numr3i;j=j+2) {
04545                         jc     = j+numr2i-1;
04546 
04547                         c1     = circ1b(jc);
04548                         c2     = circ1b(jc+1);
04549                         d1     = circ2b(jc);
04550                         d2     = circ2b(jc+1);
04551 
04552                         q(j)   +=  c1 * d1 + c2 * d2;
04553                         q(j+1) += -c1 * d2 + c2 * d1;
04554                 }
04555         }
04556 
04557         // straight
04558         fftr_d(q,ip);
04559 
04560         EMData* out = new EMData();
04561         out->set_size(maxrin,1,1);
04562         float *dout = out->get_data();
04563         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04564         free(q);
04565         return out;
04566 
04567 }
04568 
04569 
04570 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04571 {
04572 
04573         int   ip, jc, numr3i, numr2i, i, j;
04574         float c1, c2, d1, d2;
04575 
04576         int nring = numr.size()/3;
04577         int maxrin = numr[numr.size()-1];
04578 
04579         float* circ1b = circ1->get_data();
04580         float* circ2b = circ2->get_data();
04581 
04582         double *t;
04583 
04584         t = (double*)calloc(maxrin,sizeof(double));
04585 
04586 #ifdef _WIN32
04587         ip = -(int)(log((float)maxrin)/log(2.0f));
04588 #else
04589         ip = -(int)(log2(maxrin));
04590 #endif  //_WIN32
04591 
04592          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04593 
04594         for (i=1;i<=nring;i++) {
04595 
04596                 numr3i = numr(3,i);
04597                 numr2i = numr(2,i);
04598                 t(1) += circ1b(numr2i) * circ2b(numr2i);
04599 
04600                 if (numr3i == maxrin)  t(2) += circ1b(numr2i+1) * circ2b(numr2i+1);
04601                 else          t(numr3i+1) += circ1b(numr2i+1) * circ2b(numr2i+1);
04602 
04603                 for (j=3;j<=numr3i;j=j+2) {
04604                         jc     = j+numr2i-1;
04605 
04606                         c1     = circ1b(jc);
04607                         c2     = circ1b(jc+1);
04608                         d1     = circ2b(jc);
04609                         d2     = circ2b(jc+1);
04610 
04611                         t(j)   +=  c1 * d1 - c2 * d2;
04612                         t(j+1) += -c1 * d2 - c2 * d1;
04613                 }
04614         }
04615 
04616         // mirrored
04617         fftr_d(t,ip);
04618 
04619         EMData* out = new EMData();
04620         out->set_size(maxrin,1,1);
04621         float *dout = out->get_data();
04622         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04623         free(t);
04624         return out;
04625 
04626 }
04627 
04628 #undef circ1b
04629 #undef circ2b
04630 #undef dout
04631 
04632 #undef  circ1
04633 #undef  circ2
04634 #undef  t
04635 #undef  q
04636 #undef  b
04637 #undef  t7
04638 
04639 
04640 #define    QUADPI                   3.141592653589793238462643383279502884197
04641 #define    PI2                      2*QUADPI
04642 
04643 float Util::ener(EMData* ave, vector<int> numr) {
04644         ENTERFUNC;
04645         long double ener,en;
04646 
04647         int nring = numr.size()/3;
04648         float *aveptr = ave->get_data();
04649 
04650         ener = 0.0;
04651         for (int i=1; i<=nring; i++) {
04652                 int numr3i = numr(3,i);
04653                 int np     = numr(2,i)-1;
04654                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04655                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04656                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04657                 ener += en/numr3i;
04658         }
04659         EXITFUNC;
04660         return static_cast<float>(ener);
04661 }
04662 
04663 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04664         ENTERFUNC;
04665         long double ener, en;
04666         float arg, cs, si;
04667 
04668         int nima = data.size();
04669         int nring = numr.size()/3;
04670         int maxrin = numr(3,nring);
04671 
04672         ener = 0.0;
04673         for (int i=1; i<=nring; i++) {
04674                 int numr3i = numr(3,i);
04675                 int np     = numr(2,i)-1;
04676                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04677                 float temp1 = 0.0, temp2 = 0.0;
04678                 for (int kk=0; kk<nima; kk++) {
04679                         float *ptr = data[kk]->get_data();
04680                         temp1 += ptr[np];
04681                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04682                 }
04683                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04684                 for (int j=2; j<numr3i; j+=2) {
04685                         float tempr = 0.0, tempi = 0.0;
04686                         for (int kk=0; kk<nima; kk++) {
04687                                 float *ptr = data[kk]->get_data();
04688                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04689                                 cs = cos(arg);
04690                                 si = sin(arg);
04691                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04692                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04693                         }
04694                         en += tq*(tempr*tempr+tempi*tempi);
04695                 }
04696                 ener += en/numr3i;
04697         }
04698         EXITFUNC;
04699         return static_cast<float>(ener);
04700 }
04701 
04702 void Util::update_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04703         int nring = numr.size()/3;
04704         float *ave = avep->get_data();
04705         float *dat = datp->get_data();
04706         int i, j, numr3i, np;
04707         float  arg, cs, si;
04708         int maxrin = numr(3,nring);
04709         if(mirror == 1) { //for mirrored data has to be conjugated
04710                 for (i=1; i<=nring; i++) {
04711                         numr3i = numr(3,i);
04712                         np     = numr(2,i)-1;
04713                         ave[np]   += dat[np];
04714                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04715                         for (j=2; j<numr3i; j=j+2) {
04716                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04717                                 cs = cos(arg);
04718                                 si = sin(arg);
04719                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04720                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04721                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04722                         }
04723                 }
04724         } else {
04725                 for (i=1; i<=nring; i++) {
04726                         numr3i = numr(3,i);
04727                         np     = numr(2,i)-1;
04728                         ave[np]   += dat[np];
04729                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04730                         for (j=2; j<numr3i; j=j+2) {
04731                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04732                                 cs = cos(arg);
04733                                 si = sin(arg);
04734                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04735                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04736                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04737                         }
04738                 }
04739         }
04740         avep->update();
04741         EXITFUNC;
04742 }
04743 
04744 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04745         int nring = numr.size()/3;
04746         float *ave = avep->get_data();
04747         float *dat = datp->get_data();
04748         int i, j, numr3i, np;
04749         float  arg, cs, si;
04750         int maxrin = numr(3,nring);
04751         if(mirror == 1) { //for mirrored data has to be conjugated
04752                 for (i=1; i<=nring; i++) {
04753                         numr3i = numr(3,i);
04754                         np     = numr(2,i)-1;
04755                         ave[np]   -= dat[np];
04756                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04757                         for (j=2; j<numr3i; j=j+2) {
04758                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04759                                 cs = cos(arg);
04760                                 si = sin(arg);
04761                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04762                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04763                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04764                         }
04765                 }
04766         } else {
04767                 for (i=1; i<=nring; i++) {
04768                         numr3i = numr(3,i);
04769                         np     = numr(2,i)-1;
04770                         ave[np]   -= dat[np];
04771                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04772                         for (j=2; j<numr3i; j=j+2) {
04773                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04774                                 cs = cos(arg);
04775                                 si = sin(arg);
04776                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04777                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04778                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04779                         }
04780                 }
04781         }
04782         avep->update();
04783         EXITFUNC;
04784 }
04785 
04786 
04787 #undef    QUADPI
04788 #undef    PI2
04789 
04790 #undef  numr
04791 #undef  circ
04792 
04793 
04794 #define QUADPI   3.141592653589793238462643383279502884197
04795 #define PI2      QUADPI*2
04796 #define deg_rad  QUADPI/180.0
04797 #define rad_deg  180.0/QUADPI
04798 
04799 struct ori_t
04800 {
04801     int iphi;
04802     int itht;
04803     int id;
04804 };
04805 
04806 
04807 struct cmpang
04808 {
04809     bool operator()( const ori_t& a, const ori_t& b )
04810     {
04811         if( a.itht != b.itht )
04812         {
04813             return a.itht < b.itht;
04814         }
04815 
04816         return a.iphi < b.iphi;
04817     }
04818 };
04819 
04820 
04821 vector<double> Util::cml_weights(const vector<float>& cml){
04822         static const int NBIN = 100;
04823         int nline=cml.size()/2;
04824         vector<double> weights(nline);
04825 
04826         vector<ori_t> angs(nline);
04827         for( int i=0; i < nline; ++i ) {
04828                 angs[i].iphi = int( NBIN*cml[2*i] );
04829                 angs[i].itht = int( NBIN*cml[2*i+1] );
04830                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04831                 angs[i].id = i;
04832         }
04833 
04834         //std::cout << "# of angs: " << angs.size() << std::endl;
04835 
04836         std::sort( angs.begin(), angs.end(), cmpang() );
04837 
04838         vector<float> newphi;
04839         vector<float> newtht;
04840         vector< vector<int> > indices;
04841 
04842         int curt_iphi = -1;
04843         int curt_itht = -1;
04844         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04845                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04846                         Assert( indices.size() > 0 );
04847                         indices.back().push_back(angs[i].id);
04848                 } else {
04849                         curt_iphi = angs[i].iphi;
04850                         curt_itht = angs[i].itht;
04851 
04852                         newphi.push_back( float(curt_iphi)/NBIN );
04853                         newtht.push_back( float(curt_itht)/NBIN );
04854                         indices.push_back( vector<int>(1,angs[i].id) );
04855                 }
04856         }
04857 
04858         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04859 
04860 
04861         int num_agl = newphi.size();
04862 
04863         if(num_agl>2) {
04864                 vector<double> w=Util::vrdg(newphi, newtht);
04865 
04866                 Assert( w.size()==newphi.size() );
04867                 Assert( indices.size()==newphi.size() );
04868 
04869                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04870                     /*
04871                     std::cout << "phi,tht,w,n: ";
04872                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04873                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04874                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04875                     std::cout << indices[i].size() << "(";
04876                     */
04877 
04878                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04879                             int id = indices[i][j];
04880                             weights[id] = w[i]/indices[i].size();
04881                             //std::cout << id << " ";
04882                     }
04883 
04884                     //std::cout << ")" << std::endl;
04885 
04886                 }
04887         } else {
04888                 cout<<"warning in Util.cml_weights"<<endl;
04889                 double val = PI2/float(nline);
04890                 for(int i=0; i<nline; i++)  weights[i]=val;
04891         }
04892 
04893         return weights;
04894 
04895 }
04896 
04897 /****************************************************
04898  * New code for common-lines
04899  ****************************************************/
04900 
04901 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04902 {
04903         int i;
04904         int nx=img->get_xsize();
04905         float *img_ptr  = img->get_data();
04906         float *line_ptr = line->get_data();
04907         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04908         img->update();
04909 }
04910 
04911 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04912         int j;
04913         int nx = sino->get_xsize();
04914         int i = nx * pos_line;
04915         float r1, r2;
04916         float *line_ptr = line->get_data();
04917         float *sino_ptr = sino->get_data();
04918         for (j=ilf;j<=ihf; j += 2) {
04919                 r1 = line_ptr[j];
04920                 r2 = line_ptr[j + 1];
04921                 sino_ptr[i + j - ilf] = r1;
04922                 sino_ptr[i + j - ilf + 1] = r2;
04923                 sino_ptr[i + nx * nblines + j - ilf] = r1;
04924                 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04925         }
04926         sino->update();
04927 }
04928 
04929 vector<double> Util::cml_init_rot(vector<float> Ori){
04930         int nb_ori = Ori.size() / 4;
04931         int i, ind;
04932         float ph, th, ps;
04933         double cph, cth, cps, sph, sth, sps;
04934         vector<double> Rot(nb_ori*9);
04935         for (i=0; i<nb_ori; ++i){
04936                 ind = i*4;
04937                 // spider convention phi=psi-90, psi=phi+90
04938                 ph = Ori[ind+2]-90;
04939                 th = Ori[ind+1];
04940                 ps = Ori[ind]+90;
04941                 ph *= deg_rad;
04942                 th *= deg_rad;
04943                 ps *= deg_rad;
04944                 // pre-calculate some trigo stuffs
04945                 cph = cos(ph);
04946                 cth = cos(th);
04947                 cps = cos(ps);
04948                 sph = sin(ph);
04949                 sth = sin(th);
04950                 sps = sin(ps);
04951                 // fill rotation matrix
04952                 ind = i*9;
04953                 Rot[ind] = cph*cps-cth*sps*sph;
04954                 Rot[ind+1] = cph*sps+cth*cps*sph;
04955                 Rot[ind+2] = sth*sph;
04956                 Rot[ind+3] = -sph*cps-cth*sps*cph;
04957                 Rot[ind+4] = -sph*sps+cth*cps*cph;
04958                 Rot[ind+5] = sth*cph;
04959                 Rot[ind+6] = sth*sps;
04960                 Rot[ind+7] = -sth*cps;
04961                 Rot[ind+8] = cth;
04962         }
04963 
04964         return Rot;
04965 }
04966 
04967 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04968         float ph, ps;
04969         double cph, cth, cps, sph, sth, sps;
04970         int ind = iprj*9;
04971         // spider convention phi=psi-90, psi=phi+90
04972         ph = nps-90;
04973         ps = nph+90;
04974         ph *= deg_rad;
04975         th *= deg_rad;
04976         ps *= deg_rad;
04977         // pre-calculate some trigo stuffs
04978         cph = cos(ph);
04979         cth = cos(th);
04980         cps = cos(ps);
04981         sph = sin(ph);
04982         sth = sin(th);
04983         sps = sin(ps);
04984         // fill rotation matrix
04985         Rot[ind] = (float)(cph*cps-cth*sps*sph);
04986         Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04987         Rot[ind+2] = (float)(sth*sph);
04988         Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04989         Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04990         Rot[ind+5] = (float)(sth*cph);
04991         Rot[ind+6] = (float)(sth*sps);
04992         Rot[ind+7] = (float)(-sth*cps);
04993         Rot[ind+8] = (float)(cth);
04994 
04995         return Rot;
04996 }
04997 
04998 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04999         vector<int> com(2*(n_prj - 1));
05000         int a = i_prj*9;
05001         int i, b, c;
05002         int n1=0, n2=0;
05003         float vmax = 1 - 1.0e-6f;
05004         double r11, r12, r13, r23, r31, r32, r33;
05005 
05006         c = 0;
05007         for (i=0; i<n_prj; ++i){
05008                 if (i!=i_prj){
05009                         b = i*9;
05010                         // this is equivalent to R = A*B'
05011                         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
05012                         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
05013                         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
05014                         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
05015                         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
05016                         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
05017                         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
05018                         if (r33 > vmax) {
05019                             n2 = 270;
05020                             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
05021                         }
05022                         else if (r33 < -vmax) {
05023                             n2 = 270;
05024                             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
05025                         } else {
05026                             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
05027                             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
05028                             if (n1 < 0) {n1 += 360;}
05029                             if (n2 <= 0) {n2 = abs(n2);}
05030                             else {n2 = 360 - n2;}
05031                         }
05032 
05033                         if (n1 >= 360){n1 = n1 % 360;}
05034                         if (n2 >= 360){n2 = n2 % 360;}
05035 
05036                         // store common-lines
05037                         b = c*2;
05038                         com[b] = n1;
05039                         com[b+1] = n2;
05040                         ++c;
05041                 }
05042         }
05043 
05044     return com;
05045 
05046 }
05047 
05048 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
05049         vector<int> com(2*n_lines);
05050         int a=0, b, c, l;
05051         int n1=0, n2=0, mem=-1;
05052         float vmax = 1 - 1.0e-6f;
05053         double r11, r12, r13, r23, r31, r32, r33;
05054         c = 0;
05055         for (l=0; l<n_lines; ++l){
05056                 c = 2*l;
05057                 if (seq[c]!=mem){
05058                     mem = seq[c];
05059                     a = seq[c]*9;
05060                 }
05061                 b = seq[c+1]*9;
05062 
05063                 // this is equivalent to R = A*B'
05064                 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
05065                 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
05066                 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
05067                 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
05068                 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
05069                 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
05070                 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
05071                 if (r33 > vmax) {
05072                     n2 = 270;
05073                     n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
05074                 }
05075                 else if (r33 < -vmax) {
05076                     n2 = 270;
05077                     n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
05078                 } else {
05079                     n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
05080                     n1 = nint180((float)(rad_deg*atan2(r13, r23)));
05081                     if (n1 < 0) {n1 += 360;}
05082                     if (n2 <= 0) {n2 = abs(n2);}
05083                     else {n2 = 360 - n2;}
05084                 }
05085                 if (n1 >= 360){n1 = n1 % 360;}
05086                 if (n2 >= 360){n2 = n2 % 360;}
05087 
05088                 // store common-lines
05089                 com[c] = n1;
05090                 com[c+1] = n2;
05091         }
05092 
05093         return com;
05094 
05095 }
05096 
05097 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
05098         // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
05099         vector<double> cml(2*nlines); // [phi, theta] / line
05100         float ph1, th1;
05101         float ph2, th2;
05102         double nx, ny, nz;
05103         double norm;
05104         double sth1=0, sph1=0, cth1=0, cph1=0;
05105         double sth2, sph2, cth2, cph2;
05106         int l, ind, c;
05107         int mem = -1;
05108         for (l=0; l<nlines; ++l){
05109                 c = 2*l;
05110                 if (seq[c]!=mem){
05111                         mem = seq[c];
05112                         ind = 4*seq[c];
05113                         ph1 = Ori[ind]*deg_rad;
05114                         th1 = Ori[ind+1]*deg_rad;
05115                         sth1 = sin(th1);
05116                         sph1 = sin(ph1);
05117                         cth1 = cos(th1);
05118                         cph1 = cos(ph1);
05119                 }
05120                 ind = 4*seq[c+1];
05121                 ph2 = Ori[ind]*deg_rad;
05122                 th2 = Ori[ind+1]*deg_rad;
05123                 sth2 = sin(th2);
05124                 cth2 = cos(th2);
05125                 sph2 = sin(ph2);
05126                 cph2 = cos(ph2);
05127                 // cross product
05128                 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
05129                 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
05130                 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
05131                 norm = sqrt(nx*nx+ny*ny+nz*nz);
05132                 nx /= norm;
05133                 ny /= norm;
05134                 nz /= norm;
05135                 // apply mirror if need
05136                 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
05137                 // compute theta and phi
05138                 cml[c+1] = acos(nz);
05139                 if (cml[c+1] == 0) {cml[c] = 0;}
05140                 else {
05141                         cml[c+1] *= rad_deg;
05142                         if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
05143                         cml[c] = rad_deg * atan2(nx, ny);
05144                         cml[c] = fmod(360 + cml[c], 360);
05145 
05146                 }
05147         }
05148 
05149         return cml;
05150 }
05151 
05152 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
05153         double res = 0;
05154         double buf = 0;
05155         float* line_1;
05156         float* line_2;
05157         int i, n, ind;
05158         int lnlen = data[0]->get_xsize();
05159         for (n=0; n<n_lines; ++n) {
05160                 ind = n*2;
05161                 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
05162                 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
05163                 buf = 0;
05164                 for (i=0; i<lnlen; ++i) {
05165                     buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
05166                 }
05167                 res += buf * weights[n];
05168         }
05169 
05170         return res;
05171 
05172 }
05173 
05174 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
05175                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05176         // res: [best_disc, best_ipsi]
05177         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
05178         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
05179         vector<double> res(2);
05180         int lnlen = data[0]->get_xsize();
05181         int end = 2*(n_prj-1);
05182         double disc, buf, bdisc, tmp;
05183         int n, i, ipsi, ind, bipsi, c;
05184         float* line_1;
05185         float* line_2;
05186         bdisc = 1.0e6;
05187         bipsi = -1;
05188         // loop psi
05189         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05190                 // discrepancy
05191                 disc = 0;
05192                 c = 0;
05193                 for (n=0; n<n_prj; ++n) {
05194                         if(n!=iprj) {
05195                                 ind = 2*c;
05196                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05197                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05198                                 buf = 0;
05199                                 for (i=0; i<lnlen; ++i) {
05200                                         tmp = line_1[i]-line_2[i];
05201                                         buf += tmp*tmp;
05202                                 }
05203                                 disc += buf * weights[iw[c]];
05204                                 ++c;
05205                         }
05206                 }
05207                 // select the best value
05208                 if (disc <= bdisc) {
05209                         bdisc = disc;
05210                         bipsi = ipsi;
05211                 }
05212                 // update common-lines
05213                 for (i=0; i<end; i+=2){
05214                         com[i] += d_psi;
05215                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05216                 }
05217         }
05218         res[0] = bdisc;
05219         res[1] = float(bipsi);
05220 
05221         return res;
05222 }
05223 
05224 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
05225                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05226         // res: [best_disc, best_ipsi]
05227         // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
05228         // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
05229         vector<double> res(2);
05230         int lnlen = data[0]->get_xsize();
05231         int end = 2*(n_prj-1);
05232         double disc, buf, bdisc, tmp;
05233         int n, i, ipsi, ind, bipsi, c;
05234         float* line_1;
05235         float* line_2;
05236         bdisc = 1.0e6;
05237         bipsi = -1;
05238         // loop psi
05239         for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05240                 // discrepancy
05241                 disc = 0;
05242                 c = 0;
05243                 for (n=0; n<n_prj; ++n) {
05244                         if(n!=iprj) {
05245                                 ind = 2*c;
05246                                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05247                                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05248                                 buf = 0;
05249                                 for (i=0; i<lnlen; ++i) {
05250                                         tmp = line_1[i]-line_2[i];
05251                                         buf += tmp*tmp;
05252                                 }
05253                                 disc += buf;
05254                                 ++c;
05255                         }
05256                 }
05257                 // select the best value
05258                 if (disc <= bdisc) {
05259                         bdisc = disc;
05260                         bipsi = ipsi;
05261                 }
05262                 // update common-lines
05263                 for (i=0; i<end; i+=2){
05264                         com[i] += d_psi;
05265                         if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05266                 }
05267         }
05268         res[0] = bdisc;
05269         res[1] = float(bipsi);
05270 
05271         return res;
05272 }
05273 
05274 #undef  QUADPI
05275 #undef  PI2
05276 #undef  deg_rad
05277 #undef  rad_deg
05278 
05279 /****************************************************
05280  * END OF NEW CODE FOR COMMON-LINES
05281  ****************************************************/
05282 
05283 // helper function for k-means
05284 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05285         ENTERFUNC;
05286 
05287         int nima = data.size();
05288         vector<float> res(nima);
05289         double result = 0.;
05290         double valmin = 1.0e20;
05291         int valpos = -1;
05292 
05293         for (int kk=0; kk<nima; kk++){
05294         result = 0;
05295 
05296         float *y_data = data[kk]->get_data();
05297         float *x_data = image->get_data();
05298         long totsize = image->get_xsize()*image->get_ysize();
05299         for (long i = 0; i < totsize; i++) {
05300             double temp = x_data[i]- y_data[i];
05301             result += temp*temp;
05302         }
05303         result /= totsize;
05304         res[kk] = (float)result;
05305 
05306         if(result<valmin) {valmin = result; valpos = kk;}
05307 
05308         }
05309 
05310         Dict retvals;
05311         retvals["dist"] = res;
05312         retvals["pos"]  = valpos;
05313 
05314         EXITFUNC;
05315         return retvals;
05316 
05317 }
05318 
05319 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05320         ENTERFUNC;
05321 
05322         int nima = data.size();
05323         vector<float> res(nima);
05324         double result = 0.;
05325         double valmin = 1.0e20;
05326         int valpos = -1;
05327 
05328         for (int kk=0; kk<nima; kk++){
05329         result = 0;
05330         //validate_input_args(image, data[kk]);
05331 
05332         float *y_data = data[kk]->get_data();
05333         float *x_data = image->get_data();
05334 
05335         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05336         int nx  = data[kk]->get_xsize();
05337         int ny  = data[kk]->get_ysize();
05338         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05339         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05340 
05341         int ixb = 2*((nx+1)%2);
05342         int iyb = ny%2;
05343         int iz = 0;
05344 
05345         for ( int iy = 0; iy <= ny-1; iy++) {
05346             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05347                 int ii = ix + (iy  + iz * ny)* lsd2;
05348                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05349             }
05350         }
05351         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05352             int ii = (iy  + iz * ny)* lsd2;
05353             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05354             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05355         }
05356         if(nx%2 == 0) {
05357             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05358                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05359                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05360                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05361             }
05362 
05363         }
05364         result *= 2;
05365         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05366         if(ny%2 == 0) {
05367             int ii = (ny/2  + iz * ny)* lsd2;
05368             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05369         }
05370         if(nx%2 == 0) {
05371             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05372             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05373             if(ny%2 == 0) {
05374                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05375                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05376             }
05377         }
05378 
05379         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05380         res[kk] = (float)result;
05381 
05382         if(result<valmin) {valmin = result; valpos = kk;}
05383 
05384         }
05385 
05386         Dict retvals;
05387         retvals["dist"] = res;
05388         retvals["pos"]  = valpos;
05389 
05390         EXITFUNC;
05391         return retvals;
05392 }
05393 
05394 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05395     long int d2 = group2[s2 - 1] - group2[0];
05396     long int p2 = 0;
05397     long int i1 = 0;
05398     long int i2 = 0;
05399     long int max = 0;
05400     long int cont = 0;
05401     long int i = 0;
05402     int stop1 = 0;
05403     int stop2 = 0;
05404 
05405     for (i=0; i<s1; i++) {
05406         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05407         if (p2 >= s2) {p2 = s2 - 1;}
05408         i1 = p2;
05409         i2 = p2;
05410         max = s2;
05411         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05412 
05413         stop1 = 0;
05414         stop2 = 0;
05415         while (max--) {
05416             if (group1[i] == group2[i1]) {
05417                 if (flag) {stb[cont] = group1[i];}
05418                 cont++;
05419                 break;
05420             }
05421             if (group2[i1] < group1[i]) {stop1=1;}
05422             if (group1[i] == group2[i2]) {
05423                 if (flag) {stb[cont] = group1[i];}
05424                 cont++;
05425                 break;
05426             }
05427             if (group2[i2] > group1[i]) {stop2=1;}
05428             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05429 
05430             if (stop1 & stop2) {break;}
05431             i1--;
05432             i2++;
05433             if (i1 < 0) {i1 = 0;}
05434             if (i2 >= s2) {i2 = s2 - 1;}
05435         }
05436         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05437     }
05438 
05439     return cont;
05440 }
05441 
05442 
05443 
05444 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*(size_t)nx]
05445 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05446 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05447 {
05448         /* Exception Handle */
05449         if (!img) {
05450                 throw NullPointerException("NULL input image");
05451         }
05452         /* ============================== */
05453 
05454         // Get the size of the input image
05455         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05456         /* ============================== */
05457 
05458 
05459         /* Exception Handle */
05460         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)
05461         {
05462                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05463                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05464         }
05465         /* ============================== */
05466 
05467 
05468         /*    Calculation of the start point */
05469         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05470         /* ============================*/
05471 
05472 
05473         /* Calculation of the size of the decimated image */
05474         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05475         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05476         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05477         if(r1>1){r1=1;}
05478         if(r2>1){r2=1;}
05479         if(r3>1){r3=1;}
05480         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05481         /* ===========================================*/
05482 
05483 
05484         EMData* img2 = new EMData();
05485         img2->set_size(new_nx,new_ny,new_nz);
05486         float *new_ptr = img2->get_data();
05487         float *old_ptr = img->get_data();
05488         int iptr, jptr, kptr = 0;
05489         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05490                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05491                         for (int i=new_st_x; i<nx; i+=x_step) {
05492                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05493                         iptr++;}
05494                 jptr++;}
05495         kptr++;}
05496         img2->update();
05497         return img2;
05498 }
05499 #undef old_ptr
05500 #undef new_ptr
05501 
05502 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05503 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05504 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05505 {
05506         /* Exception Handle */
05507         if (!img) throw NullPointerException("NULL input image");
05508         /* ============================== */
05509 
05510         // Get the size of the input image
05511         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05512         /* ============================== */
05513 
05514         /* Exception Handle */
05515         if(new_nx>nx || new_ny>ny || new_nz>nz)
05516                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05517         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)
05518                 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05519         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))))
05520                 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05521         /* ============================== */
05522 
05523         /*    Calculation of the start point */
05524         int  new_st_x = nx/2-new_nx/2 + x_offset,
05525              new_st_y = ny/2-new_ny/2 + y_offset,
05526              new_st_z = nz/2-new_nz/2 + z_offset;
05527         /* ============================== */
05528 
05529         /* Exception Handle */
05530         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05531                 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05532         /* ============================== */
05533 
05534         EMData* wind = img->copy_empty_head();
05535         wind->set_size(new_nx, new_ny, new_nz);
05536         float *outp=wind->get_data();
05537         float *inp=img->get_data();
05538 
05539         for (int k=0; k<new_nz; k++)
05540                 for(int j=0; j<new_ny; j++)
05541                         for(int i=0; i<new_nx; i++)
05542                                 outp(i,j,k) = inp(i,j,k);
05543         wind->update();
05544         return wind;
05545 }
05546 #undef inp
05547 #undef outp
05548 
05549 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05550 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05551 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)
05552 {
05553         /* Exception Handle */
05554         if (!img)  throw NullPointerException("NULL input image");
05555         /* ============================== */
05556 
05557         // Get the size of the input image
05558         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05559         /* ============================== */
05560 
05561         /* Exception Handle */
05562         if(new_nx<nx || new_ny<ny || new_nz<nz)
05563                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05564         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)
05565                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05566         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))))
05567                 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05568         /* ============================== */
05569 
05570         EMData* pading = img->copy_head();
05571         pading->set_size(new_nx, new_ny, new_nz);
05572         float *inp  = img->get_data();
05573         float *outp = pading->get_data();
05574 
05575 
05576         /* Calculation of the average and the circumference values for background substitution
05577         =======================================================================================*/
05578         float background;
05579 
05580         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05581         else if (strcmp(params,"circumference")==0) {
05582                 float sum1=0.0f;
05583                 size_t cnt=0;
05584                 for(int i=0;i<nx;i++) {
05585                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05586                         cnt+=2;
05587                 }
05588                 if(nz-1 == 0) {
05589                         for (int j=1;j<ny-1;j++) {
05590                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05591                                 cnt+=2;
05592                         }
05593                 } else {
05594                         for (int k=1;k<nz-1;k++) {
05595                                 for (int j=1;j<ny-1;j++) {
05596                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05597                                         cnt+=2;
05598                                 }
05599                         }
05600                 }
05601                 background = sum1/cnt;
05602         } else {
05603                 background = static_cast<float>( atof( params ) );
05604         }
05605         /*=====================================================================================*/
05606 
05607          /*Initial Padding */
05608         int new_st_x=0,new_st_y=0,new_st_z=0;
05609         for (int k=0;k<new_nz;k++)
05610                 for(int j=0;j<new_ny;j++)
05611                         for (int i=0;i<new_nx;i++)
05612                                 outp(i,j,k)=background;
05613         /*============================== */
05614 
05615         /*    Calculation of the start point */
05616         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05617         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05618         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05619         /* ============================== */
05620 
05621         for (int k=0;k<nz;k++)
05622                 for(int j=0;j<ny;j++)
05623                         for(int i=0;i<nx;i++)
05624                                 outp(i,j,k)=inp(i,j,k);
05625         pading->update();
05626         return pading;
05627 }
05628 #undef inp
05629 #undef outp
05630 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05631 
05632 void Util::colreverse(float* beg, float* end, int nx) {
05633         float* tmp = new float[nx];
05634         int n = (end - beg)/nx;
05635         int nhalf = n/2;
05636         for (int i = 0; i < nhalf; i++) {
05637                 // swap col i and col n-1-i
05638                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05639                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05640                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05641         }
05642         delete[] tmp;
05643 }
05644 
05645 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05646 {
05647         int nxy = nx*ny;
05648         colreverse(beg, end, nxy);
05649 }
05650 
05651 
05652 void Util::cyclicshift(EMData *image, Dict params) {
05653 
05654         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05655 
05656         int dx = params["dx"];
05657         int dy = params["dy"];
05658         int dz = params["dz"];
05659 
05660         // The reverse trick we're using shifts to the left (a negative shift)
05661         int nx = image->get_xsize();
05662         dx %= nx;
05663         if (dx < 0) dx += nx;
05664         int ny = image->get_ysize();
05665         dy %= ny;
05666         if (dy < 0) dy += ny;
05667         int nz = image->get_zsize();
05668         dz %= nz;
05669         if (dz < 0) dz += nz;
05670 
05671         int mx = -(dx - nx);
05672         int my = -(dy - ny);
05673         int mz = -(dz - nz);
05674 
05675         float* data = image->get_data();
05676         // x-reverses
05677         if (mx != 0) {
05678                 for (int iz = 0; iz < nz; iz++)
05679                        for (int iy = 0; iy < ny; iy++) {
05680                                 // reverses for column iy
05681                                 size_t offset = nx*iy + (size_t)nx*ny*iz; // starting location for column iy in slice iz
05682                                 reverse(&data[offset],&data[offset+mx]);
05683                                 reverse(&data[offset+mx],&data[offset+nx]);
05684                                 reverse(&data[offset],&data[offset+nx]);
05685                         }
05686         }
05687         // y-reverses
05688         if (my != 0) {
05689                 for (int iz = 0; iz < nz; iz++) {
05690                         size_t offset = (size_t)nx*ny*iz;
05691                         colreverse(&data[offset], &data[offset + my*nx], nx);
05692                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05693                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05694                 }
05695         }
05696         if (mz != 0) {
05697                 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05698                 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05699                 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05700         }
05701         image->update();
05702 }
05703 
05704 //-----------------------------------------------------------------------------------------------------------------------
05705 
05706 
05707 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05708 {
05709         if (image->is_complex())
05710                 throw ImageFormatException("Cannot do histogram on Fourier image");
05711         //float hmax, hmin;
05712         float *imageptr=0, *maskptr=0;
05713         int nx=image->get_xsize();
05714         int ny=image->get_ysize();
05715         int nz=image->get_zsize();
05716 
05717         if(mask != NULL){
05718                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05719                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05720                 maskptr =mask->get_data();
05721         }
05722         if( nbins == 0) nbins = nx;
05723         vector <float> freq(2*nbins, 0.0);
05724 
05725         imageptr=image->get_data();
05726         if( hmin == hmax ) {
05727                 if(mask == NULL) {
05728                         hmax = image->get_attr("maximum");
05729                         hmin = image->get_attr("minimum");
05730                 } else {
05731                         bool  First = true;
05732                         for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05733                         if (maskptr[i]>=0.5f) {
05734                                         if(First) {
05735                                                 hmax = imageptr[i];
05736                                                 hmin = imageptr[i];
05737                                                 First = false;
05738                                         } else {
05739                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05740                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05741                                         }
05742                                 }
05743                         }
05744                 }
05745         }
05746         float hdiff = hmax - hmin;
05747         float ff = (nbins-1)/hdiff;
05748         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05749         if(mask == NULL) {
05750                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05751                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05752                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05753                 }
05754         } else {
05755                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05756                         if(maskptr[i] >= 0.5) {
05757                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05758                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05759                         }
05760                 }
05761         }
05762         return freq;
05763 }
05764 
05765 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05766 {
05767         /* Exception Handle */
05768         if (img->is_complex() || ref->is_complex())
05769                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05770 
05771         if(mask != NULL){
05772                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05773                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05774         /* ===================================================== */
05775 
05776         /* Image size calculation */
05777         size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05778         size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05779         /* ===================================================== */
05780 
05781         /* The reference image attributes */
05782         float *ref_ptr = ref->get_data();
05783         float ref_h_min = ref->get_attr("minimum");
05784         float ref_h_max = ref->get_attr("maximum");
05785         float ref_h_avg = ref->get_attr("mean");
05786         float ref_h_sig = ref->get_attr("sigma");
05787         /* ===================================================== */
05788 
05789         /* Input image under mask attributes */
05790         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05791 
05792         vector<float> img_data = Util::infomask(img, mask);
05793         float img_avg = img_data[0];
05794         float img_sig = img_data[1];
05795 
05796         /* The image under mask -- size calculation */
05797         int cnt=0;
05798         for(size_t i=0;i<size_img;++i)
05799                 if (mask_ptr[i]>0.5f)
05800                                 cnt++;
05801         /* ===================================================== */
05802 
05803         /* Histogram of reference image calculation */
05804         float ref_h_diff = ref_h_max - ref_h_min;
05805 
05806         #ifdef _WIN32
05807                 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05808         #else
05809                 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05810         #endif  //_WIN32
05811 
05812         float *ref_freq_bin = new float[3*hist_len];
05813 
05814         //initialize value in each bin to zero
05815         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05816 
05817         for (size_t i = 0;i < size_ref;++i) {
05818                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05819                 ref_freq_bin[L]++;
05820         }
05821         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05822 
05823         //Parameters Calculation (i.e) 'A' x + 'B'
05824         float A = ref_h_sig/img_sig;
05825         float B = ref_h_avg - (A*img_avg);
05826 
05827         vector<float> args;
05828         args.push_back(A);
05829         args.push_back(B);
05830 
05831         vector<float> scale;
05832         scale.push_back(1.e-7f*A);
05833         scale.push_back(-1.e-7f*B);
05834 
05835         vector<float> ref_freq_hist;
05836         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05837 
05838         vector<float> data;
05839         data.push_back(ref_h_diff);
05840         data.push_back(ref_h_min);
05841 
05842         Dict parameter;
05843 
05844         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05845         parameter["args"] = args;
05846         parameter["scale"]= scale;
05847         parameter["data"] = data;
05848         parameter["ref_freq_bin"] = ref_freq_hist;
05849         parameter["size_img"]=(double)size_img;
05850         parameter["hist_len"]=hist_len;
05851         /* ===================================================== */
05852 
05853         return parameter;
05854 }
05855 
05856 
05857 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)
05858 {
05859         float *img_ptr = img->get_data();
05860         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05861 
05862         int *img_freq_bin = new int[3*hist_len];
05863         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05864         for(size_t i = 0;i < size_img;++i) {
05865                 if(mask_ptr[i] > 0.5f) {
05866                         float img_xn = img_ptr[i]*PA + PB;
05867                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05868                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05869                 }
05870         }
05871         int freq_hist = 0;
05872 
05873         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);
05874         freq_hist = (-freq_hist);
05875         return static_cast<float>(freq_hist);
05876 }
05877 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05878 #define    QUADPI                       3.141592653589793238462643383279502884197
05879 #define    DGR_TO_RAD                   QUADPI/180
05880 #define    DM(I)                        DM          [I-1]
05881 #define    SS(I)                        SS          [I-1]
05882 Dict Util::CANG(float PHI,float THETA,float PSI)
05883 {
05884         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05885         vector<float>   DM,SS;
05886 
05887         for(int i =0;i<9;i++) DM.push_back(0);
05888 
05889         for(int i =0;i<6;i++) SS.push_back(0);
05890 
05891         CPHI = cos(double(PHI)*DGR_TO_RAD);
05892         SPHI = sin(double(PHI)*DGR_TO_RAD);
05893         CTHE = cos(double(THETA)*DGR_TO_RAD);
05894         STHE = sin(double(THETA)*DGR_TO_RAD);
05895         CPSI = cos(double(PSI)*DGR_TO_RAD);
05896         SPSI = sin(double(PSI)*DGR_TO_RAD);
05897 
05898         SS(1) = float(CPHI);
05899         SS(2) = float(SPHI);
05900         SS(3) = float(CTHE);
05901         SS(4) = float(STHE);
05902         SS(5) = float(CPSI);
05903         SS(6) = float(SPSI);
05904 
05905         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05906         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05907         DM(3) = float(-STHE*CPSI);
05908         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05909         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05910         DM(6) = float(STHE*SPSI);
05911         DM(7) = float(STHE*CPHI);
05912         DM(8) = float(STHE*SPHI);
05913         DM(9) = float(CTHE);
05914 
05915         Dict DMnSS;
05916         DMnSS["DM"] = DM;
05917         DMnSS["SS"] = SS;
05918 
05919         return(DMnSS);
05920 }
05921 #undef SS
05922 #undef DM
05923 #undef QUADPI
05924 #undef DGR_TO_RAD
05925 //-----------------------------------------------------------------------------------------------------------------------
05926 struct t_BPCQ_line{
05927         int rX;     // radius along X axe
05928         int offset; // offset of voxel in volume - beginning of the line
05929         float xbb;  // XBB coefficient
05930         float ybb;  // YBB coefficient
05931 };
05932 
05933 void Util::BPCQ( EMData *B, EMData *CUBE, const int radius )
05934 {
05935         if (B->is_complex()) {
05936                 B->do_ift_inplace();
05937                 B->depad();
05938         }
05939 
05940         const Transform * transform = B->get_attr("xform.projection");
05941         Dict transform_params = transform->get_params("spider");
05942 
05943         // ---- build DM matrix (transform matrix) - convert from 3x4 matrix to 2x3 matrix (only 2 first rows are nedeed)
05944         std::vector<float> DM = transform->get_matrix();
05945         DM[3+0] = DM[4+0];
05946         DM[3+1] = DM[4+1];
05947         DM[3+2] = DM[4+2];
05948 
05949         delete transform;
05950 
05951         const int NSAM = B->get_xsize();
05952         const int NROW = B->get_ysize();
05953 
05954         // buffer "lines_to_process" should be aligned to size of cache line (usually 64 or 128 bytes)
05955         t_BPCQ_line * lines_to_process;
05956 #ifdef _WIN32
05957         if ( (lines_to_process = (t_BPCQ_line *)_aligned_malloc( 4*radius*radius*sizeof(t_BPCQ_line), 256 )) == NULL )
05958 #else
05959         if ( posix_memalign( reinterpret_cast<void**>(&lines_to_process), 256, 4*radius*radius*sizeof(t_BPCQ_line) ) != 0 )
05960 #endif  //_WIN32
05961         {
05962                 throw std::bad_alloc();
05963         }
05964         t_BPCQ_line * first_free_line = lines_to_process;
05965 
05966         // calculate lines parameters
05967         {
05968                 //  Unsure about sign of shifts, check later PAP 06/28/09
05969                 const float x_shift_plus_center = float(NSAM/2 +1) + float(transform_params[ "tx" ]);
05970                 const float y_shift_plus_center = float(NROW/2 +1) + float(transform_params[ "ty" ]);
05971 
05972                 const int sizeX = CUBE->get_xsize();
05973                 const int sizeY = CUBE->get_ysize();
05974                 const int sizeZ = CUBE->get_zsize();
05975 
05976                 const int centerX = sizeX / 2;
05977                 const int centerY = sizeY / 2;
05978                 const int centerZ = sizeZ / 2;
05979 
05980                 const int minRZ = (centerZ >= radius) ? (-radius) : (-centerZ);
05981                 const int maxRZ = (sizeZ > centerZ+radius) ? (radius) : (sizeZ-centerZ-1);
05982 
05983                 for ( int rZ=minRZ; rZ<=maxRZ; ++rZ ) {
05984                         for ( int rY=-radius; rY<=radius; ++rY ) {
05985                                 const int sqRX = radius*radius - rZ*rZ - rY*rY;
05986                                 if (sqRX >= 0) {
05987 #ifdef  _WIN32
05988                                         first_free_line->rX     = static_cast<int>( floor(sqrtf(sqRX)+0.5) );
05989 #else
05990                                         first_free_line->rX     = static_cast<int>( roundf(sqrtf(sqRX)) );
05991 #endif  //_WIN32
05992                                         first_free_line->offset = sizeX*( centerY+rY + sizeY*(centerZ+rZ) ) + centerX - first_free_line->rX;
05993                                         first_free_line->xbb    = rZ*DM[2] + rY*DM[1] + x_shift_plus_center;
05994                                         first_free_line->ybb    = rZ*DM[5] + rY*DM[4] + y_shift_plus_center;
05995                                         ++first_free_line;
05996                                 }
05997                         }
05998                 }
05999         }
06000 
06001         const float * const Bptr = B->get_data();
06002         float * const CUBE_begin = CUBE->get_data();
06003 
06004         // update voxels in volume
06005         // this loop takes more than 95% of calculations time spent in Util::BPCQ function
06006         for ( t_BPCQ_line * iLine = lines_to_process; iLine < first_free_line; ++iLine ) {
06007                 const int rX_first = -(iLine->rX);
06008                 const int rX_last  =   iLine->rX;
06009                 float  *CUBE_ptr = CUBE_begin + iLine->offset;
06010                 for (int rX=rX_first; rX<=rX_last; ++rX, ++CUBE_ptr) {
06011                         const float XB  = rX * DM[0] + iLine->xbb;
06012                         const float YB  = rX * DM[3] + iLine->ybb;
06013                         const int IQX = int(XB);
06014                         const int IQY = int(YB);
06015                         if ( IQX < 1 || IQX >= NSAM || IQY < 1 || IQY >= NROW )  continue;
06016                         const float DIPX = XB-IQX;
06017                         const float DIPY = YB-IQY;
06018                         const float b00 = Bptr[IQX-1+((IQY-1)*NSAM)];
06019                         const float b01 = Bptr[IQX-1+((IQY-0)*NSAM)];
06020                         const float b10 = Bptr[IQX-0+((IQY-1)*NSAM)];
06021                         const float b11 = Bptr[IQX-0+((IQY-0)*NSAM)];
06022                         *(CUBE_ptr) = *(CUBE_ptr) + b00 + DIPY*(b01-b00) + DIPX*(b10-b00+DIPY*(b11-b10-b01+b00));
06023                 }
06024         }
06025 
06026         free(lines_to_process);
06027 }
06028 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
06029 #define    W(i,j)                       Wptr    [i+(j)*Wnx]
06030 #define    PROJ(i,j)            PROJptr [i+(j)*NNNN]
06031 #define    SS(i,j)              SS          [i+(j)*6]
06032 
06033 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
06034 {
06035 
06036         --K; // now indexes are started from 0
06037 
06038         int NSAM = PROJ->get_xsize();
06039         int NROW = PROJ->get_ysize();
06040 
06041         if (PROJ->is_fftpadded()) {
06042                 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);  // correction for DFT image
06043         }
06044 
06045     const int ntotal = NSAM*NROW;
06046         const float q = 2.0f;
06047         const float qt = 8.0f/q;
06048         //  Fix for padding 2x
06049         const int ipad = 1;
06050         NSAM *= ipad;
06051         NROW *= ipad;
06052         const int NNNN = NSAM+2-(NSAM%2);
06053         const int NX2 = NSAM/2;
06054         const int NR2  = NROW/2;
06055 
06056         const int NANG = int(SS.size())/6;
06057 
06058         EMData* W = new EMData();
06059         const int Wnx = NNNN/2;
06060         W->set_size(Wnx,NROW,1);
06061         W->to_zero();
06062         float *Wptr = W->get_data();
06063 
06064         for (int L=0; L<NANG; L++) {
06065                 const float tmp1 = SS(2,K)*SS(3,L)*(SS(0,K)*SS(0,L) + SS(1,K)*SS(1,L)) - SS(2,L)*SS(3,K);
06066                 const float tmp2 = SS(3,L)*( SS(0,K)*SS(1,L) - SS(0,L)*SS(1,K) );
06067                 float OX = SS(5,K)*tmp2 + SS(4,K)*tmp1;
06068                 float OY = SS(4,K)*tmp2 - SS(5,K)*tmp1;
06069                 if(OX < 0.0f) {
06070                         OX = -OX;
06071                         OY = -OY;
06072                 }
06073 
06074                 if( OX > 1.0e-6f || fabs(OY) > 1.0e-6f ) {
06075                         for (int J=0; J<NROW; ++J) {
06076                                 const float JY_OY = (J > NR2) ? ((J-NROW)*OY) : (J*OY);
06077                                 int xma = NX2;
06078                                 int xmi = 0;
06079                                 const float fxma = ( q-JY_OY) / OX;
06080                                 const float fxmi = (-q-JY_OY) / OX;
06081                                 if (fxma < xmi || fxmi > xma ) {
06082                                         continue;
06083                                 }
06084                                 if (fxma < xma) {
06085                                         xma = static_cast<int>(fxma+0.5f);
06086                                 }
06087                                 if (fxmi > xmi) {
06088                                         xmi = static_cast<int>(fxmi+0.5f);
06089                                 }
06090                                 for( int I=xmi; I<=xma; ++I ) {
06091                                         const float Y = I*OX + JY_OY;
06092                                         W(I,J) += exp(-qt*Y*Y);
06093                                 }
06094                         }
06095                 } else {
06096                         for (int J=0; J<NROW; ++J) {
06097                                 for (int I=0; I<NNNN/2; ++I) {
06098                                         W(I,J) += 1.0f;
06099                                 }
06100                         }
06101                 }
06102         }
06103 
06104     EMData* proj_in = PROJ;
06105 
06106         const bool realOnInput = PROJ->is_real();
06107     if (realOnInput) {
06108                 // copy input image and run DFT on it
06109                 PROJ = PROJ->norm_pad( false, ipad);
06110                 PROJ->do_fft_inplace();
06111                 PROJ->update();
06112     }
06113     float * PROJptr = PROJ->get_data();
06114 
06115         const float osnr = 1.0f/SNR;
06116         const float WNRMinv = 1.0f/W(0,0);
06117         for (int J=0; J<NROW; ++J)  {
06118                 float sy = (J > NR2) ? (J - NROW) : (J);
06119                 sy /= NROW;
06120                 sy *= sy;
06121                 for (int I=0; I<NNNN; I+=2) {
06122                         const int KX = I/2;
06123                         const float temp = W(KX,J)*WNRMinv;
06124                         float WW = temp/(temp*temp + osnr);
06125                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
06126                         const float sx = float(KX) / NSAM;
06127                         WW *= exp(qt*(sy + sx*sx));
06128                         PROJ(I,J)   *= WW;
06129                         PROJ(I+1,J) *= WW;
06130                 }
06131         }
06132         delete W; W = 0;
06133 
06134         PROJ->do_ift_inplace();
06135         PROJ->depad();
06136 
06137         if (realOnInput) {
06138                 // copy data back to input image
06139                 float* data_src = PROJ->get_data();
06140                 float* data_dst = proj_in->get_data();
06141                 memcpy( data_dst, data_src, ntotal * sizeof(float) );
06142                 delete PROJ;
06143         }
06144 
06145         proj_in->update();
06146 }
06147 /*
06148 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
06149 {
06150         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
06151         float WW,OX,OY,Y;
06152 
06153         NSAM = PROJ->get_xsize();
06154         NROW = PROJ->get_ysize();
06155         //  Fix for padding 2x
06156         int ntotal = NSAM*NROW;
06157         int ipad = 1;
06158         NSAM *= ipad;
06159         NROW *= ipad;
06160         NNNN = NSAM+2-(NSAM%2);
06161         NR2  = NROW/2;
06162 
06163         NANG = int(SS.size())/6;
06164 
06165         EMData* W = new EMData();
06166         int Wnx = NNNN/2;
06167         W->set_size(Wnx,NROW,1);
06168         W->to_zero();
06169         float *Wptr = W->get_data();
06170         float *PROJptr = PROJ->get_data();
06171         for (L=1; L<=NANG; L++) {
06172                 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);
06173                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
06174                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
06175                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
06176         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
06177 
06178                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
06179                         for(int J=1;J<=NROW;J++) {
06180                                 JY = (J-1);
06181                                 if(JY > NR2) JY=JY-NROW;
06182                                 for(int I=1;I<=NNNN/2;I++) {
06183                                         Y =  fabs(OX * (I-1) + OY * JY);
06184                                         if(Y < 2.0f) {
06185                                         W(I,J) += exp(-4*Y*Y);
06186         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
06187                                 }
06188                         }
06189                 } else {
06190                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
06191                 }
06192         }
06193         EMData* proj_in = PROJ;
06194 
06195         PROJ = PROJ->norm_pad( false, ipad);
06196         PROJ->do_fft_inplace();
06197         PROJ->update();
06198         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
06199         PROJptr = PROJ->get_data();
06200 
06201         float WNRMinv,temp;
06202         float osnr = 1.0f/SNR;
06203         WNRMinv = 1.0f/W(1,1);
06204         for(int J=1;J<=NROW;J++)
06205                 for(int I=1;I<=NNNN;I+=2) {
06206                         KX           = (I+1)/2;
06207                         temp         = W(KX,J)*WNRMinv;
06208                         WW           = temp/(temp*temp + osnr);
06209                         PROJ(I,J)   *= WW;
06210                         PROJ(I+1,J) *= WW;
06211                 }
06212         delete W; W = 0;
06213         PROJ->do_ift_inplace();
06214         PROJ->depad();
06215 
06216         float* data_src = PROJ->get_data();
06217         float* data_dst = proj_in->get_data();
06218 
06219         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
06220 
06221         proj_in->update();
06222 
06223         delete PROJ;
06224 }
06225 */
06226 #undef PROJ
06227 #undef W
06228 #undef SS
06229 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
06230 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
06231 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
06232 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
06233 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
06234 #define    CC(i)                        CC          [i-1]
06235 #define    CP(i)                        CP          [i-1]
06236 #define    VP(i)                        VP          [i-1]
06237 #define    VV(i)                        VV          [i-1]
06238 #define    AMAX1(i,j)                   i>j?i:j
06239 #define    AMIN1(i,j)                   i<j?i:j
06240 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
06241 {
06242         float rad2deg =(180.0f/3.1415926f);
06243         float deg2rad = (3.1415926f/180.0f);
06244 
06245         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
06246 
06247         NSAM = PROJ->get_xsize();
06248         NROW = PROJ->get_ysize();
06249 
06250         if (PROJ->is_fftpadded()) {
06251                 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);  // correction for DFT image
06252         }
06253 
06254         NNNN = NSAM+2-(NSAM%2);
06255         NR2  = NROW/2;
06256         NANG = int(SS.size())/6;
06257 
06258         float RI[9];
06259         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06260         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06261         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06262         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06263         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06264         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06265         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06266         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06267         RI(3,3)=SS(3,NUMP);
06268 
06269         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06270 
06271         EMData* W = new EMData();
06272         int Wnx = NNNN/2;
06273         W->set_size(NNNN/2,NROW,1);
06274         W->to_one();
06275         float *Wptr = W->get_data();
06276 
06277         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06278 
06279         for (L=1; L<=NANG; L++) {
06280                 if (L != NUMP) {
06281                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06282                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06283                         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);
06284 
06285                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
06286                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06287                         ALPHA=rad2deg*float(asin(CCN));
06288                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06289                         if (ALPHA>90.0f)  ALPHA=180.0f-ALPHA;
06290                         if(ALPHA<1.0E-6) {
06291                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06292                         } else {
06293                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06294                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
06295                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06296                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06297                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06298                                 CP(1)   = 0.0;CP(2) = 0.0;
06299                                 VP(1)   = 0.0;VP(2) = 0.0;
06300 
06301                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06302                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06303                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06304                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06305 
06306                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06307 
06308                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
06309                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
06310                                 float tmpinv = 1.0f/TMP;
06311                                 for(int J=1;J<=NROW;J++) {
06312                                         JY = (J-1);
06313                                         if (JY>NR2)  JY=JY-NROW;
06314                                         for(int I=1;I<=NNNN/2;I++) {
06315                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06316                                                 RT     = 1.0f-FV/FM;
06317                                                 W(I,J) += ((RT>0.0f)*RT);
06318                                         }
06319                                 }
06320                         }
06321                 }
06322         }
06323 
06324     EMData* proj_in = PROJ;
06325     const bool realOnInput = PROJ->is_real();
06326 
06327         if (realOnInput) {
06328                 // copy input image and run DFT on it
06329                 PROJ = PROJ->norm_pad( false, 1 );
06330                 PROJ->do_fft_inplace();
06331                 PROJ->update();
06332         }
06333         float *PROJptr = PROJ->get_data();
06334 
06335         int KX;
06336         float WW;
06337         for(int J=1; J<=NROW; J++)
06338                 for(int I=1; I<=NNNN; I+=2) {
06339                         KX          =  (I+1)/2;
06340                         WW          =  1.0f/W(KX,J);
06341                         PROJ(I,J)   = PROJ(I,J)*WW;
06342                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
06343                 }
06344         delete W; W = 0;
06345         PROJ->do_ift_inplace();
06346         PROJ->depad();
06347 
06348         if (realOnInput) {
06349                 // copy data back to input image
06350                 float* data_src = PROJ->get_data();
06351                 float* data_dst = proj_in->get_data();
06352                 int ntotal = NSAM*NROW;
06353                 memcpy( data_dst, data_src, ntotal * sizeof(float) );
06354                 delete PROJ;
06355         }
06356 
06357         proj_in->update();
06358 }
06359 #undef   AMAX1
06360 #undef   AMIN1
06361 #undef   RI
06362 #undef   CC
06363 #undef   CP
06364 #undef   VV
06365 #undef   VP
06366 #undef   W
06367 #undef   SS
06368 #undef   PROJ
06369 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
06370 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06371 {
06372         float cst  = cs*1.0e7f;
06373 
06374         wgh /= 100.0;
06375         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06376         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06377         float ak2 = ak*ak;
06378         float g1 = dzz*1.0e4f*lambda*ak2;
06379         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06380 
06381         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06382         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06383         return ctfv;
06384 }
06385 
06386 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06387 {
06388         /***********
06389         ***get the size of the image for validation purpose
06390         **************/
06391         int nx = image->get_xsize();
06392         int ny = image->get_ysize();
06393         int nz = image->get_zsize();
06394         /********
06395         ***Exception Handle
06396         *************/
06397         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06398                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06399 
06400         size_t i, size = (size_t)nx*ny*nz;
06401 
06402         float* img_ptr = image->get_data();
06403         float* mask_ptr = mask->get_data();
06404 
06405         int ln=0;  //length of the output image = number of points under the mask.
06406         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06407 
06408         EMData* new_image = new EMData();
06409         new_image->set_size(ln,1,1); /* set size of the new image */
06410         float *new_ptr    = new_image->get_data();
06411 
06412         ln=-1;
06413         for(i = 0;i < size;i++){
06414                 if(mask_ptr[i] > 0.5f) {
06415                         ln++;
06416                         new_ptr[ln]=img_ptr[i];
06417                 }
06418         }
06419 
06420         return new_image;
06421 }
06422 
06423 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06424 {
06425         /********
06426         ***Exception Handle
06427         *************/
06428         if(mask == NULL)
06429                 throw ImageDimensionException("The mask cannot be an null image");
06430 
06431         /***********
06432         ***get the size of the mask
06433         **************/
06434         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06435 
06436         size_t i,size = (size_t)nx*ny*nz;                        /* loop counters */
06437         /* new image declaration */
06438         EMData *new_image = new EMData();
06439         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06440         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06441         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06442         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06443         int count = 0;
06444         float sum_under_mask = 0.0 ;
06445         for(i = 0;i < size;i++){
06446                         if(mask_ptr[i] > 0.5f){
06447                                 new_ptr[i] = img_ptr[count];
06448                                 sum_under_mask += img_ptr[count];
06449                                 count++;
06450                                 if( count > image->get_xsize() ) {
06451                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06452                                 }
06453                         }
06454         }
06455 
06456         if( count > image->get_xsize() ) {
06457             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06458         }
06459 
06460         float avg_under_mask = sum_under_mask / count;
06461         for(i = 0;i < size;i++) {
06462                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06463         }
06464         new_image->update();
06465         return new_image;
06466 }
06467 
06468 
06469 
06470 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06471 {
06472         vector<float>new_peak;
06473         int n1=peak1.size()/3;
06474         float p_size2=p_size*p_size;
06475         for (int i=0;i<n1;++i) {
06476                 vector<float>::iterator it2= peak1.begin()+3*i;
06477                 bool push_back1=true;
06478                 int n2=peak2.size()/3;
06479                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06480                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06481                 if(n2 ==0) {
06482                         new_peak.push_back(*it2);
06483                         new_peak.push_back(*(it2+1));
06484                         new_peak.push_back(*(it2+2));
06485                 } else  {
06486                         int j=0;
06487                         while (j< n2-1 ) {
06488                                 vector<float>::iterator it3= peak2.begin()+3*j;
06489                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06490                                 if(d2< p_size2 ) {
06491                                         if( (*it2)<(*it3) ) {
06492                                                 new_peak.push_back(*it3);
06493                                                 new_peak.push_back(*(it3+1));
06494                                                 new_peak.push_back(*(it3+2));
06495                                                 peak2.erase(it3);
06496                                                 peak2.erase(it3);
06497                                                 peak2.erase(it3);
06498                                                 push_back1=false;
06499                                         } else {
06500                                                 peak2.erase(it3);
06501                                                 peak2.erase(it3);
06502                                                 peak2.erase(it3);
06503                                         }
06504                                 } else  j=j+1;
06505                                 n2=peak2.size()/3;
06506                         }
06507                         if(push_back1) {
06508                                 new_peak.push_back(*it2);
06509                                 new_peak.push_back(*(it2+1));
06510                                 new_peak.push_back(*(it2+2));
06511                         }
06512                 }
06513         }
06514         return new_peak;
06515 }
06516 
06517 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06518 {
06519         // n size of the covariance/correlation matrix
06520         // covmat --- covariance/correlation matrix (n by n)
06521         // eigval --- returns eigenvalues
06522         // eigvec --- returns eigenvectors
06523 
06524         ENTERFUNC;
06525 
06526         int i;
06527 
06528         // make a copy of covmat so that it will not be overwritten
06529         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06530 
06531         char NEEDV = 'V';
06532         char UPLO = 'U';
06533         int lwork = -1;
06534         int info = 0;
06535         float *work, wsize;
06536 
06537         //  query to get optimal workspace
06538         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06539         lwork = (int)wsize;
06540 
06541         work = (float *)calloc(lwork, sizeof(float));
06542         //  calculate eigs
06543         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06544         free(work);
06545         EXITFUNC;
06546         return info;
06547 }
06548 
06549 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06550 {
06551 
06552         ENTERFUNC;
06553         int len = covmatpy.size();
06554         float *eigvec;
06555         float *eigval;
06556         float *covmat;
06557         int status = 0;
06558         eigval = (float*)calloc(ncov,sizeof(float));
06559         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06560         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06561 
06562         const float *covmat_ptr;
06563         covmat_ptr = &covmatpy[0];
06564         for(int i=0;i<len;i++){
06565             covmat[i] = covmat_ptr[i];
06566         }
06567 
06568         status = Util::coveig(ncov, covmat, eigval, eigvec);
06569 
06570         vector<float> eigval_py(ncov);
06571         const float *eigval_ptr;
06572         eigval_ptr = &eigval[0];
06573         for(int i=0;i<ncov;i++){
06574             eigval_py[i] = eigval_ptr[i];
06575         }
06576 
06577         vector<float> eigvec_py(ncov*ncov);
06578         const float *eigvec_ptr;
06579         eigvec_ptr = &eigvec[0];
06580         for(int i=0;i<ncov*ncov;i++){
06581             eigvec_py[i] = eigvec_ptr[i];
06582         }
06583 
06584         Dict res;
06585         res["eigval"] = eigval_py;
06586         res["eigvec"] = eigvec_py;
06587 
06588         EXITFUNC;
06589         return res;
06590 }
06591 
06592 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06593 {
06594         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06595 
06596         k=(int)pw.size();
06597         l=0;
06598         m=k;
06599         n2=n+2;
06600         n1=n+1;
06601         klmd=k+l+m;
06602         klm2d= k+l+m+2;
06603         nklmd=k+l+m+n;
06604         n2d=n+2;
06605         /*size has to be increased when N is large*/
06606         n_larg=klmd*2;
06607         klm2d=n_larg+klm2d;
06608         klmd=n_larg+klmd;
06609         nklmd=n_larg+nklmd;
06610         int size_q=klm2d*n2d;
06611         int size_cu=nklmd*2;
06612         static int i__;
06613 
06614         double *q ;
06615         double *x ;
06616         double *res;
06617         double *cu;
06618         float  *q2;
06619         float  *pw_;
06620         long int *iu;
06621         double *s;
06622         q   = (double*)calloc(size_q,sizeof(double));
06623         x   = (double*)calloc(n2d,sizeof(double));
06624         res = (double*)calloc(klmd,sizeof(double));
06625         cu  = (double*)calloc(size_cu,sizeof(double));
06626         s   = (double*)calloc(klmd,sizeof(double));
06627         q2  = (float*)calloc(size_q,sizeof(float));
06628         iu  = (long int*)calloc(size_cu,sizeof(long int));
06629         pw_ = (float*)calloc(k,sizeof(float));
06630 
06631         for( i__ =0; i__<k; ++i__) pw_[i__] = log(pw[i__]);
06632         long int l_k=k;
06633         long int l_n=n;
06634         long int l_iswi=iswi;
06635         vector<float> cl1_res;
06636         cl1_res = Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06637         free(q);
06638         free(x);
06639         free(res);
06640         free(s);
06641         free(cu);
06642         free(q2);
06643         free(iu);
06644         free(pw_);
06645         return cl1_res;
06646 }
06647 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)
06648 {
06649     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06650     float r__1;
06651     int tmp__i;
06652     long int i__, j;
06653     --s;
06654     --res;
06655     iu -= 3;
06656     cu -= 3;
06657     --x;
06658     long int klm2d;
06659     klm2d= *k+*k+2;
06660     klm2d=klm2d+klm2d;
06661     q_dim1 = klm2d;
06662     q_offset = 1 + q_dim1;
06663     q -= q_offset;
06664     q2_dim1 = klm2d;
06665     q2_offset = 1 + q2_dim1;
06666     q2 -= q2_offset;
06667     i__2=0;
06668     i__1 = *n - 1;
06669     tmp__i=0;
06670     for (j = 1; j <= i__1; ++j) {
06671         i__2 = *k;
06672         tmp__i+=1;
06673         for (i__ = 1; i__ <= i__2; ++i__) {
06674             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06675             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06676             }
06677     }
06678     for  (i__ = 1; i__ <= i__2; ++i__)
06679       { q2[i__ + *n * q2_dim1] = 1.f;
06680             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06681         }
06682    vector<float> fit_res;
06683    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06684    return fit_res;
06685 }
06686 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)
06687 {
06688     /* System generated locals */
06689     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06690 
06691     /* Local variables */
06692     long int i__, j, m, n1, ii, jj;
06693     double tmp;
06694     vector<float> p;
06695     --x;
06696     q_dim1 = *klm2d;
06697     q_offset = 1 + q_dim1;
06698     q -= q_offset;
06699     q1_dim1 = *klm2d;
06700     q1_offset = 1 + q1_dim1;
06701     q1 -= q1_offset;
06702     --s;
06703     --res;
06704     iu -= 3;
06705     cu -= 3;
06706 
06707     /* Function Body */
06708     long int l = 0;
06709 
06710 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06711     m = *ks;
06712     n1 = *n + 1;
06713     if (*iswi == 1) {
06714         i__1 = n1;
06715         for (jj = 1; jj <= i__1; ++jj) {
06716             i__2 = *ks;
06717             for (ii = 1; ii <= i__2; ++ii) {
06718         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06719 
06720                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06721                         ;
06722             }
06723         }
06724     } else if (*iswi == 2) {
06725         i__1 = *ks;
06726         for (ii = 1; ii <= i__1; ++ii) {
06727             i__2 = n1;
06728             for (jj = 1; jj <= i__2; ++jj) {
06729                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06730                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06731             }
06732         }
06733     } else if (*iswi == 3) {
06734         l = 2;
06735         i__1 = n1;
06736         for (jj = 1; jj <= i__1; ++jj) {
06737             i__2 = *ks + 2;
06738             for (ii = 1; ii <= i__2; ++ii) {
06739                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06740             }
06741             i__2 = *ks;
06742             for (ii = 1; ii <= i__2; ++ii) {
06743                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06744             }
06745         }
06746     } else if (*iswi == 4) {
06747         l = 2;
06748         i__1 = n1;
06749         for (jj = 1; jj <= i__1; ++jj) {
06750             i__2 = *ks + 2;
06751             for (ii = 1; ii <= i__2; ++ii) {
06752                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06753             }
06754             i__2 = *ks;
06755             for (ii = 1; ii <= i__2; ++ii) {
06756                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06757             }
06758         }
06759     } else if (*iswi == 5) {
06760         l = 1;
06761         i__1 = n1;
06762         for (jj = 1; jj <= i__1; ++jj) {
06763             i__2 = *ks + 1;
06764             for (ii = 1; ii <= i__2; ++ii) {
06765                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06766             }
06767             i__2 = *ks;
06768             for (ii = 1; ii <= i__2; ++ii) {
06769                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06770             }
06771         }
06772     } else if (*iswi == 6) {
06773         l = 1;
06774         i__1 = n1;
06775         for (jj = 1; jj <= i__1; ++jj) {
06776             i__2 = *ks + 1;
06777             for (ii = 1; ii <= i__2; ++ii) {
06778                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06779             }
06780             i__2 = *ks;
06781             for (ii = 1; ii <= i__2; ++ii) {
06782                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06783             }
06784         }
06785     } else if (*iswi == 7) {
06786         l = 3;
06787         i__1 = n1;
06788         for (jj = 1; jj <= i__1; ++jj) {
06789             i__2 = *ks + 3;
06790             for (ii = 1; ii <= i__2; ++ii) {
06791                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06792             }
06793             i__2 = *ks;
06794             for (ii = 1; ii <= i__2; ++ii) {
06795                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06796             }
06797         }
06798     } else if (*iswi == 8) {
06799         l = 4;
06800         i__1 = n1;
06801         for (jj = 1; jj <= i__1; ++jj) {
06802             i__2 = *ks + 4;
06803             for (ii = 1; ii <= i__2; ++ii) {
06804                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06805             }
06806             i__2 = *ks;
06807             for (ii = 1; ii <= i__2; ++ii) {
06808                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06809             }
06810         }
06811     }
06812 
06813     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06814     i__1 = *ks;
06815     int tmp__j=0;
06816     for (i__ = 1; i__ <= i__1; ++i__) {
06817         tmp = 0.f;
06818         i__2 = *n - 1;
06819         for (j = 1; j <= i__2; ++j) {
06820             tmp__j=j;
06821             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06822             }
06823         tmp += x[*n];
06824         p.push_back(static_cast<float>(exp(tmp)));
06825         p.push_back(q1[i__ + q1_dim1]);
06826     }
06827     i__2=*n;
06828     for (i__=1;i__<=i__2;++i__)
06829         { p.push_back(static_cast<float>(x[i__]));}
06830     return p;
06831 }
06832 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06833         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06834 {
06835 
06836     long int q_dim1, q_offset, i__1, i__2;
06837     double d__1;
06838 
06839     static long int i__, j;
06840     static double z__;
06841     static long int n1, n2, ia, ii, kk, in, nk, js;
06842     static double sn, zu, zv;
06843     static long int nk1, klm, nkl, jmn, jpn;
06844     static double cuv;
06845     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06846     static float xmin;
06847     static double xmax;
06848     static long int iout;
06849     static double xsum;
06850     static long int iineg, maxit;
06851     static double toler;
06852     static float error;
06853     static double pivot;
06854     static long int kforce, iphase;
06855     static double tpivot;
06856 
06857     --s;
06858     --res;
06859     iu -= 3;
06860     cu -= 3;
06861     --x;
06862     q_dim1 = *klm2d;
06863     q_offset = 1 + q_dim1;
06864     q -= q_offset;
06865 
06866     /* Function Body */
06867     maxit = 500;
06868     kode = 0;
06869     toler = 1e-4f;
06870     iter = 0;
06871     n1 = *n + 1;
06872     n2 = *n + 2;
06873     nk = *n + *k;
06874     nk1 = nk + 1;
06875     nkl = nk + *l;
06876     nkl1 = nkl + 1;
06877     klm = *k + *l + *m;
06878     klm1 = klm + 1;
06879     klm2 = klm + 2;
06880     nklm = *n + klm;
06881     kforce = 1;
06882     iter = 0;
06883     js = 1;
06884     ia = 0;
06885 /* SET UP LABELS IN Q. */
06886     i__1 = *n;
06887     for (j = 1; j <= i__1; ++j) {
06888         q[klm2 + j * q_dim1] = (double) j;
06889 /* L10: */
06890     }
06891     i__1 = klm;
06892     for (i__ = 1; i__ <= i__1; ++i__) {
06893         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06894         if (q[i__ + n1 * q_dim1] >= 0.f) {
06895             goto L30;
06896         }
06897         i__2 = n2;
06898         for (j = 1; j <= i__2; ++j) {
06899             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06900 /* L20: */
06901         }
06902 L30:
06903         ;
06904     }
06905 /* SET UP PHASE 1 COSTS. */
06906     iphase = 2;
06907     i__1 = nklm;
06908     for (j = 1; j <= i__1; ++j) {
06909         cu[(j << 1) + 1] = 0.f;
06910         cu[(j << 1) + 2] = 0.f;
06911         iu[(j << 1) + 1] = 0;
06912         iu[(j << 1) + 2] = 0;
06913 /* L40: */
06914     }
06915     if (*l == 0) {
06916         goto L60;
06917     }
06918     i__1 = nkl;
06919     for (j = nk1; j <= i__1; ++j) {
06920         cu[(j << 1) + 1] = 1.f;
06921         cu[(j << 1) + 2] = 1.f;
06922         iu[(j << 1) + 1] = 1;
06923         iu[(j << 1) + 2] = 1;
06924 /* L50: */
06925     }
06926     iphase = 1;
06927 L60:
06928     if (*m == 0) {
06929         goto L80;
06930     }
06931     i__1 = nklm;
06932     for (j = nkl1; j <= i__1; ++j) {
06933         cu[(j << 1) + 2] = 1.f;
06934         iu[(j << 1) + 2] = 1;
06935         jmn = j - *n;
06936         if (q[jmn + n2 * q_dim1] < 0.f) {
06937             iphase = 1;
06938         }
06939 /* L70: */
06940     }
06941 L80:
06942     if (kode == 0) {
06943         goto L150;
06944     }
06945     i__1 = *n;
06946     for (j = 1; j <= i__1; ++j) {
06947         if ((d__1 = x[j]) < 0.) {
06948             goto L90;
06949         } else if (d__1 == 0) {
06950             goto L110;
06951         } else {
06952             goto L100;
06953         }
06954 L90:
06955         cu[(j << 1) + 1] = 1.f;
06956         iu[(j << 1) + 1] = 1;
06957         goto L110;
06958 L100:
06959         cu[(j << 1) + 2] = 1.f;
06960         iu[(j << 1) + 2] = 1;
06961 L110:
06962         ;
06963     }
06964     i__1 = *k;
06965     for (j = 1; j <= i__1; ++j) {
06966         jpn = j + *n;
06967         if ((d__1 = res[j]) < 0.) {
06968             goto L120;
06969         } else if (d__1 == 0) {
06970             goto L140;
06971         } else {
06972             goto L130;
06973         }
06974 L120:
06975         cu[(jpn << 1) + 1] = 1.f;
06976         iu[(jpn << 1) + 1] = 1;
06977         if (q[j + n2 * q_dim1] > 0.f) {
06978             iphase = 1;
06979         }
06980         goto L140;
06981 L130:
06982         cu[(jpn << 1) + 2] = 1.f;
06983         iu[(jpn << 1) + 2] = 1;
06984         if (q[j + n2 * q_dim1] < 0.f) {
06985             iphase = 1;
06986         }
06987 L140:
06988         ;
06989     }
06990 L150:
06991     if (iphase == 2) {
06992         goto L500;
06993     }
06994 /* COMPUTE THE MARGINAL COSTS. */
06995 L160:
06996     i__1 = n1;
06997     for (j = js; j <= i__1; ++j) {
06998         xsum = 0.;
06999         i__2 = klm;
07000         for (i__ = 1; i__ <= i__2; ++i__) {
07001             ii = (long int) q[i__ + n2 * q_dim1];
07002             if (ii < 0) {
07003                 goto L170;
07004             }
07005             z__ = cu[(ii << 1) + 1];
07006             goto L180;
07007 L170:
07008             iineg = -ii;
07009             z__ = cu[(iineg << 1) + 2];
07010 L180:
07011             xsum += q[i__ + j * q_dim1] * z__;
07012 /*  180       XSUM = XSUM + Q(I,J)*Z */
07013 /* L190: */
07014         }
07015         q[klm1 + j * q_dim1] = xsum;
07016 /* L200: */
07017     }
07018     i__1 = *n;
07019     for (j = js; j <= i__1; ++j) {
07020         ii = (long int) q[klm2 + j * q_dim1];
07021         if (ii < 0) {
07022             goto L210;
07023         }
07024         z__ = cu[(ii << 1) + 1];
07025         goto L220;
07026 L210:
07027         iineg = -ii;
07028         z__ = cu[(iineg << 1) + 2];
07029 L220:
07030         q[klm1 + j * q_dim1] -= z__;
07031 /* L230: */
07032     }
07033 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
07034 L240:
07035     xmax = 0.f;
07036     if (js > *n) {
07037         goto L490;
07038     }
07039     i__1 = *n;
07040     for (j = js; j <= i__1; ++j) {
07041         zu = q[klm1 + j * q_dim1];
07042         ii = (long int) q[klm2 + j * q_dim1];
07043         if (ii > 0) {
07044             goto L250;
07045         }
07046         ii = -ii;
07047         zv = zu;
07048         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
07049         goto L260;
07050 L250:
07051         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
07052 L260:
07053         if (kforce == 1 && ii > *n) {
07054             goto L280;
07055         }
07056         if (iu[(ii << 1) + 1] == 1) {
07057             goto L270;
07058         }
07059         if (zu <= xmax) {
07060             goto L270;
07061         }
07062         xmax = zu;
07063         in = j;
07064 L270:
07065         if (iu[(ii << 1) + 2] == 1) {
07066             goto L280;
07067         }
07068         if (zv <= xmax) {
07069             goto L280;
07070         }
07071         xmax = zv;
07072         in = j;
07073 L280:
07074         ;
07075     }
07076     if (xmax <= toler) {
07077         goto L490;
07078     }
07079     if (q[klm1 + in * q_dim1] == xmax) {
07080         goto L300;
07081     }
07082     i__1 = klm2;
07083     for (i__ = 1; i__ <= i__1; ++i__) {
07084         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
07085 /* L290: */
07086     }
07087     q[klm1 + in * q_dim1] = xmax;
07088 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
07089 L300:
07090     if (iphase == 1 || ia == 0) {
07091         goto L330;
07092     }
07093     xmax = 0.f;
07094     i__1 = ia;
07095     for (i__ = 1; i__ <= i__1; ++i__) {
07096         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
07097         if (z__ <= xmax) {
07098             goto L310;
07099         }
07100         xmax = z__;
07101         iout = i__;
07102 L310:
07103         ;
07104     }
07105     if (xmax <= toler) {
07106         goto L330;
07107     }
07108     i__1 = n2;
07109     for (j = 1; j <= i__1; ++j) {
07110         z__ = q[ia + j * q_dim1];
07111         q[ia + j * q_dim1] = q[iout + j * q_dim1];
07112         q[iout + j * q_dim1] = z__;
07113 /* L320: */
07114     }
07115     iout = ia;
07116     --ia;
07117     pivot = q[iout + in * q_dim1];
07118     goto L420;
07119 L330:
07120     kk = 0;
07121     i__1 = klm;
07122     for (i__ = 1; i__ <= i__1; ++i__) {
07123         z__ = q[i__ + in * q_dim1];
07124         if (z__ <= toler) {
07125             goto L340;
07126         }
07127         ++kk;
07128         res[kk] = q[i__ + n1 * q_dim1] / z__;
07129         s[kk] = (double) i__;
07130 L340:
07131         ;
07132     }
07133 L350:
07134     if (kk > 0) {
07135         goto L360;
07136     }
07137     kode = 2;
07138     goto L590;
07139 L360:
07140     xmin = static_cast<float>( res[1] );
07141     iout = (long int) s[1];
07142     j = 1;
07143     if (kk == 1) {
07144         goto L380;
07145     }
07146     i__1 = kk;
07147     for (i__ = 2; i__ <= i__1; ++i__) {
07148         if (res[i__] >= xmin) {
07149             goto L370;
07150         }
07151         j = i__;
07152         xmin = static_cast<float>( res[i__] );
07153         iout = (long int) s[i__];
07154 L370:
07155         ;
07156     }
07157     res[j] = res[kk];
07158     s[j] = s[kk];
07159 L380:
07160     --kk;
07161     pivot = q[iout + in * q_dim1];
07162     ii = (long int) q[iout + n2 * q_dim1];
07163     if (iphase == 1) {
07164         goto L400;
07165     }
07166     if (ii < 0) {
07167         goto L390;
07168     }
07169     if (iu[(ii << 1) + 2] == 1) {
07170         goto L420;
07171     }
07172     goto L400;
07173 L390:
07174     iineg = -ii;
07175     if (iu[(iineg << 1) + 1] == 1) {
07176         goto L420;
07177     }
07178 /* 400 II = IABS(II) */
07179 L400:
07180     ii = abs(ii);
07181     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
07182     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
07183         goto L420;
07184     }
07185 /* BYPASS INTERMEDIATE VERTICES. */
07186     i__1 = n1;
07187     for (j = js; j <= i__1; ++j) {
07188         z__ = q[iout + j * q_dim1];
07189         q[klm1 + j * q_dim1] -= z__ * cuv;
07190         q[iout + j * q_dim1] = -z__;
07191 /* L410: */
07192     }
07193     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
07194     goto L350;
07195 /* GAUSS-JORDAN ELIMINATION. */
07196 L420:
07197     if (iter < maxit) {
07198         goto L430;
07199     }
07200     kode = 3;
07201     goto L590;
07202 L430:
07203     ++iter;
07204     i__1 = n1;
07205     for (j = js; j <= i__1; ++j) {
07206         if (j != in) {
07207             q[iout + j * q_dim1] /= pivot;
07208         }
07209 /* L440: */
07210     }
07211 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
07212 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
07213 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
07214 /*     DO 460 J=JS,N1 */
07215 /*        IF(J .EQ. IN) GO TO 460 */
07216 /*        Z = -Q(IOUT,J) */
07217 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
07218 /* 460 CONTINUE */
07219     i__1 = n1;
07220     for (j = js; j <= i__1; ++j) {
07221         if (j == in) {
07222             goto L460;
07223         }
07224         z__ = -q[iout + j * q_dim1];
07225         i__2 = klm1;
07226         for (i__ = 1; i__ <= i__2; ++i__) {
07227             if (i__ != iout) {
07228                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
07229             }
07230 /* L450: */
07231         }
07232 L460:
07233         ;
07234     }
07235     tpivot = -pivot;
07236     i__1 = klm1;
07237     for (i__ = 1; i__ <= i__1; ++i__) {
07238         if (i__ != iout) {
07239             q[i__ + in * q_dim1] /= tpivot;
07240         }
07241 /* L470: */
07242     }
07243     q[iout + in * q_dim1] = 1.f / pivot;
07244     z__ = q[iout + n2 * q_dim1];
07245     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
07246     q[klm2 + in * q_dim1] = z__;
07247     ii = (long int) abs(z__);
07248     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
07249         goto L240;
07250     }
07251     i__1 = klm2;
07252     for (i__ = 1; i__ <= i__1; ++i__) {
07253         z__ = q[i__ + in * q_dim1];
07254         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
07255         q[i__ + js * q_dim1] = z__;
07256 /* L480: */
07257     }
07258     ++js;
07259     goto L240;
07260 /* TEST FOR OPTIMALITY. */
07261 L490:
07262     if (kforce == 0) {
07263         goto L580;
07264     }
07265     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07266         goto L500;
07267     }
07268     kforce = 0;
07269     goto L240;
07270 /* SET UP PHASE 2 COSTS. */
07271 L500:
07272     iphase = 2;
07273     i__1 = nklm;
07274     for (j = 1; j <= i__1; ++j) {
07275         cu[(j << 1) + 1] = 0.f;
07276         cu[(j << 1) + 2] = 0.f;
07277 /* L510: */
07278     }
07279     i__1 = nk;
07280     for (j = n1; j <= i__1; ++j) {
07281         cu[(j << 1) + 1] = 1.f;
07282         cu[(j << 1) + 2] = 1.f;
07283 /* L520: */
07284     }
07285     i__1 = klm;
07286     for (i__ = 1; i__ <= i__1; ++i__) {
07287         ii = (long int) q[i__ + n2 * q_dim1];
07288         if (ii > 0) {
07289             goto L530;
07290         }
07291         ii = -ii;
07292         if (iu[(ii << 1) + 2] == 0) {
07293             goto L560;
07294         }
07295         cu[(ii << 1) + 2] = 0.f;
07296         goto L540;
07297 L530:
07298         if (iu[(ii << 1) + 1] == 0) {
07299             goto L560;
07300         }
07301         cu[(ii << 1) + 1] = 0.f;
07302 L540:
07303         ++ia;
07304         i__2 = n2;
07305         for (j = 1; j <= i__2; ++j) {
07306             z__ = q[ia + j * q_dim1];
07307             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07308             q[i__ + j * q_dim1] = z__;
07309 /* L550: */
07310         }
07311 L560:
07312         ;
07313     }
07314     goto L160;
07315 L570:
07316     if (q[klm1 + n1 * q_dim1] <= toler) {
07317         goto L500;
07318     }
07319     kode = 1;
07320     goto L590;
07321 L580:
07322     if (iphase == 1) {
07323         goto L570;
07324     }
07325 /* PREPARE OUTPUT. */
07326     kode = 0;
07327 L590:
07328     xsum = 0.;
07329     i__1 = *n;
07330     for (j = 1; j <= i__1; ++j) {
07331         x[j] = 0.f;
07332 /* L600: */
07333     }
07334     i__1 = klm;
07335     for (i__ = 1; i__ <= i__1; ++i__) {
07336         res[i__] = 0.f;
07337 /* L610: */
07338     }
07339     i__1 = klm;
07340     for (i__ = 1; i__ <= i__1; ++i__) {
07341         ii = (long int) q[i__ + n2 * q_dim1];
07342         sn = 1.f;
07343         if (ii > 0) {
07344             goto L620;
07345         }
07346         ii = -ii;
07347         sn = -1.f;
07348 L620:
07349         if (ii > *n) {
07350             goto L630;
07351         }
07352         x[ii] = sn * q[i__ + n1 * q_dim1];
07353         goto L640;
07354 L630:
07355         iimn = ii - *n;
07356         res[iimn] = sn * q[i__ + n1 * q_dim1];
07357         if (ii >= n1 && ii <= nk) {
07358             xsum += q[i__ + n1 * q_dim1];
07359         }
07360 L640:
07361         ;
07362     }
07363     error = (float)xsum;
07364     return;
07365 }
07366 
07367 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07368 {
07369         int j,d;
07370         EMData * e = new EMData();
07371         float *eptr, *imgptr;
07372         imgptr = img->get_data();
07373         float SSE = 0.f;
07374         for (j = 0 ; j < N ; j++) {
07375                 e->read_image(images,S[j]);
07376                 eptr = e->get_data();
07377                 for (d = 0; d < size; d++) {
07378                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07379                 }
07380         delete e;
07381         return SSE;
07382 }
07383 
07384 
07385 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07386 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07387 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07388 
07389 
07390 #define         quadpi                  3.141592653589793238462643383279502884197
07391 #define         dgr_to_rad              quadpi/180
07392 #define         deg_to_rad              quadpi/180
07393 #define         rad_to_deg              180/quadpi
07394 #define         rad_to_dgr              180/quadpi
07395 #define         TRUE                    1
07396 #define         FALSE                   0
07397 
07398 
07399 #define theta(i)                theta   [i-1]
07400 #define phi(i)                  phi     [i-1]
07401 #define weight(i)               weight  [i-1]
07402 #define lband(i)                lband   [i-1]
07403 #define ts(i)                   ts      [i-1]
07404 #define thetast(i)              thetast [i-1]
07405 #define key(i)                  key     [i-1]
07406 
07407 
07408 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07409 {
07410 
07411         ENTERFUNC;
07412 
07413         if ( th.size() != ph.size() ) {
07414                 LOGERR("images not same size");
07415                 throw ImageFormatException( "images not same size");
07416         }
07417 
07418         // rand_seed
07419         srand(10);
07420 
07421         int i,*key;
07422         int len = th.size();
07423         double *theta,*phi,*weight;
07424         theta   =       (double*) calloc(len,sizeof(double));
07425         phi     =       (double*) calloc(len,sizeof(double));
07426         weight  =       (double*) calloc(len,sizeof(double));
07427         key     =       (int*) calloc(len,sizeof(int));
07428         const float *thptr, *phptr;
07429 
07430         thptr = &th[0];
07431         phptr = &ph[0];
07432         for(i=1;i<=len;i++){
07433                 key(i) = i;
07434                 weight(i) = 0.0;
07435         }
07436 
07437         for(i = 0;i<len;i++){
07438                 theta[i] = thptr[i];
07439                 phi[i]   = phptr[i];
07440         }
07441 
07442         //  sort by theta
07443         Util::hsortd(theta, phi, key, len, 1);
07444 
07445         //Util::voronoidiag(theta,phi, weight, len);
07446         Util::voronoi(phi, theta, weight, len);
07447 
07448         //sort by key
07449         Util::hsortd(weight, weight, key, len, 2);
07450 
07451         free(theta);
07452         free(phi);
07453         free(key);
07454         vector<double> wt;
07455         double count = 0;
07456         for(i=1; i<= len; i++)
07457         {
07458                 wt.push_back(weight(i));
07459                 count += weight(i);
07460         }
07461 
07462         //if( abs(count-6.28) > 0.1 )
07463         //{
07464         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07465         //}
07466 
07467         free(weight);
07468 
07469         EXITFUNC;
07470         return wt;
07471 
07472 }
07473 
07474 struct  tmpstruct{
07475         double theta1,phi1;
07476         int key1;
07477         };
07478 
07479 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07480 {
07481         ENTERFUNC;
07482         vector<tmpstruct> tmp(len);
07483         int i;
07484         for(i = 1;i<=len;i++)
07485         {
07486                 tmp[i-1].theta1 = theta(i);
07487                 tmp[i-1].phi1 = phi(i);
07488                 tmp[i-1].key1 = key(i);
07489         }
07490 
07491         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07492         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07493 
07494         for(i = 1;i<=len;i++)
07495         {
07496                 theta(i) = tmp[i-1].theta1;
07497                 phi(i)   = tmp[i-1].phi1;
07498                 key(i)   = tmp[i-1].key1;
07499         }
07500         EXITFUNC;
07501 }
07502 
07503 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07504 {
07505         return(tmp1.theta1 < tmp2.theta1);
07506 }
07507 
07508 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07509 {
07510         return(tmp1.key1 < tmp2.key1);
07511 }
07512 
07513 /******************  VORONOI DIAGRAM **********************************/
07514 /*
07515 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07516 {
07517         ENTERFUNC;
07518 
07519         int     *lband;
07520         double  aat=0.0f,*ts;
07521         double  aa,acum,area;
07522         int     last;
07523         int numth       =       1;
07524         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07525 
07526         int i,it,l,k;
07527         int nband,lb,low,medium,lhigh,lbw,lenw;
07528 
07529 
07530         lband   =       (int*)calloc(nbt,sizeof(int));
07531         ts      =       (double*)calloc(nbt,sizeof(double));
07532 
07533         if(lband == NULL || ts == NULL ){
07534                 fprintf(stderr,"memory allocation failure!\n");
07535                 exit(1);
07536         }
07537 
07538         nband=nbt;
07539 
07540         while(nband>0){
07541                 Util::angstep(ts,nband);
07542 
07543                 l=1;
07544                 for(i=1;i<=n;i++){
07545                         if(theta(i)>ts(l)){
07546                                 lband(l)=i;
07547                                 l=l+1;
07548                                 if(l>nband)  exit(1);
07549                         }
07550                 }
07551 
07552                 l=1;
07553                 for(i=1;i<=n;i++){
07554                         if(theta(i)>ts(l)){
07555                                 lband(l)=i;
07556                                 l=l+1;
07557                                 if(l>nband)  exit(1);
07558                         }
07559                 }
07560 
07561                 lband(l)=n+1;
07562                 acum=0.0;
07563                 for(it=l;it>=1;it-=numth){
07564                         for(i=it;i>=mymax(1,it-numth+1);i--){
07565                         if(i==l) last   =        TRUE;
07566                         else     last   =        FALSE;
07567 
07568                         if(l==1){
07569                                 lb=1;
07570                                 low=1;
07571                                 medium=n+1;
07572                                 lhigh=n-lb+1;
07573                                 lbw=1;
07574                         }
07575                         else if(i==1){
07576                                 lb=1;
07577                                 low=1;
07578                                 medium=lband(1);
07579                                 lhigh=lband(2)-1;
07580                                 lbw=1;
07581                         }
07582                         else if(i==l){
07583                                 if(l==2)        lb=1;
07584                                 else            lb=lband(l-2);
07585                                 low=lband(l-1)-lb+1;
07586                                 medium=lband(l)-lb+1;
07587                                 lhigh=n-lb+1;
07588                                 lbw=lband(i-1);
07589                         }
07590                         else{
07591                                 if(i==2)        lb=1;
07592                                 else            lb=lband(i-2);
07593                                 low=lband(i-1)-lb+1;
07594                                 medium=lband(i)-lb+1;
07595                                 lhigh=lband(i+1)-1-lb+1;
07596                                 lbw=lband(i-1);
07597                         }
07598                         lenw=medium-low;
07599 
07600 
07601                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07602 
07603 
07604                         if(nband>1){
07605                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07606                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07607 
07608                                 aa = 0.0;
07609                                 for(k = lbw;k<=lbw+lenw-1;k++)
07610                                         aa = aa+weight(k);
07611 
07612                                 acum=acum+aa;
07613                                 aat=aa/area;
07614                                 }
07615 
07616                         }
07617                         for(i=it;mymax(1,it-numth+1);i--){
07618                         if(fabs(aat-1.0)>0.02){
07619                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07620                                 goto  label2;
07621                                 }
07622                         }
07623                 acum=acum/quadpi/2.0;
07624                 exit(1);
07625 label2:
07626 
07627                 continue;
07628                 }
07629 
07630         free(ts);
07631         free(lband);
07632 
07633         }
07634 
07635         EXITFUNC;
07636 }
07637 
07638 
07639 void Util::angstep(double* thetast,int len){
07640 
07641         ENTERFUNC;
07642 
07643         double t1,t2,tmp;
07644         int i;
07645         if(len>1){
07646                 t1=0;
07647                 for(i=1;i<=len-1;i++){
07648                         tmp=cos(t1)-1.0/((float)len);
07649                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07650                         thetast(i)=t2 * rad_to_deg;
07651                         t1=t2;
07652                 }
07653         }
07654         thetast(len)=90.0;
07655 
07656         EXITFUNC;
07657 }
07658 */
07659 /*
07660 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07661 {
07662 
07663         ENTERFUNC;
07664         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07665         int nt6, n, ier,nout,lnew,mdup,nd;
07666         int i,k,mt,status;
07667 
07668 
07669         double *ds, *x, *y, *z;
07670         double tol=1.0e-8;
07671         double a;
07672 
07673         if(last){
07674                 if(medium>nt)  n = nt+nt;
07675                 else           n = nt+nt-medium+1;
07676         }
07677         else{
07678                 n=nt;
07679         }
07680 
07681         nt6 = n*6;
07682 
07683         list = (int*)calloc(nt6,sizeof(int));
07684         lptr = (int*)calloc(nt6,sizeof(int));
07685         lend = (int*)calloc(n  ,sizeof(int));
07686         iwk  = (int*)calloc(n  ,sizeof(int));
07687         good = (int*)calloc(n  ,sizeof(int));
07688         key  = (int*)calloc(n  ,sizeof(int));
07689         indx = (int*)calloc(n  ,sizeof(int));
07690         lcnt = (int*)calloc(n  ,sizeof(int));
07691 
07692         ds      =       (double*) calloc(n,sizeof(double));
07693         x       =       (double*) calloc(n,sizeof(double));
07694         y       =       (double*) calloc(n,sizeof(double));
07695         z       =       (double*) calloc(n,sizeof(double));
07696 
07697         if (list == NULL ||
07698         lptr == NULL ||
07699         lend == NULL ||
07700         iwk  == NULL ||
07701         good == NULL ||
07702         key  == NULL ||
07703         indx == NULL ||
07704         lcnt == NULL ||
07705         x    == NULL ||
07706         y    == NULL ||
07707         z    == NULL ||
07708         ds   == NULL) {
07709                 printf("memory allocation failure!\n");
07710                 exit(1);
07711         }
07712 
07713 
07714 
07715         for(i = 1;i<=nt;i++){
07716                 x[i-1] = theta(i);
07717                 y[i-1] = phi(i);
07718         }
07719 
07720 
07721 
07722         if (last) {
07723                 for(i=nt+1;i<=n;i++){
07724                         x[i-1]=180.0-x[2*nt-i];
07725                         y[i-1]=180.0+y[2*nt-i];
07726                 }
07727         }
07728 
07729 
07730         Util::disorder2(x,y,key,n);
07731 
07732         Util::ang_to_xyz(x,y,z,n);
07733 
07734 
07735         //  Make sure that first three are no colinear
07736         label1:
07737         for(k=0; k<2; k++){
07738                 for(i=k+1; i<3; i++){
07739                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07740                                 Util::flip23(x, y, z, key, k, n);
07741                                 goto label1;
07742                         }
07743                 }
07744         }
07745 
07746 
07747         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07748 
07749 
07750         if (status != 0) {
07751                 printf(" error in trmsh3 \n");
07752                 exit(1);
07753         }
07754 
07755 
07756         mdup=n-nout;
07757         if (ier == -2) {
07758                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07759                 exit(1);
07760         }
07761         else if (ier > 0) {
07762                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07763                 exit(1);
07764         }
07765 
07766         nd=0;
07767         for (k=1;k<=n;k++){
07768                 if (indx[k-1]>0){
07769                         nd++;
07770                         good[nd-1]=k;
07771                 }
07772         }
07773 
07774 
07775         for(i = 1;i<=nout;i++) {
07776                 k=good[i-1];
07777                 if (key[k-1] >= low && key[k-1]<medium){
07778                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07779                         if (ier != 0){
07780                                 weight[key[k-1]-low] =-1.0;
07781                         }
07782                         else {
07783                                 weight[key[k-1]-low]=a/lcnt[i-1];
07784                         }
07785                 }
07786         }
07787 
07788 // Fill out the duplicated weights
07789         for(i = 1;i<=n;i++){
07790                 mt=-indx[i-1];
07791                 if (mt>0){
07792                         k=good[mt-1];
07793 //  This is a duplicated entry, get the already calculated
07794 //   weight and assign it.
07795                         if (key[i-1]>=low && key[i-1]<medium){
07796 //  Is it already calculated weight??
07797                                 if(key[k-1]>=low && key[k-1]<medium){
07798                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07799                                 }
07800                                 else{
07801 //  No, the weight is from the outside of valid region, calculate it anyway
07802                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07803                                         if (ier != 0){
07804                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07805                                                 weight[key[i-1]-low] =-1.0;
07806                                         }
07807                                         else {
07808                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07809                                         }
07810                                 }
07811                         }
07812                 }
07813         }
07814 
07815 
07816         free(list);
07817         free(lend);
07818         free(iwk);
07819         free(good);
07820         free(key);
07821 
07822         free(indx);
07823         free(lcnt);
07824         free(ds);
07825         free(x);
07826         free(y);
07827         free(z);
07828         EXITFUNC;
07829 }
07830 */
07831 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07832 {
07833 
07834         ENTERFUNC;
07835 
07836         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07837         int nt6, n, ier, nout, lnew, mdup, nd;
07838         int i,k,mt,status;
07839 
07840 
07841         double *ds, *x, *y, *z;
07842         double tol  = 1.0e-8;
07843         double dtol = 15;
07844         double a;
07845 
07846         /*if(last){
07847                 if(medium>nt)  n = nt+nt;
07848                 else           n = nt+nt-medium+1;
07849         }
07850         else{
07851                 n=nt;
07852         }*/
07853 
07854         n = nt + nt;
07855 
07856         nt6 = n*6;
07857 
07858         list = (int*)calloc(nt6,sizeof(int));
07859         lptr = (int*)calloc(nt6,sizeof(int));
07860         lend = (int*)calloc(n  ,sizeof(int));
07861         iwk  = (int*)calloc(n  ,sizeof(int));
07862         good = (int*)calloc(n  ,sizeof(int));
07863         key  = (int*)calloc(n  ,sizeof(int));
07864         indx = (int*)calloc(n  ,sizeof(int));
07865         lcnt = (int*)calloc(n  ,sizeof(int));
07866 
07867         ds      =       (double*) calloc(n,sizeof(double));
07868         x       =       (double*) calloc(n,sizeof(double));
07869         y       =       (double*) calloc(n,sizeof(double));
07870         z       =       (double*) calloc(n,sizeof(double));
07871 
07872         if (list == NULL ||
07873         lptr == NULL ||
07874         lend == NULL ||
07875         iwk  == NULL ||
07876         good == NULL ||
07877         key  == NULL ||
07878         indx == NULL ||
07879         lcnt == NULL ||
07880         x    == NULL ||
07881         y    == NULL ||
07882         z    == NULL ||
07883         ds   == NULL) {
07884                 printf("memory allocation failure!\n");
07885                 exit(1);
07886         }
07887 
07888         bool colinear=true;
07889         while(colinear)
07890         {
07891 
07892         L1:
07893             for(i = 0; i<nt; i++){
07894                 x[i] = theta[i];
07895                 y[i] = phi[i];
07896                 x[nt+i] = 180.0 - x[i];
07897                 y[nt+i] = 180.0 + y[i];
07898             }
07899 
07900             Util::disorder2(x, y, key, n);
07901 
07902             // check if the first three angles are not close, else shuffle
07903             double val;
07904             for(k=0; k<2; k++){
07905                 for(i=k+1; i<3; i++){
07906                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07907                     if( val  < dtol) {
07908                         goto L1;
07909                     }
07910                 }
07911             }
07912 
07913             Util::ang_to_xyz(x, y, z, n);
07914 
07915             //  Make sure that first three has no duplication
07916             bool dupnode=true;
07917             dupnode=true;
07918             while(dupnode)
07919             {
07920                 for(k=0; k<2; k++){
07921                     for(i=k+1; i<3; i++){
07922                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07923                                 Util::flip23(x, y, z, key, k, n);
07924                                 continue;
07925                         }
07926                     }
07927                 }
07928                 dupnode = false;
07929             }
07930 
07931 
07932             ier = 0;
07933 
07934             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07935 
07936             if (status != 0) {
07937                 printf(" error in trmsh3 \n");
07938                 exit(1);
07939             }
07940 
07941             if (ier > 0) {
07942                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07943                 exit(1);
07944             }
07945 
07946             mdup=n-nout;
07947             if (ier == -2) {
07948                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07949             }
07950             else
07951             {
07952                 colinear=false;
07953             }
07954         }
07955 
07956 
07957         Assert( ier != -2 );
07958 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07959 //  INDX contains node numbers from the squeezed list
07960         nd=0;
07961         for (k=1; k<=n; k++){
07962                 if (indx[k-1]>0) {
07963                         nd++;
07964                         good[nd-1]=k;
07965                 }
07966         }
07967 
07968 //
07969 // *** Compute the Voronoi region areas.
07970 //
07971         for(i = 1; i<=nout; i++) {
07972                 k=good[i-1];
07973                 //  We only need n weights from hemisphere
07974                 if (key[k-1] <= nt) {
07975 //  CALCULATE THE AREA
07976                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07977                         if (ier != 0){
07978 //  We set the weight to -1, this will signal the error in the calling
07979 //   program, as the area will turn out incorrect
07980                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07981                                 weight[key[k-1]-1] =-1.0;
07982                         } else {
07983 //  Assign the weight
07984                                 weight[key[k-1]-1]=a/lcnt[i-1];
07985                         }
07986                 }
07987         }
07988 
07989 
07990 // Fill out the duplicated weights
07991         for(i = 1; i<=n; i++){
07992                 mt -= indx[i-1];
07993                 if (mt>0){
07994                         k = good[mt-1];
07995 //  This is a duplicated entry, get the already calculated
07996 //   weight and assign it.
07997                 //  We only need n weights from hemisphere
07998                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07999                         }
08000         }
08001 
08002         free(list);
08003         free(lend);
08004         free(iwk);
08005         free(good);
08006         free(key);
08007         free(lptr);
08008         free(indx);
08009         free(lcnt);
08010         free(ds);
08011         free(x);
08012         free(y);
08013         free(z);
08014 
08015 
08016         EXITFUNC;
08017 }
08018 
08019 void Util::disorder2(double *x,double *y, int *key, int len)
08020 {
08021         ENTERFUNC;
08022         int k, i;
08023         for(i=0; i<len; i++) key[i]=i+1;
08024 
08025         for(i = 0; i<len;i++){
08026                 k = rand()%len;
08027                 std::swap(key[k], key[i]);
08028                 std::swap(x[k], x[i]);
08029                 std::swap(y[k], y[i]);
08030         }
08031         EXITFUNC;
08032 }
08033 
08034 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
08035 {
08036         ENTERFUNC;
08037         double costheta,sintheta,cosphi,sinphi;
08038         for(int i = 0;  i<len;  i++) {
08039                 cosphi = cos(y[i]*dgr_to_rad);
08040                 sinphi = sin(y[i]*dgr_to_rad);
08041                 if(fabs(x[i]-90.0)< 1.0e-5){
08042                         x[i] = cosphi;
08043                         y[i] = sinphi;
08044                         z[i] = 0.0;
08045                 } else {
08046                         costheta = cos(x[i]*dgr_to_rad);
08047                         sintheta = sin(x[i]*dgr_to_rad);
08048                         x[i] = cosphi*sintheta;
08049                         y[i] = sinphi*sintheta;
08050                         z[i] = costheta;
08051                 }
08052         }
08053         EXITFUNC;
08054 }
08055 
08056 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
08057 {
08058         ENTERFUNC;
08059         int i = k;
08060         while( i == k )  i = rand()%len;
08061         std::swap(key[i], key[k]);
08062         std::swap(x[i], x[k]);
08063         std::swap(y[i], y[k]);
08064         std::swap(z[i], z[k]);
08065         EXITFUNC;
08066 }
08067 
08068 
08069 #undef  mymax
08070 #undef  mymin
08071 #undef  sign
08072 #undef  quadpi
08073 #undef  dgr_to_rad
08074 #undef  deg_to_rad
08075 #undef  rad_to_deg
08076 #undef  rad_to_dgr
08077 #undef  TRUE
08078 #undef  FALSE
08079 #undef  theta
08080 #undef  phi
08081 #undef  weight
08082 #undef  lband
08083 #undef  ts
08084 #undef  thetast
08085 #undef  key
08086 
08087 
08088 /*################################################################################################
08089 ##########  strid.f -- translated by f2c (version 20030320). ###################################
08090 ######   You must link the resulting object file with the libraries: #############################
08091 ####################    -lf2c -lm   (in that order)   ############################################
08092 ################################################################################################*/
08093 
08094 /* Common Block Declarations */
08095 
08096 
08097 #define TRUE_ (1)
08098 #define FALSE_ (0)
08099 #define abs(x) ((x) >= 0 ? (x) : -(x))
08100 
08101 struct stcom_{
08102     double y;
08103 };
08104 stcom_ stcom_1;
08105 #ifdef KR_headers
08106 double floor();
08107 int i_dnnt(x) double *x;
08108 #else
08109 int i_dnnt(double *x)
08110 #endif
08111 {
08112         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
08113 }
08114 
08115 
08116 
08117 
08118 /* ____________________STRID______________________________________ */
08119 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
08120         double *y, double *z__, int *n, int *list, int *
08121         lptr, int *lend, int *lnew, int *indx, int *lcnt,
08122         int *near__, int *next, double *dist, int *ier)
08123 {
08124     /* System generated locals */
08125     int i__1, i__2;
08126 
08127     /* Local variables */
08128     static double d__;
08129     static int i__, j;
08130     static double d1, d2, d3;
08131     static int i0, lp, kt, ku, lpl, nku;
08132     static int nexti;
08133 
08134 
08135 /* *********************************************************** */
08136 
08137 /*                                              From STRIPACK */
08138 /*                                            Robert J. Renka */
08139 /*                                  Dept. of Computer Science */
08140 /*                                       Univ. of North Texas */
08141 /*                                           renka@cs.unt.edu */
08142 /*                                                   01/20/03 */
08143 
08144 /*   This is an alternative to TRMESH with the inclusion of */
08145 /* an efficient means of removing duplicate or nearly dupli- */
08146 /* cate nodes. */
08147 
08148 /*   This subroutine creates a Delaunay triangulation of a */
08149 /* set of N arbitrarily distributed points, referred to as */
08150 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
08151 /* routine TRMESH for definitions and a list of additional */
08152 /* subroutines.  This routine is an alternative to TRMESH */
08153 /* with the inclusion of an efficient means of removing dup- */
08154 /* licate or nearly duplicate nodes. */
08155 
08156 /*   The algorithm has expected time complexity O(N*log(N)) */
08157 /* for random nodal distributions. */
08158 
08159 
08160 /* On input: */
08161 
08162 /*       N0 = Number of nodes, possibly including duplicates. */
08163 /*            N0 .GE. 3. */
08164 
08165 /*       TOL = Tolerance defining a pair of duplicate nodes: */
08166 /*             bound on the deviation from 1 of the cosine of */
08167 /*             the angle between the nodes.  Note that */
08168 /*             |1-cos(A)| is approximately A*A/2. */
08169 
08170 /* The above parameters are not altered by this routine. */
08171 
08172 /*       X,Y,Z = Arrays of length at least N0 containing the */
08173 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
08174 /*               Z(K)) is referred to as node K, and K is re- */
08175 /*               ferred to as a nodal index.  It is required */
08176 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
08177 /*               K.  The first three nodes must not be col- */
08178 /*               linear (lie on a common great circle). */
08179 
08180 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
08181 
08182 /*       LEND = Array of length at least N0. */
08183 
08184 /*       INDX = Array of length at least N0. */
08185 
08186 /*       LCNT = Array of length at least N0 (length N is */
08187 /*              sufficient). */
08188 
08189 /*       NEAR,NEXT,DIST = Work space arrays of length at */
08190 /*                        least N0.  The space is used to */
08191 /*                        efficiently determine the nearest */
08192 /*                        triangulation node to each un- */
08193 /*                        processed node for use by ADDNOD. */
08194 
08195 /* On output: */
08196 
08197 /*       N = Number of nodes in the triangulation.  3 .LE. N */
08198 /*           .LE. N0, or N = 0 if IER < 0. */
08199 
08200 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
08201 /*               of the triangulation nodes in the first N */
08202 /*               locations.  The original array elements are */
08203 /*               shifted down as necessary to eliminate dup- */
08204 /*               licate nodes. */
08205 
08206 /*       LIST = Set of nodal indexes which, along with LPTR, */
08207 /*              LEND, and LNEW, define the triangulation as a */
08208 /*              set of N adjacency lists -- counterclockwise- */
08209 /*              ordered sequences of neighboring nodes such */
08210 /*              that the first and last neighbors of a bound- */
08211 /*              ary node are boundary nodes (the first neigh- */
08212 /*              bor of an interior node is arbitrary).  In */
08213 /*              order to distinguish between interior and */
08214 /*              boundary nodes, the last neighbor of each */
08215 /*              boundary node is represented by the negative */
08216 /*              of its index. */
08217 
08218 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
08219 /*              correspondence with the elements of LIST. */
08220 /*              LIST(LPTR(I)) indexes the node which follows */
08221 /*              LIST(I) in cyclical counterclockwise order */
08222 /*              (the first neighbor follows the last neigh- */
08223 /*              bor). */
08224 
08225 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
08226 /*              points to the last neighbor of node K for */
08227 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
08228 /*              only if K is a boundary node. */
08229 
08230 /*       LNEW = Pointer to the first empty location in LIST */
08231 /*              and LPTR (list length plus one).  LIST, LPTR, */
08232 /*              LEND, and LNEW are not altered if IER < 0, */
08233 /*              and are incomplete if IER > 0. */
08234 
08235 /*       INDX = Array of output (triangulation) nodal indexes */
08236 /*              associated with input nodes.  For I = 1 to */
08237 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
08238 /*              the triangulation node with the same (or */
08239 /*              nearly the same) coordinates as input node I. */
08240 
08241 /*       LCNT = Array of int weights (counts) associated */
08242 /*              with the triangulation nodes.  For I = 1 to */
08243 /*              N, LCNT(I) is the number of occurrences of */
08244 /*              node I in the input node set, and thus the */
08245 /*              number of duplicates is LCNT(I)-1. */
08246 
08247 /*       NEAR,NEXT,DIST = Garbage. */
08248 
08249 /*       IER = Error indicator: */
08250 /*             IER =  0 if no errors were encountered. */
08251 /*             IER = -1 if N0 < 3 on input. */
08252 /*             IER = -2 if the first three nodes are */
08253 /*                      collinear. */
08254 /*             IER = -3 if Subroutine ADDNOD returns an error */
08255 /*                      flag.  This should not occur. */
08256 
08257 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
08258 /*                                INSERT, INTADD, JRAND, */
08259 /*                                LEFT, LSTPTR, STORE, SWAP, */
08260 /*                                SWPTST, TRFIND */
08261 
08262 /* Intrinsic function called by TRMSH3:  ABS */
08263 
08264 /* *********************************************************** */
08265 
08266 
08267 /* Local parameters: */
08268 
08269 /* D =        (Negative cosine of) distance from node KT to */
08270 /*              node I */
08271 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
08272 /*              respectively */
08273 /* I,J =      Nodal indexes */
08274 /* I0 =       Index of the node preceding I in a sequence of */
08275 /*              unprocessed nodes:  I = NEXT(I0) */
08276 /* KT =       Index of a triangulation node */
08277 /* KU =       Index of an unprocessed node and DO-loop index */
08278 /* LP =       LIST index (pointer) of a neighbor of KT */
08279 /* LPL =      Pointer to the last neighbor of KT */
08280 /* NEXTI =    NEXT(I) */
08281 /* NKU =      NEAR(KU) */
08282 
08283     /* Parameter adjustments */
08284     --dist;
08285     --next;
08286     --near__;
08287     --indx;
08288     --lend;
08289     --z__;
08290     --y;
08291     --x;
08292     --list;
08293     --lptr;
08294     --lcnt;
08295 
08296     /* Function Body */
08297     if (*n0 < 3) {
08298         *n = 0;
08299         *ier = -1;
08300         return 0;
08301     }
08302 
08303 /* Store the first triangle in the linked list. */
08304 
08305     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08306             z__[3])) {
08307 
08308 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
08309 
08310         list[1] = 3;
08311         lptr[1] = 2;
08312         list[2] = -2;
08313         lptr[2] = 1;
08314         lend[1] = 2;
08315 
08316         list[3] = 1;
08317         lptr[3] = 4;
08318         list[4] = -3;
08319         lptr[4] = 3;
08320         lend[2] = 4;
08321 
08322         list[5] = 2;
08323         lptr[5] = 6;
08324         list[6] = -1;
08325         lptr[6] = 5;
08326         lend[3] = 6;
08327 
08328     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08329             y[3], &z__[3])) {
08330 
08331 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
08332 /*     i.e., node 3 lies in the left hemisphere defined by */
08333 /*     arc 1->2. */
08334 
08335         list[1] = 2;
08336         lptr[1] = 2;
08337         list[2] = -3;
08338         lptr[2] = 1;
08339         lend[1] = 2;
08340 
08341         list[3] = 3;
08342         lptr[3] = 4;
08343         list[4] = -1;
08344         lptr[4] = 3;
08345         lend[2] = 4;
08346 
08347         list[5] = 1;
08348         lptr[5] = 6;
08349         list[6] = -2;
08350         lptr[6] = 5;
08351         lend[3] = 6;
08352 
08353 
08354     } else {
08355 
08356 /*   The first three nodes are collinear. */
08357 
08358         *n = 0;
08359         *ier = -2;
08360         return 0;
08361     }
08362 
08363     //printf("pass check colinear\n");
08364 
08365 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08366 
08367     *lnew = 7;
08368     indx[1] = 1;
08369     indx[2] = 2;
08370     indx[3] = 3;
08371     lcnt[1] = 1;
08372     lcnt[2] = 1;
08373     lcnt[3] = 1;
08374     if (*n0 == 3) {
08375         *n = 3;
08376         *ier = 0;
08377         return 0;
08378     }
08379 
08380 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08381 /*   used to obtain an expected-time (N*log(N)) incremental */
08382 /*   algorithm by enabling constant search time for locating */
08383 /*   each new node in the triangulation. */
08384 
08385 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08386 /*   triangulation node closest to KU (used as the starting */
08387 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08388 /*   is an increasing function of the arc length (angular */
08389 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08390 /*   arc length a. */
08391 
08392 /* Since it is necessary to efficiently find the subset of */
08393 /*   unprocessed nodes associated with each triangulation */
08394 /*   node J (those that have J as their NEAR entries), the */
08395 /*   subsets are stored in NEAR and NEXT as follows:  for */
08396 /*   each node J in the triangulation, I = NEAR(J) is the */
08397 /*   first unprocessed node in J's set (with I = 0 if the */
08398 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08399 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08400 /*   set are initially ordered by increasing indexes (which */
08401 /*   maximizes efficiency) but that ordering is not main- */
08402 /*   tained as the data structure is updated. */
08403 
08404 /* Initialize the data structure for the single triangle. */
08405 
08406     near__[1] = 0;
08407     near__[2] = 0;
08408     near__[3] = 0;
08409     for (ku = *n0; ku >= 4; --ku) {
08410         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08411         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08412         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08413         if (d1 <= d2 && d1 <= d3) {
08414             near__[ku] = 1;
08415             dist[ku] = d1;
08416             next[ku] = near__[1];
08417             near__[1] = ku;
08418         } else if (d2 <= d1 && d2 <= d3) {
08419             near__[ku] = 2;
08420             dist[ku] = d2;
08421             next[ku] = near__[2];
08422             near__[2] = ku;
08423         } else {
08424             near__[ku] = 3;
08425             dist[ku] = d3;
08426             next[ku] = near__[3];
08427             near__[3] = ku;
08428         }
08429 /* L1: */
08430     }
08431 
08432 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08433 /*   in the triangulation, and NKU = NEAR(KU). */
08434 
08435     kt = 3;
08436     i__1 = *n0;
08437     for (ku = 4; ku <= i__1; ++ku) {
08438         nku = near__[ku];
08439 
08440 /* Remove KU from the set of unprocessed nodes associated */
08441 /*   with NEAR(KU). */
08442         i__ = nku;
08443         if (near__[i__] == ku) {
08444             near__[i__] = next[ku];
08445         } else {
08446             i__ = near__[i__];
08447 L2:
08448             i0 = i__;
08449             i__ = next[i0];
08450             if (i__ != ku) {
08451                 goto L2;
08452             }
08453             next[i0] = next[ku];
08454         }
08455         near__[ku] = 0;
08456 
08457 /* Bypass duplicate nodes. */
08458 
08459         if (dist[ku] <= *tol - 1.) {
08460             indx[ku] = -nku;
08461             ++lcnt[nku];
08462             goto L6;
08463         }
08464 
08465 
08466 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08467         ++kt;
08468         x[kt] = x[ku];
08469         y[kt] = y[ku];
08470         z__[kt] = z__[ku];
08471         indx[ku] = kt;
08472         lcnt[kt] = 1;
08473         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08474                 , lnew, ier);
08475         if (*ier != 0) {
08476             *n = 0;
08477             *ier = -3;
08478             return 0;
08479         }
08480 
08481 /* Loop on neighbors J of node KT. */
08482 
08483         lpl = lend[kt];
08484         lp = lpl;
08485 L3:
08486         lp = lptr[lp];
08487         j = (i__2 = list[lp], abs(i__2));
08488 
08489 /* Loop on elements I in the sequence of unprocessed nodes */
08490 /*   associated with J:  KT is a candidate for replacing J */
08491 /*   as the nearest triangulation node to I.  The next value */
08492 /*   of I in the sequence, NEXT(I), must be saved before I */
08493 /*   is moved because it is altered by adding I to KT's set. */
08494 
08495         i__ = near__[j];
08496 L4:
08497         if (i__ == 0) {
08498             goto L5;
08499         }
08500         nexti = next[i__];
08501 
08502 /* Test for the distance from I to KT less than the distance */
08503 /*   from I to J. */
08504 
08505         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08506         if (d__ < dist[i__]) {
08507 
08508 /* Replace J by KT as the nearest triangulation node to I: */
08509 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08510 /*   of unprocessed nodes and add it to KT's set. */
08511 
08512             near__[i__] = kt;
08513             dist[i__] = d__;
08514             if (i__ == near__[j]) {
08515                 near__[j] = nexti;
08516             } else {
08517                 next[i0] = nexti;
08518             }
08519             next[i__] = near__[kt];
08520             near__[kt] = i__;
08521         } else {
08522             i0 = i__;
08523         }
08524 
08525 /* Bottom of loop on I. */
08526 
08527         i__ = nexti;
08528         goto L4;
08529 
08530 /* Bottom of loop on neighbors J. */
08531 
08532 L5:
08533         if (lp != lpl) {
08534             goto L3;
08535         }
08536 L6:
08537         ;
08538     }
08539     *n = kt;
08540     *ier = 0;
08541     return 0;
08542 } /* trmsh3_ */
08543 
08544 /* stripack.dbl sent by Robert on 06/03/03 */
08545 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08546         double *y, double *z__, int *list, int *lptr, int
08547         *lend, int *lnew, int *ier)
08548 {
08549     /* Initialized data */
08550 
08551     static double tol = 0.;
08552 
08553     /* System generated locals */
08554     int i__1;
08555 
08556     /* Local variables */
08557     static int l;
08558     static double p[3], b1, b2, b3;
08559     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08560     /* Subroutine */ int swap_(int *, int *, int *,
08561             int *, int *, int *, int *, int *);
08562     static int lpo1s;
08563     /* Subroutine */ int bdyadd_(int *, int *, int *,
08564             int *, int *, int *, int *), intadd_(int *,
08565             int *, int *, int *, int *, int *, int *,
08566             int *), trfind_(int *, double *, int *,
08567             double *, double *, double *, int *, int *,
08568             int *, double *, double *, double *, int *,
08569             int *, int *), covsph_(int *, int *, int *,
08570             int *, int *, int *);
08571     int lstptr_(int *, int *, int *, int *);
08572     long int swptst_(int *, int *, int *, int *,
08573             double *, double *, double *);
08574 
08575 
08576 /* *********************************************************** */
08577 
08578 /*                                              From STRIPACK */
08579 /*                                            Robert J. Renka */
08580 /*                                  Dept. of Computer Science */
08581 /*                                       Univ. of North Texas */
08582 /*                                           renka@cs.unt.edu */
08583 /*                                                   01/08/03 */
08584 
08585 /*   This subroutine adds node K to a triangulation of the */
08586 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08587 /* of the convex hull of nodes 1,...,K. */
08588 
08589 /*   The algorithm consists of the following steps:  node K */
08590 /* is located relative to the triangulation (TRFIND), its */
08591 /* index is added to the data structure (INTADD or BDYADD), */
08592 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08593 /* the arcs opposite K so that all arcs incident on node K */
08594 /* and opposite node K are locally optimal (satisfy the cir- */
08595 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08596 /* input, a Delaunay triangulation will result. */
08597 
08598 
08599 /* On input: */
08600 
08601 /*       NST = Index of a node at which TRFIND begins its */
08602 /*             search.  Search time depends on the proximity */
08603 /*             of this node to K.  If NST < 1, the search is */
08604 /*             begun at node K-1. */
08605 
08606 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08607 /*           new node to be added.  K .GE. 4. */
08608 
08609 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08610 /*               tesian coordinates of the nodes. */
08611 /*               (X(I),Y(I),Z(I)) defines node I for */
08612 /*               I = 1,...,K. */
08613 
08614 /* The above parameters are not altered by this routine. */
08615 
08616 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08617 /*                             the triangulation of nodes 1 */
08618 /*                             to K-1.  The array lengths are */
08619 /*                             assumed to be large enough to */
08620 /*                             add node K.  Refer to Subrou- */
08621 /*                             tine TRMESH. */
08622 
08623 /* On output: */
08624 
08625 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08626 /*                             the addition of node K as the */
08627 /*                             last entry unless IER .NE. 0 */
08628 /*                             and IER .NE. -3, in which case */
08629 /*                             the arrays are not altered. */
08630 
08631 /*       IER = Error indicator: */
08632 /*             IER =  0 if no errors were encountered. */
08633 /*             IER = -1 if K is outside its valid range */
08634 /*                      on input. */
08635 /*             IER = -2 if all nodes (including K) are col- */
08636 /*                      linear (lie on a common geodesic). */
08637 /*             IER =  L if nodes L and K coincide for some */
08638 /*                      L < K.  Refer to TOL below. */
08639 
08640 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08641 /*                                INTADD, JRAND, LSTPTR, */
08642 /*                                STORE, SWAP, SWPTST, */
08643 /*                                TRFIND */
08644 
08645 /* Intrinsic function called by ADDNOD:  ABS */
08646 
08647 /* *********************************************************** */
08648 
08649 
08650 /* Local parameters: */
08651 
08652 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08653 /*              by TRFIND. */
08654 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08655 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08656 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08657 /*              counterclockwise order. */
08658 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08659 /*              be tested for a swap */
08660 /* IST =      Index of node at which TRFIND begins its search */
08661 /* KK =       Local copy of K */
08662 /* KM1 =      K-1 */
08663 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08664 /*              if node K coincides with a vertex */
08665 /* LP =       LIST pointer */
08666 /* LPF =      LIST pointer to the first neighbor of K */
08667 /* LPO1 =     LIST pointer to IO1 */
08668 /* LPO1S =    Saved value of LPO1 */
08669 /* P =        Cartesian coordinates of node K */
08670 /* TOL =      Tolerance defining coincident nodes:  bound on */
08671 /*              the deviation from 1 of the cosine of the */
08672 /*              angle between the nodes. */
08673 /*              Note that |1-cos(A)| is approximately A*A/2. */
08674 
08675     /* Parameter adjustments */
08676     --lend;
08677     --z__;
08678     --y;
08679     --x;
08680     --list;
08681     --lptr;
08682 
08683     /* Function Body */
08684 
08685     kk = *k;
08686     if (kk < 4) {
08687         goto L3;
08688     }
08689 
08690 /* Initialization: */
08691     km1 = kk - 1;
08692     ist = *nst;
08693     if (ist < 1) {
08694         ist = km1;
08695     }
08696     p[0] = x[kk];
08697     p[1] = y[kk];
08698     p[2] = z__[kk];
08699 
08700 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08701 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08702 /*   from node K. */
08703     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08704             , &b1, &b2, &b3, &i1, &i2, &i3);
08705 
08706 /*   Test for collinear or (nearly) duplicate nodes. */
08707 
08708     if (i1 == 0) {
08709         goto L4;
08710     }
08711     l = i1;
08712     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08713         goto L5;
08714     }
08715     l = i2;
08716     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08717         goto L5;
08718     }
08719     if (i3 != 0) {
08720         l = i3;
08721         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08722             goto L5;
08723         }
08724         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08725     } else {
08726         if (i1 != i2) {
08727             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08728         } else {
08729             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08730         }
08731     }
08732     *ier = 0;
08733 
08734 /* Initialize variables for optimization of the */
08735 /*   triangulation. */
08736     lp = lend[kk];
08737     lpf = lptr[lp];
08738     io2 = list[lpf];
08739     lpo1 = lptr[lpf];
08740     io1 = (i__1 = list[lpo1], abs(i__1));
08741 
08742 /* Begin loop:  find the node opposite K. */
08743 
08744 L1:
08745     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08746     if (list[lp] < 0) {
08747         goto L2;
08748     }
08749     lp = lptr[lp];
08750     in1 = (i__1 = list[lp], abs(i__1));
08751 
08752 /* Swap test:  if a swap occurs, two new arcs are */
08753 /*             opposite K and must be tested. */
08754 
08755     lpo1s = lpo1;
08756     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08757         goto L2;
08758     }
08759     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08760     if (lpo1 == 0) {
08761 
08762 /*   A swap is not possible because KK and IN1 are already */
08763 /*     adjacent.  This error in SWPTST only occurs in the */
08764 /*     neutral case and when there are nearly duplicate */
08765 /*     nodes. */
08766 
08767         lpo1 = lpo1s;
08768         goto L2;
08769     }
08770     io1 = in1;
08771     goto L1;
08772 
08773 /* No swap occurred.  Test for termination and reset */
08774 /*   IO2 and IO1. */
08775 
08776 L2:
08777     if (lpo1 == lpf || list[lpo1] < 0) {
08778         return 0;
08779     }
08780     io2 = io1;
08781     lpo1 = lptr[lpo1];
08782     io1 = (i__1 = list[lpo1], abs(i__1));
08783     goto L1;
08784 
08785 /* KK < 4. */
08786 
08787 L3:
08788     *ier = -1;
08789     return 0;
08790 
08791 /* All nodes are collinear. */
08792 
08793 L4:
08794     *ier = -2;
08795     return 0;
08796 
08797 /* Nodes L and K coincide. */
08798 
08799 L5:
08800     *ier = l;
08801     return 0;
08802 } /* addnod_ */
08803 
08804 double angle_(double *v1, double *v2, double *v3)
08805 {
08806     /* System generated locals */
08807     double ret_val;
08808 
08809     /* Builtin functions */
08810     //double sqrt(double), acos(double);
08811 
08812     /* Local variables */
08813     static double a;
08814     static int i__;
08815     static double ca, s21, s23, u21[3], u23[3];
08816 
08817 
08818 /* *********************************************************** */
08819 
08820 /*                                              From STRIPACK */
08821 /*                                            Robert J. Renka */
08822 /*                                  Dept. of Computer Science */
08823 /*                                       Univ. of North Texas */
08824 /*                                           renka@cs.unt.edu */
08825 /*                                                   06/03/03 */
08826 
08827 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08828 /* face of the unit sphere, this function returns the */
08829 /* interior angle at V2 -- the dihedral angle between the */
08830 /* plane defined by V2 and V3 (and the origin) and the plane */
08831 /* defined by V2 and V1 or, equivalently, the angle between */
08832 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08833 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08834 /* wise.  The surface area of a spherical polygon with CCW- */
08835 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08836 /* Asum is the sum of the m interior angles computed from the */
08837 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08838 /* (Vm-1,Vm,V1). */
08839 
08840 
08841 /* On input: */
08842 
08843 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08844 /*                  sian coordinates of unit vectors.  These */
08845 /*                  vectors, if nonzero, are implicitly */
08846 /*                  scaled to have length 1. */
08847 
08848 /* Input parameters are not altered by this function. */
08849 
08850 /* On output: */
08851 
08852 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08853 /*               V2 X V3 = 0. */
08854 
08855 /* Module required by ANGLE:  LEFT */
08856 
08857 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08858 
08859 /* *********************************************************** */
08860 
08861 
08862 /* Local parameters: */
08863 
08864 /* A =       Interior angle at V2 */
08865 /* CA =      cos(A) */
08866 /* I =       DO-loop index and index for U21 and U23 */
08867 /* S21,S23 = Sum of squared components of U21 and U23 */
08868 /* U21,U23 = Unit normal vectors to the planes defined by */
08869 /*             pairs of triangle vertices */
08870 
08871 
08872 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08873 
08874     /* Parameter adjustments */
08875     --v3;
08876     --v2;
08877     --v1;
08878 
08879     /* Function Body */
08880     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08881     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08882     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08883 
08884     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08885     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08886     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08887 
08888 /* Normalize U21 and U23 to unit vectors. */
08889 
08890     s21 = 0.;
08891     s23 = 0.;
08892     for (i__ = 1; i__ <= 3; ++i__) {
08893         s21 += u21[i__ - 1] * u21[i__ - 1];
08894         s23 += u23[i__ - 1] * u23[i__ - 1];
08895 /* L1: */
08896     }
08897 
08898 /* Test for a degenerate triangle associated with collinear */
08899 /*   vertices. */
08900 
08901     if (s21 == 0. || s23 == 0.) {
08902         ret_val = 0.;
08903         return ret_val;
08904     }
08905     s21 = sqrt(s21);
08906     s23 = sqrt(s23);
08907     for (i__ = 1; i__ <= 3; ++i__) {
08908         u21[i__ - 1] /= s21;
08909         u23[i__ - 1] /= s23;
08910 /* L2: */
08911     }
08912 
08913 /* Compute the angle A between normals: */
08914 
08915 /*   CA = cos(A) = <U21,U23> */
08916 
08917     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08918     if (ca < -1.) {
08919         ca = -1.;
08920     }
08921     if (ca > 1.) {
08922         ca = 1.;
08923     }
08924     a = acos(ca);
08925 
08926 /* Adjust A to the interior angle:  A > Pi iff */
08927 /*   V3 Right V1->V2. */
08928 
08929     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08930             , &v3[3])) {
08931         a = acos(-1.) * 2. - a;
08932     }
08933     ret_val = a;
08934     return ret_val;
08935 } /* angle_ */
08936 
08937 double areas_(double *v1, double *v2, double *v3)
08938 {
08939     /* System generated locals */
08940     double ret_val;
08941 
08942     /* Builtin functions */
08943     //double sqrt(double), acos(double);
08944 
08945     /* Local variables */
08946     static int i__;
08947     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08948             ca2, ca3;
08949 
08950 
08951 /* *********************************************************** */
08952 
08953 /*                                              From STRIPACK */
08954 /*                                            Robert J. Renka */
08955 /*                                  Dept. of Computer Science */
08956 /*                                       Univ. of North Texas */
08957 /*                                           renka@cs.unt.edu */
08958 /*                                                   06/22/98 */
08959 
08960 /*   This function returns the area of a spherical triangle */
08961 /* on the unit sphere. */
08962 
08963 
08964 /* On input: */
08965 
08966 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08967 /*                  sian coordinates of unit vectors (the */
08968 /*                  three triangle vertices in any order). */
08969 /*                  These vectors, if nonzero, are implicitly */
08970 /*                  scaled to have length 1. */
08971 
08972 /* Input parameters are not altered by this function. */
08973 
08974 /* On output: */
08975 
08976 /*       AREAS = Area of the spherical triangle defined by */
08977 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08978 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08979 /*               if and only if V1, V2, and V3 lie in (or */
08980 /*               close to) a plane containing the origin. */
08981 
08982 /* Modules required by AREAS:  None */
08983 
08984 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08985 
08986 /* *********************************************************** */
08987 
08988 
08989 /* Local parameters: */
08990 
08991 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08992 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08993 /* I =           DO-loop index and index for Uij */
08994 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08995 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08996 /*                 pairs of triangle vertices */
08997 
08998 
08999 /* Compute cross products Uij = Vi X Vj. */
09000 
09001     /* Parameter adjustments */
09002     --v3;
09003     --v2;
09004     --v1;
09005 
09006     /* Function Body */
09007     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
09008     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
09009     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
09010 
09011     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
09012     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
09013     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
09014 
09015     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
09016     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
09017     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
09018 
09019 /* Normalize Uij to unit vectors. */
09020 
09021     s12 = 0.;
09022     s23 = 0.;
09023     s31 = 0.;
09024     for (i__ = 1; i__ <= 3; ++i__) {
09025         s12 += u12[i__ - 1] * u12[i__ - 1];
09026         s23 += u23[i__ - 1] * u23[i__ - 1];
09027         s31 += u31[i__ - 1] * u31[i__ - 1];
09028 /* L2: */
09029     }
09030 
09031 /* Test for a degenerate triangle associated with collinear */
09032 /*   vertices. */
09033 
09034     if (s12 == 0. || s23 == 0. || s31 == 0.) {
09035         ret_val = 0.;
09036         return ret_val;
09037     }
09038     s12 = sqrt(s12);
09039     s23 = sqrt(s23);
09040     s31 = sqrt(s31);
09041     for (i__ = 1; i__ <= 3; ++i__) {
09042         u12[i__ - 1] /= s12;
09043         u23[i__ - 1] /= s23;
09044         u31[i__ - 1] /= s31;
09045 /* L3: */
09046     }
09047 
09048 /* Compute interior angles Ai as the dihedral angles between */
09049 /*   planes: */
09050 /*           CA1 = cos(A1) = -<U12,U31> */
09051 /*           CA2 = cos(A2) = -<U23,U12> */
09052 /*           CA3 = cos(A3) = -<U31,U23> */
09053 
09054     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
09055     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
09056     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
09057     if (ca1 < -1.) {
09058         ca1 = -1.;
09059     }
09060     if (ca1 > 1.) {
09061         ca1 = 1.;
09062     }
09063     if (ca2 < -1.) {
09064         ca2 = -1.;
09065     }
09066     if (ca2 > 1.) {
09067         ca2 = 1.;
09068     }
09069     if (ca3 < -1.) {
09070         ca3 = -1.;
09071     }
09072     if (ca3 > 1.) {
09073         ca3 = 1.;
09074     }
09075     a1 = acos(ca1);
09076     a2 = acos(ca2);
09077     a3 = acos(ca3);
09078 
09079 /* Compute AREAS = A1 + A2 + A3 - PI. */
09080 
09081     ret_val = a1 + a2 + a3 - acos(-1.);
09082     if (ret_val < 0.) {
09083         ret_val = 0.;
09084     }
09085     return ret_val;
09086 } /* areas_ */
09087 
09088 //double areas_(double *, double *, double *);
09089 
09090 double Util::areav_(int *k, int *n, double *x, double *y,
09091         double *z__, int *list, int *lptr, int *lend, int
09092         *ier)
09093 {
09094     /* Initialized data */
09095 
09096     static double amax = 6.28;
09097 
09098     /* System generated locals */
09099     double ret_val;
09100 
09101     /* Local variables */
09102     static double a, c0[3], c2[3], c3[3];
09103     static int n1, n2, n3;
09104     static double v1[3], v2[3], v3[3];
09105     static int lp, lpl, ierr;
09106     static double asum;
09107     static long int first;
09108 
09109 
09110 /* *********************************************************** */
09111 
09112 /*                                            Robert J. Renka */
09113 /*                                  Dept. of Computer Science */
09114 /*                                       Univ. of North Texas */
09115 /*                                           renka@cs.unt.edu */
09116 /*                                                   10/25/02 */
09117 
09118 /*   Given a Delaunay triangulation and the index K of an */
09119 /* interior node, this subroutine returns the (surface) area */
09120 /* of the Voronoi region associated with node K.  The Voronoi */
09121 /* region is the polygon whose vertices are the circumcenters */
09122 /* of the triangles that contain node K, where a triangle */
09123 /* circumcenter is the point (unit vector) lying at the same */
09124 /* angular distance from the three vertices and contained in */
09125 /* the same hemisphere as the vertices. */
09126 
09127 
09128 /* On input: */
09129 
09130 /*       K = Nodal index in the range 1 to N. */
09131 
09132 /*       N = Number of nodes in the triangulation.  N > 3. */
09133 
09134 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09135 /*               coordinates of the nodes (unit vectors). */
09136 
09137 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09138 /*                        gulation.  Refer to Subroutine */
09139 /*                        TRMESH. */
09140 
09141 /* Input parameters are not altered by this function. */
09142 
09143 /* On output: */
09144 
09145 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09146 /*               in which case AREAV = 0. */
09147 
09148 /*       IER = Error indicator: */
09149 /*             IER = 0 if no errors were encountered. */
09150 /*             IER = 1 if K or N is outside its valid range */
09151 /*                     on input. */
09152 /*             IER = 2 if K indexes a boundary node. */
09153 /*             IER = 3 if an error flag is returned by CIRCUM */
09154 /*                     (null triangle). */
09155 /*             IER = 4 if AREAS returns a value greater than */
09156 /*                     AMAX (defined below). */
09157 
09158 /* Modules required by AREAV:  AREAS, CIRCUM */
09159 
09160 /* *********************************************************** */
09161 
09162 
09163 /* Maximum valid triangle area is less than 2*Pi: */
09164 
09165     /* Parameter adjustments */
09166     --lend;
09167     --z__;
09168     --y;
09169     --x;
09170     --list;
09171     --lptr;
09172 
09173     /* Function Body */
09174 
09175 /* Test for invalid input. */
09176 
09177     if (*k < 1 || *k > *n || *n <= 3) {
09178         goto L11;
09179     }
09180 
09181 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09182 /*   FIRST = TRUE only for the first triangle. */
09183 /*   The Voronoi region area is accumulated in ASUM. */
09184 
09185     n1 = *k;
09186     v1[0] = x[n1];
09187     v1[1] = y[n1];
09188     v1[2] = z__[n1];
09189     lpl = lend[n1];
09190     n3 = list[lpl];
09191     if (n3 < 0) {
09192         goto L12;
09193     }
09194     lp = lpl;
09195     first = TRUE_;
09196     asum = 0.;
09197 
09198 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09199 
09200 L1:
09201     n2 = n3;
09202     lp = lptr[lp];
09203     n3 = list[lp];
09204     v2[0] = x[n2];
09205     v2[1] = y[n2];
09206     v2[2] = z__[n2];
09207     v3[0] = x[n3];
09208     v3[1] = y[n3];
09209     v3[2] = z__[n3];
09210     if (first) {
09211 
09212 /* First triangle:  compute the circumcenter C3 and save a */
09213 /*   copy in C0. */
09214 
09215         circum_(v1, v2, v3, c3, &ierr);
09216         if (ierr != 0) {
09217             goto L13;
09218         }
09219         c0[0] = c3[0];
09220         c0[1] = c3[1];
09221         c0[2] = c3[2];
09222         first = FALSE_;
09223     } else {
09224 
09225 /* Set C2 to C3, compute the new circumcenter C3, and compute */
09226 /*   the area A of triangle (V1,C2,C3). */
09227 
09228         c2[0] = c3[0];
09229         c2[1] = c3[1];
09230         c2[2] = c3[2];
09231         circum_(v1, v2, v3, c3, &ierr);
09232         if (ierr != 0) {
09233             goto L13;
09234         }
09235         a = areas_(v1, c2, c3);
09236         if (a > amax) {
09237             goto L14;
09238         }
09239         asum += a;
09240     }
09241 
09242 /* Bottom on loop on neighbors of K. */
09243 
09244     if (lp != lpl) {
09245         goto L1;
09246     }
09247 
09248 /* Compute the area of triangle (V1,C3,C0). */
09249 
09250     a = areas_(v1, c3, c0);
09251     if (a > amax) {
09252         goto L14;
09253     }
09254     asum += a;
09255 
09256 /* No error encountered. */
09257 
09258     *ier = 0;
09259     ret_val = asum;
09260     return ret_val;
09261 
09262 /* Invalid input. */
09263 
09264 L11:
09265     *ier = 1;
09266     ret_val = 0.;
09267     return ret_val;
09268 
09269 /* K indexes a boundary node. */
09270 
09271 L12:
09272     *ier = 2;
09273     ret_val = 0.;
09274     return ret_val;
09275 
09276 /* Error in CIRCUM. */
09277 
09278 L13:
09279     *ier = 3;
09280     ret_val = 0.;
09281     return ret_val;
09282 
09283 /* AREAS value larger than AMAX. */
09284 
09285 L14:
09286     *ier = 4;
09287     ret_val = 0.;
09288     return ret_val;
09289 } /* areav_ */
09290 
09291 double areav_new__(int *k, int *n, double *x, double *y,
09292         double *z__, int *list, int *lptr, int *lend, int
09293         *ier)
09294 {
09295     /* System generated locals */
09296     double ret_val = 0;
09297 
09298     /* Builtin functions */
09299     //double acos(double);
09300 
09301     /* Local variables */
09302     static int m;
09303     static double c1[3], c2[3], c3[3];
09304     static int n1, n2, n3;
09305     static double v1[3], v2[3], v3[3];
09306     static int lp;
09307     static double c1s[3], c2s[3];
09308     static int lpl, ierr;
09309     static double asum;
09310     double angle_(double *, double *, double *);
09311     static float areav;
09312 
09313 
09314 /* *********************************************************** */
09315 
09316 /*                                            Robert J. Renka */
09317 /*                                  Dept. of Computer Science */
09318 /*                                       Univ. of North Texas */
09319 /*                                           renka@cs.unt.edu */
09320 /*                                                   06/03/03 */
09321 
09322 /*   Given a Delaunay triangulation and the index K of an */
09323 /* interior node, this subroutine returns the (surface) area */
09324 /* of the Voronoi region associated with node K.  The Voronoi */
09325 /* region is the polygon whose vertices are the circumcenters */
09326 /* of the triangles that contain node K, where a triangle */
09327 /* circumcenter is the point (unit vector) lying at the same */
09328 /* angular distance from the three vertices and contained in */
09329 /* the same hemisphere as the vertices.  The Voronoi region */
09330 /* area is computed as Asum-(m-2)*Pi, where m is the number */
09331 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
09332 /* of interior angles at the vertices. */
09333 
09334 
09335 /* On input: */
09336 
09337 /*       K = Nodal index in the range 1 to N. */
09338 
09339 /*       N = Number of nodes in the triangulation.  N > 3. */
09340 
09341 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09342 /*               coordinates of the nodes (unit vectors). */
09343 
09344 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09345 /*                        gulation.  Refer to Subroutine */
09346 /*                        TRMESH. */
09347 
09348 /* Input parameters are not altered by this function. */
09349 
09350 /* On output: */
09351 
09352 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09353 /*               in which case AREAV = 0. */
09354 
09355 /*       IER = Error indicator: */
09356 /*             IER = 0 if no errors were encountered. */
09357 /*             IER = 1 if K or N is outside its valid range */
09358 /*                     on input. */
09359 /*             IER = 2 if K indexes a boundary node. */
09360 /*             IER = 3 if an error flag is returned by CIRCUM */
09361 /*                     (null triangle). */
09362 
09363 /* Modules required by AREAV:  ANGLE, CIRCUM */
09364 
09365 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09366 
09367 /* *********************************************************** */
09368 
09369 
09370 /* Test for invalid input. */
09371 
09372     /* Parameter adjustments */
09373     --lend;
09374     --z__;
09375     --y;
09376     --x;
09377     --list;
09378     --lptr;
09379 
09380     /* Function Body */
09381     if (*k < 1 || *k > *n || *n <= 3) {
09382         goto L11;
09383     }
09384 
09385 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09386 /*   The number of neighbors and the sum of interior angles */
09387 /*   are accumulated in M and ASUM, respectively. */
09388 
09389     n1 = *k;
09390     v1[0] = x[n1];
09391     v1[1] = y[n1];
09392     v1[2] = z__[n1];
09393     lpl = lend[n1];
09394     n3 = list[lpl];
09395     if (n3 < 0) {
09396         goto L12;
09397     }
09398     lp = lpl;
09399     m = 0;
09400     asum = 0.;
09401 
09402 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09403 
09404 L1:
09405     ++m;
09406     n2 = n3;
09407     lp = lptr[lp];
09408     n3 = list[lp];
09409     v2[0] = x[n2];
09410     v2[1] = y[n2];
09411     v2[2] = z__[n2];
09412     v3[0] = x[n3];
09413     v3[1] = y[n3];
09414     v3[2] = z__[n3];
09415     if (m == 1) {
09416 
09417 /* First triangle:  compute the circumcenter C2 and save a */
09418 /*   copy in C1S. */
09419 
09420         circum_(v1, v2, v3, c2, &ierr);
09421         if (ierr != 0) {
09422             goto L13;
09423         }
<