psychlops/extension/standard/fft/psychlops_FFTW_bridge.cpp

   1 /*\r
   2  *  psychlops_FFTW_bridge.cpp\r
   3  *  Psychlops Standard Library (Universal)\r
   4  *\r
   5  *  Last Modified 2010/03/05 by Kenchi HOSOKAWA\r
   6  *  (C) 2010 Kenchi HOSOKAWA, Kazushi MARUYA, Takao SATO\r
   7  */\r
   8 \r
   9 \r
  10 #include "../../../psychlops_core.h"\r
  11 #include "psychlops_fftw_bridge.h"\r
  12 #include "fftw3.h"\r
  13 \r
  14 \r
  15 \r
  16 namespace Psychlops {\r
  17 \r
  18         double sigmoid(double x, double d, double a) { return 1 / ( 1 + pow(Math::E, -(a*(x-d))) ); }\r
  19         double log2NormalDistibution(double log_x, double octave_mu, double octave_sigma) { return Math::normalDistibution(log(log_x)*Math::LOG2E, octave_mu, octave_sigma); }\r
  20         double cumulativeLog2NormalDistibution(double log_x, double octave_mu, double octave_sigma) { return Math::cumulativeNormalDistibution(log(log_x)*Math::LOG2E, octave_mu, octave_sigma); }\r
  21         enum { REAL, IMAGINARY };\r
  22 \r
  23 \r
  24 \r
  25 \r
  26         FFT2::FFT2()\r
  27         {\r
  28                 construct_default();\r
  29         }\r
  30         FFT2::FFT2(int wid, int hei)\r
  31         {\r
  32                 construct_default();\r
  33                 set(wid, hei);\r
  34         }\r
  35         FFT2::FFT2(const Image &source)\r
  36         {\r
  37                 construct_default();\r
  38                 set(source);\r
  39         }\r
  40         FFT2::FFT2(const Matrix &source)\r
  41         {\r
  42                 construct_default();\r
  43                 set(source);\r
  44         }\r
  45         FFT2::FFT2(const Matrix &reali, const Matrix &imagi)\r
  46         {\r
  47                 width_=0; height_=0;\r
  48                 set(reali, imagi);\r
  49         }\r
  50         FFT2::~FFT2()\r
  51         {\r
  52                 release();\r
  53         }\r
  54         void FFT2::construct_default()\r
  55         {\r
  56                 width_=0; height_=0;\r
  57                 img_spc=0; frq_spc=0;\r
  58         }\r
  59         void FFT2::release()\r
  60         {\r
  61                 if(img_spc!=0) { free(img_spc); img_spc = 0; }\r
  62                 if(frq_spc!=0) { free(frq_spc); frq_spc = 0; }\r
  63                 width_=0; height_=0;\r
  64         }\r
  65         void FFT2::set(int wid, int hei)\r
  66         {\r
  67                 if(img_spc==0 || frq_spc==0 || (width_*height_ != wid*hei) ) {\r
  68                         release();\r
  69                         img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei);\r
  70                         frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei);\r
  71                 }\r
  72                 width_  = wid;\r
  73                 height_ = hei;\r
  74                 x_zero = wid + 1;\r
  75                 y_zero = hei + 1;\r
  76                 left = (wid)/2;\r
  77                 top = (hei)/2;\r
  78         }\r
  79         void FFT2::set(const Image &source)\r
  80         {\r
  81                 set(source.getWidth(), source.getHeight());\r
  82                 for(int y=0; y<height_; y++) {\r
  83                         for(int x=0; x<width_; x++) {\r
  84                                 img_spc[x + width_*y][REAL] = source.getPix(x, y).getR();\r
  85                                 img_spc[x + width_*y][IMAGINARY] = 0.0;\r
  86                         }\r
  87                 }\r
  88         }\r
  89         void FFT2::set(const Matrix &source)\r
  90         {\r
  91                 set(source.getCols(), source.getRows());\r
  92                 for(int y=0; y<height_; y++) {\r
  93                         for(int x=0; x<width_; x++) {\r
  94                                 img_spc[x + width_*y][REAL] = source(y+1, x+1);\r
  95                                 img_spc[x + width_*y][IMAGINARY] = 0.0;\r
  96                         }\r
  97                 }\r
  98         }\r
  99         void FFT2::set(const Matrix &reali, const Matrix &imagi)\r
 100         {\r
 101                 set(reali.getCols(), reali.getRows());\r
 102                 for(int y=0; y<height_; y++) {\r
 103                         for(int x=0; x<width_; x++) {\r
 104                                 img_spc[x + width_*y][REAL] = reali(y+1, x+1);\r
 105                                 img_spc[x + width_*y][IMAGINARY] = imagi(y+1, x+1);\r
 106                         }\r
 107                 }\r
 108         }\r
 109         void FFT2::setSpectrum(const Matrix &reali, const Matrix &imagi)\r
 110         {\r
 111                 set(reali.getCols(), reali.getRows());\r
 112                 for(int y=0; y<height_; y++) {\r
 113                         for(int x=0; x<width_; x++) {\r
 114                                 frq_spc[x + width_*y][REAL] = reali(y+1, x+1);\r
 115                                 frq_spc[x + width_*y][IMAGINARY] = imagi(y+1, x+1);\r
 116                         }\r
 117                 }\r
 118         }\r
 119 \r
 120         int FFT2::freqX(int x) const\r
 121         {\r
 122                 return x<0 ? x_zero+x : x;\r
 123         }\r
 124         int FFT2::freqY(int y) const\r
 125         {\r
 126                 return y<0 ? y_zero+y : y;\r
 127         }\r
 128 \r
 129         double FFT2::pix(int x, int y, double l)\r
 130         {\r
 131                 return img_spc[x + width_*y][REAL] = l;\r
 132         }\r
 133         double FFT2::pix(int x, int y, const Color &c)\r
 134         {\r
 135                 return img_spc[x + width_*y][REAL] = c.getR();\r
 136         }\r
 137         double FFT2::getPix(int x, int y)\r
 138         {\r
 139                 return img_spc[x + width_*y][REAL];\r
 140         }\r
 141 \r
 142         double FFT2::getDC()\r
 143         {\r
 144                 return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);\r
 145         }\r
 146         double FFT2::setDC(double l)\r
 147         {\r
 148                 frq_spc[0][REAL] = l;\r
 149                 frq_spc[0][IMAGINARY] = 0;\r
 150                 return l;\r
 151         }\r
 152 \r
 153 \r
 154         void FFT2::fft()\r
 155         {\r
 156                 fftw_plan plan = fftw_plan_dft_2d(height_, width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);\r
 157                 fftw_execute(plan);\r
 158                 normalizeFFT();\r
 159         }\r
 160         void FFT2::ifft()\r
 161         {\r
 162                 fftw_plan plan = fftw_plan_dft_2d(height_, width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);\r
 163                 fftw_execute(plan);\r
 164         }\r
 165         void FFT2::normalizeFFT()\r
 166         {\r
 167                 double num = height_*width_;\r
 168                 for (int i=0; i<num; i++) {\r
 169                         frq_spc[i][REAL] /= num;\r
 170                         frq_spc[i][IMAGINARY] /= num;\r
 171                 }\r
 172         }\r
 173         void FFT2::getImage(Image &target)\r
 174         {\r
 175                 double rl, im;\r
 176                 Color col;\r
 177                 if( !target.hasInstance() || (target.getWidth()!=width_) || (target.getHeight()!=height_) ) {\r
 178                         target.release();\r
 179                         target.set(width_, height_);\r
 180                 }\r
 181                 for (int y=0; y<height_; y++) {\r
 182                         for (int x=0; x<width_; x++) {\r
 183                                 rl = img_spc[x + width_*y][REAL];\r
 184                                 im = img_spc[x + width_*y][IMAGINARY];\r
 185                                 col.set(sqrt(rl*rl+im*im));\r
 186                                 target.pix(x, y, col);\r
 187                         }\r
 188                 }\r
 189         }\r
 190         void FFT2::getSpectrum(Image &target, double gamma)\r
 191         {\r
 192                 const int bottom = height_%2==0 ? top : top+1;\r
 193                 const int right = width_%2==0 ? left : left+1;\r
 194                 double rl, im;\r
 195                 Color col;\r
 196                 if( !target.hasInstance() || (target.getWidth()!=width_) || (target.getHeight()!=height_) ) {\r
 197                         target.release();\r
 198                         target.set(width_, height_);\r
 199                 }\r
 200                 int h, v;\r
 201                 for (int y=0; y<height_; y++) {\r
 202                         v = (y<bottom ? y+top : y-bottom);\r
 203                         for (int x=0; x<right; x++) {\r
 204                                 h = x+left;\r
 205                                 rl = frq_spc[x + width_*y][REAL];\r
 206                                 im = frq_spc[x + width_*y][IMAGINARY];\r
 207                                 col.set(pow( sqrt(rl*rl+im*im), gamma ));\r
 208                                 target.pix(h, v, col);\r
 209                         }\r
 210                         for (int x=right; x<width_; x++) {\r
 211                                 h = x-right;\r
 212                                 rl = frq_spc[x + width_*y][REAL];\r
 213                                 im = frq_spc[x + width_*y][IMAGINARY];\r
 214                                 col.set(pow( sqrt(rl*rl+im*im), gamma ));\r
 215                                 target.pix(h, v, col);\r
 216                         }\r
 217                 }\r
 218         }\r
 219         void FFT2::getImage(Matrix &target)\r
 220         {\r
 221                 double rl, im;\r
 222                 int h, v;\r
 223                 if( (target.getCols()!=width_) || (target.getRows()!=height_) ) {\r
 224                         target.release();\r
 225                         target.set(height_, width_);\r
 226                 }\r
 227                 for (int y=0; y<height_; y++) {\r
 228                         v = y+1;\r
 229                         for (int x=0; x<width_; x++) {\r
 230                                 rl = img_spc[x + width_*y][REAL];\r
 231                                 im = img_spc[x + width_*y][IMAGINARY];\r
 232                                 target(v, x+1) = sqrt(rl*rl+im*im);\r
 233                         }\r
 234                 }\r
 235         }\r
 236         void FFT2::getSpectrum(Matrix &target, double gamma)\r
 237         {\r
 238                 const int bottom = height_%2==0 ? top : top+1;\r
 239                 const int right = width_%2==0 ? left : left+1;\r
 240                 double rl, im;\r
 241                 Color col;\r
 242                 if( (target.getCols()!=width_) || (target.getRows()!=height_) ) {\r
 243                         target.release();\r
 244                         target.set(height_, width_);\r
 245                 }\r
 246                 int h, v;\r
 247                 for (int y=0; y<height_; y++) {\r
 248                         v = (y<bottom ? y+top : y-bottom) + 1;\r
 249                         for (int x=0; x<right; x++) {\r
 250                                 h = x+left + 1;\r
 251                                 rl = frq_spc[x + width_*y][REAL];\r
 252                                 im = frq_spc[x + width_*y][IMAGINARY];\r
 253                                 target(v, h) = pow( sqrt(rl*rl+im*im), gamma );\r
 254                         }\r
 255                         for (int x=right; x<width_; x++) {\r
 256                                 h = x-right + 1;\r
 257                                 rl = frq_spc[x + width_*y][REAL];\r
 258                                 im = frq_spc[x + width_*y][IMAGINARY];\r
 259                                 target(v, h) = pow( sqrt(rl*rl+im*im), gamma );\r
 260                         }\r
 261                 }\r
 262                 /*for (int y=0; y<height_; y++) {\r
 263                         v = y+1;\r
 264                         for (int x=0; x<width_; x++) {\r
 265                                 rl = frq_spc[x + width_*y][REAL];\r
 266                                 im = frq_spc[x + width_*y][IMAGINARY];\r
 267                                 col.set(rl);\r
 268                                 target(v, x+1) = pow( sqrt(rl*rl+im*im), gamma );\r
 269                         }\r
 270                 }*/\r
 271         }\r
 272         void FFT2::getImage(Matrix &reali, Matrix &imagi)\r
 273         {\r
 274                 if( (reali.getCols()!=width_) || (reali.getRows()!=height_) ) { reali.release(); reali.set(height_, width_); }\r
 275                 if( (imagi.getCols()!=width_) || (imagi.getRows()!=height_) ) { imagi.release(); imagi.set(height_, width_); }\r
 276                 for (int y=0; y<height_; y++) {\r
 277                         for (int x=0; x<width_; x++) {\r
 278                                 reali(y+1, x+1) = img_spc[x + width_*y][REAL];\r
 279                                 imagi(y+1, x+1) = img_spc[x + width_*y][IMAGINARY];\r
 280                         }\r
 281                 }\r
 282         }\r
 283         void FFT2::getSpectrum(Matrix &reali, Matrix &imagi)\r
 284         {\r
 285                 const int bottom = height_%2==0 ? top : top+1;\r
 286                 const int right = width_%2==0 ? left : left+1;\r
 287                 if( (reali.getCols()!=width_) || (reali.getRows()!=height_) ) { reali.release(); reali.set(height_, width_); }\r
 288                 if( (imagi.getCols()!=width_) || (imagi.getRows()!=height_) ) { imagi.release(); imagi.set(height_, width_); }\r
 289                 int h, v;\r
 290                 for (int y=0; y<height_; y++) {\r
 291                         v = (y<bottom ? y+top : y-bottom) + 1;\r
 292                         for (int x=0; x<width_; x++) {\r
 293                                 reali(y+1, x+1) = frq_spc[x + width_*y][REAL];\r
 294                                 imagi(y+1, x+1) = frq_spc[x + width_*y][IMAGINARY];\r
 295                         }\r
 296                 }\r
 297         }\r
 298         void FFT2::filtering(const Matrix &filter)\r
 299         {\r
 300                 const int bottom = height_%2==0 ? top : top+1;\r
 301                 const int right = width_%2==0 ? left : left+1;\r
 302                 int h, v;\r
 303                 double factor;\r
 304                 //std::cout << width_ << ":" << left << " " << height_ << ":" << top << "*" << bottom << std::endl;\r
 305                 for (int y=0; y<height_; y++) {\r
 306                         v = (y<bottom ? y+top : y-bottom) + 1;\r
 307                         //std::cout << v << ":" << y << "  ";\r
 308                         if(v<=0 || v>height_) exit(v);\r
 309                         for (int x=0; x<right; x++) {\r
 310                                 h = x+left + 1;\r
 311                                 //std::cout << h << ":" << x << "  ";\r
 312                                 factor = filter(v, h);\r
 313                                 frq_spc[x + width_*y][REAL] *= factor;\r
 314                                 frq_spc[x + width_*y][IMAGINARY] *= factor;\r
 315                         }\r
 316                         for (int x=right; x<width_; x++) {\r
 317                                 h = x-right + 1;\r
 318                                 //std::cout << h << ":" << x << "  ";\r
 319                                 factor = filter(v, h);\r
 320                                 frq_spc[x + width_*y][REAL] *= factor;\r
 321                                 frq_spc[x + width_*y][IMAGINARY] *= factor;\r
 322                         }\r
 323                         //std::cout << std::endl;\r
 324                 }\r
 325         }\r
 326         void FFT2::filtering(double cutoff_lambda1, double cutoff_lambda2, double slope)\r
 327         {\r
 328                 Matrix filter = makeFilter(height_, width_, cutoff_lambda1, cutoff_lambda2, slope);\r
 329                 filtering(filter);\r
 330         }\r
 331 \r
 332         double gaussian(const double x, const double sigma) { return exp( -(x*x) / (2.0*sigma*sigma) ); }\r
 333         void FFT2::getSpectrumExec(FFT2 &tmp, Image &result, double gamma)\r
 334         {\r
 335                 double xx,yy;\r
 336                 for (int y=0; y<tmp.height_; y++) {\r
 337                         yy = (double)(y-tmp.top)/tmp.height_;\r
 338                         for (int x=0; x<tmp.width_; x++) {\r
 339                                 xx = (double)(x-tmp.left)/tmp.width_;\r
 340                                 tmp.img_spc[x + tmp.width_*y][REAL] = (tmp.img_spc[x + tmp.width_*y][REAL]-.5) * gaussian(sqrt(xx*xx+yy*yy), 0.125) + 0.5;\r
 341                         }\r
 342                 }\r
 343                 tmp.fft();\r
 344                 tmp.getSpectrum(result, gamma);\r
 345         }\r
 346         void FFT2::getSpectrum(const Image &source, Image &result, double gamma)\r
 347         {\r
 348                 FFT2 tmp(source);\r
 349                 getSpectrumExec(tmp, result, gamma);\r
 350         }\r
 351         void FFT2::getSpectrum(const Matrix &source, Image &result, double gamma)\r
 352         {\r
 353                 FFT2 tmp(source);\r
 354                 getSpectrumExec(tmp, result, gamma);\r
 355         }\r
 356         void FFT2::getRawSpectrumExec(FFT2 &tmp, Image &reali, Image &imagi, double gamma)\r
 357         {\r
 358                 tmp.fft();\r
 359                 tmp.getSpectrum(reali, imagi, gamma);\r
 360         }\r
 361         void FFT2::getSpectrum(const Image &source, Image &reali, Image &imagi, double gamma)\r
 362         {\r
 363                 FFT2 tmp(source);\r
 364                 getRawSpectrumExec(tmp, reali, imagi, gamma);\r
 365         }\r
 366         void FFT2::getSpectrum(const Matrix &source, Image &reali, Image &imagi, double gamma)\r
 367         {\r
 368                 FFT2 tmp(source);\r
 369                 getRawSpectrumExec(tmp, reali, imagi, gamma);\r
 370         }\r
 371 \r
 372         void FFT2::filterImageExec(FFT2 &tmp, Image &result, const Matrix &carnel)\r
 373         {\r
 374                 tmp.fft();\r
 375                 tmp.filtering(carnel);\r
 376                 tmp.ifft();\r
 377                 tmp.getImage(result);\r
 378         }\r
 379         void FFT2::filterImage(const Image &source, Image &result, const Matrix &carnel)\r
 380         {\r
 381                 FFT2 tmp(source);\r
 382                 filterImageExec(tmp, result, carnel);\r
 383         }\r
 384         void FFT2::filterImage(const Matrix &source, Image &result, const Matrix &carnel)\r
 385         {\r
 386                 FFT2 tmp(source);\r
 387                 filterImageExec(tmp, result, carnel);\r
 388         }\r
 389         void FFT2::filterImage(const Image &source, Image &result, double cutoff_lambda1, double cutoff_lambda2, double slope)\r
 390         {\r
 391                 FFT2 tmp(source);\r
 392                 Matrix carnel = makeFilter(tmp.height_, tmp.width_, cutoff_lambda1, cutoff_lambda2, slope);\r
 393                 filterImageExec(tmp, result, carnel);\r
 394         }\r
 395         void FFT2::filterImage(const Matrix &source, Image &result, double cutoff_lambda1, double cutoff_lambda2, double slope)\r
 396         {\r
 397                 FFT2 tmp(source);\r
 398                 Matrix carnel = makeFilter(tmp.height_, tmp.width_, cutoff_lambda1, cutoff_lambda2, slope);\r
 399                 filterImageExec(tmp, result, carnel);\r
 400         }\r
 401 \r
 402         Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double slope)\r
 403         {\r
 404                 Matrix filter(height, width);\r
 405 \r
 406                 const double originY = (height)/2+1;\r
 407                 const double originX = (width )/2+1;\r
 408                 const double cutoffL = cutoff_lambda1;\r
 409                 const double cutoffH = cutoff_lambda2;\r
 410                 const double half = (cutoffL + cutoffH)/2.0;\r
 411                 double modulus = 0.0;\r
 412                 double sslope = slope<=0.0 ? 0.0000000000000001 : slope;\r
 413                 double value;\r
 414 \r
 415                 if(cutoffL<=cutoffH) {\r
 416                         for(int y=1; y<=height; y++) {\r
 417                                 for(int x=1; x<=width; x++) {\r
 418                                         modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));\r
 419                                         if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope);\r
 420                                         else if(modulus<=cutoffH) filter(y, x) = 1.0;\r
 421                                         else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope);\r
 422                                         //if(filter(y,x)<0)filter(y,x)=0;\r
 423                                 }\r
 424                         }\r
 425                 } else {\r
 426                         for(int y=1; y<=height; y++) {\r
 427                                 for(int x=1; x<=width; x++) {\r
 428                                         modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));\r
 429                                         if(modulus<cutoffH) value = 1.0;\r
 430                                         else if(modulus<half) value = (pow(1.0/(modulus-cutoffH+1.0), 1.0/sslope));\r
 431                                         else if(modulus<cutoffL) value = (pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope));\r
 432                                         else value = 1.0;\r
 433                                         filter(y, x) = value;\r
 434                                         //if(filter(y,x)<0)filter(y,x)=0;\r
 435                                 }\r
 436                         }\r
 437                 }\r
 438                 filter(originY, originX) = 1.0;\r
 439                 return filter;\r
 440         }\r
 441         Matrix FFT2::makeFilterSector(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double theta1, double theta2, double half_power_width, double half_power_width_ori)\r
 442         {\r
 443                 Matrix filter(height, width);\r
 444 \r
 445                 const double originY = (height)/2+1;\r
 446                 const double originX = (width )/2+1;\r
 447                 const double cutoffL = cutoff_lambda1;\r
 448                 const double cutoffH = cutoff_lambda2;\r
 449                 const double half = (cutoffL + cutoffH)/2;\r
 450                 double modulus = 0.0;\r
 451                 double sslope = half_power_width<=0.0 ? 0.0000000001 : half_power_width;\r
 452                 double oslope = half_power_width<=0.0 ? 0.0000000001 : half_power_width_ori;\r
 453                 const double cutoffOL = 0, cutoffOH = Math::mod(theta2-theta1, PI), halfO = Math::mod(cutoffOH + (PI-cutoffOH)/2, PI);\r
 454                 double yy, xx, phy, sec_factor;\r
 455 \r
 456 \r
 457                 if(cutoffL<=cutoffH) {\r
 458                         for(int y=1; y<=height; y++) {\r
 459                                 yy = y-originY;\r
 460                                 for(int x=1; x<=width; x++) {\r
 461                                         xx = x-originX;\r
 462                                         phy = Math::mod(atan2(yy, xx) - theta1, PI);\r
 463                                         if(phy<cutoffOH) sec_factor = 1.0;\r
 464                                         else if(phy<halfO) sec_factor = pow(1.0/(-cutoffOH+phy+1.0), 1.0/oslope);\r
 465                                         else sec_factor = pow(1.0/(PI-phy+1.0), 1.0/oslope);\r
 466                                         //if(isinf(sec_factor)) { sec_factor = 1.0; }\r
 467                                         //if(isnan(sec_factor)) { sec_factor = 0.0; }\r
 468                                         modulus = sqrt(yy*yy + xx*xx);\r
 469                                         if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope) * sec_factor;\r
 470                                         else if(modulus<cutoffH) filter(y, x) = 1.0 * sec_factor;\r
 471                                         else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope) * sec_factor;\r
 472                                         //if(filter(y,x)<0)filter(y,x)=0;\r
 473                                 }\r
 474                         }\r
 475                 } else {\r
 476                         for(int y=1; y<=height; y++) {\r
 477                                 yy = y-originY;\r
 478                                 for(int x=1; x<=width; x++) {\r
 479                                         xx = x-originX;\r
 480                                         phy = Math::mod(atan2(yy, xx) - theta1, PI);\r
 481                                         if(phy<cutoffOH) sec_factor = 1.0;\r
 482                                         else if(phy<halfO) sec_factor = pow(1.0/(-cutoffOH+phy+1.0), 1.0/oslope);\r
 483                                         else sec_factor = pow(1.0/(PI-phy+1.0), 1.0/oslope);\r
 484 \r
 485                                         modulus = sqrt(yy*yy + xx*xx);\r
 486                                         if(modulus<cutoffH) filter(y, x) = 1.0 * sec_factor;\r
 487                                         else if(modulus<half) filter(y, x) = (pow(1.0/(modulus-cutoffH+1.0), 1.0/sslope)) * sec_factor;\r
 488                                         else if(modulus<cutoffL) filter(y, x) = (pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope)) * sec_factor;\r
 489                                         else filter(y, x) = 1.0 * sec_factor;\r
 490                                         //if(filter(y,x)<0)filter(y,x)=0;\r
 491                                 }\r
 492                         }\r
 493                 }\r
 494 \r
 495                 filter(originY, originX) = 1.0;\r
 496                 return filter;\r
 497         }\r
 498 \r
 499         Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double cutoff_lambda3, double cutoff_lambda4, double theta1, double theta2, double slope)\r
 500         {\r
 501                 Matrix filter(height, width);\r
 502 \r
 503                 const double originY = (height)/2+1;\r
 504                 const double originX = (width )/2+1;\r
 505                 double modulus = 0.0, phyL = 0.0, phyH = 0.0;\r
 506                 double sslope = slope<=0.0 ? 0.0000000001 : slope;\r
 507                 double cutoffL, cutoffH, half;\r
 508                 Point p;\r
 509 \r
 510                 if(cutoff_lambda1<=cutoff_lambda2) {\r
 511                         for(int y=1; y<=height; y++) {\r
 512                                 for(int x=1; x<=width; x++) {\r
 513                                         phyL = atan2(y-originY, x-originX);\r
 514                                         phyH = Math::mod(phyL - theta2, 2*PI);\r
 515                                         phyL = Math::mod(phyL - theta1, 2*PI);\r
 516                                         modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));\r
 517                                         cutoffL = cutoff_lambda1*cutoff_lambda3 / p.set(cutoff_lambda3*cos(phyL), cutoff_lambda1*sin(phyL)).length();\r
 518                                         cutoffH = cutoff_lambda2*cutoff_lambda4 / p.set(cutoff_lambda4*cos(phyH), cutoff_lambda2*sin(phyH)).length();\r
 519                                         if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope);\r
 520                                         else if(modulus<cutoffH) filter(y, x) = 1.0;\r
 521                                         else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope);\r
 522                                         //if(filter(y,x)<0)filter(y,x)=0;\r
 523                                 }\r
 524                         }\r
 525                 } else {\r
 526                         for(int y=1; y<=height; y++) {\r
 527                                 for(int x=1; x<=width; x++) {\r
 528                                         phyL = atan2(y-originY, x-originX);\r
 529                                         phyH = Math::mod(phyL - theta2, 2*PI);\r
 530                                         phyL = Math::mod(phyL - theta1, 2*PI);\r
 531                                         modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));\r
 532                                         cutoffL = cutoff_lambda1*cutoff_lambda3 / p.set(cutoff_lambda3*cos(phyL), cutoff_lambda1*sin(phyL)).length();\r
 533                                         cutoffH = cutoff_lambda2*cutoff_lambda4 / p.set(cutoff_lambda4*cos(phyH), cutoff_lambda2*sin(phyH)).length();\r
 534                                         half = (cutoffL + cutoffH)/2;\r
 535                                         if(modulus<cutoffH) filter(y, x) = 1.0;\r
 536                                         else if(modulus<half) filter(y, x) = (pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope));\r
 537                                         else if(modulus<cutoffL) filter(y, x) = (pow(1.0/(+modulus-cutoffL+1.0), 1.0/sslope));\r
 538                                         else filter(y, x) = 1.0;\r
 539                                         //if(filter(y,x)<0)filter(y,x)=0;\r
 540                                 }\r
 541                         }\r
 542                 }\r
 543                 filter(originY, originX) = 1.0;\r
 544                 return filter;\r
 545         }\r
 546 \r
 547 \r
 548         void FFT2::copyBoxSpectrum(FFT2 &source, int s_left, int s_top, FFT2 &target, int t_left, int t_top, int width, int height)\r
 549         {\r
 550                 int t_width = target.width_, s_width = source.width_;\r
 551                 int sv, sh, tv, th;\r
 552                 double factor;\r
 553                 //std::cout << width_ << ":" << left << " " << height_ << ":" << top << "*" << bottom << std::endl;\r
 554                 for (int y=0; y<height; y++) {\r
 555                         sv = s_top + y;\r
 556                         tv = t_top + y;\r
 557                         if(sv<0 || sv>=source.height_) throw Exception("FFT2::copyBoxSpectrum: sv :");\r
 558                         if(tv<0 || tv>=target.height_) throw Exception("FFT2::copyBoxSpectrum: tv :");\r
 559                         for (int x=0; x<width; x++) {\r
 560                                 //std::cout << h << ":" << x << "  ";\r
 561                                 target.frq_spc[(t_left+x) + t_width*tv][REAL] = source.frq_spc[(s_left+x) + s_width*sv][REAL];\r
 562                                 target.frq_spc[(t_left+x) + t_width*tv][IMAGINARY] = source.frq_spc[(s_left+x) + s_width*sv][IMAGINARY];\r
 563                         }\r
 564                         //std::cout << std::endl;\r
 565                 }\r
 566         }\r
 567 \r
 568 \r
 569         void FFT2::resizeImage(const Image &source, Image &result, int new_width, int new_height)\r
 570         {\r
 571                 FFT2 src;\r
 572                 FFT2 rst;\r
 573                 Matrix source_buf[4];\r
 574                 Matrix result_buf[4];\r
 575 \r
 576                 //src.getSpectrum(buf_r, buf_i);\r
 577 \r
 578                 const int s_width = source.getWidth();\r
 579                 const int s_height = source.getHeight();\r
 580                 const int cp_width = Math::min(source.getWidth(), new_width);\r
 581                 const int cp_height = Math::min(source.getHeight(), new_height);\r
 582 \r
 583                 int s_right_b = 0, s_left_b, s_bottom_b = 0, s_top_b;\r
 584                 int t_right_b = 0, t_left_b, t_bottom_b = 0, t_top_b;\r
 585                 int right_length, left_length, top_length, bottom_length;\r
 586 \r
 587 \r
 588                 if(s_width>=new_width) {\r
 589                         right_length = cp_width - originFor(cp_width);\r
 590                         left_length = originFor(cp_width);\r
 591 \r
 592                 } else {\r
 593                         right_length = cp_width - originFor(cp_width);\r
 594                         left_length = originFor(cp_width);\r
 595                 }\r
 596                 s_left_b = s_width - left_length;\r
 597                 t_left_b = new_width - left_length;\r
 598 \r
 599                 if(s_width>=new_width) {\r
 600                         bottom_length = cp_width - originFor(cp_width);\r
 601                         top_length = originFor(cp_width);\r
 602 \r
 603                 } else {\r
 604                         bottom_length = cp_height - originFor(cp_height);\r
 605                         top_length = originFor(cp_height);\r
 606                 }\r
 607                 s_top_b = s_height - top_length;\r
 608                 t_top_b = new_height - top_length;\r
 609 \r
 610                 int itr;\r
 611                 switch (source.getComponentKind()) {\r
 612                         case Image::GRAY:\r
 613                                 source.to(source_buf[0]);\r
 614                                 itr = 1;\r
 615                                 break;\r
 616                         case Image::RGB:\r
 617                                 source.to(source_buf[0], source_buf[1], source_buf[2]);\r
 618                                 itr = 3;\r
 619                                 break;\r
 620                         case Image::RGBA:\r
 621                                 source.to(source_buf[0], source_buf[1], source_buf[2], source_buf[3]);\r
 622                                 itr = 4;\r
 623                                 break;\r
 624                         default:\r
 625                                 throw Exception("FFT2::resizeImage");\r
 626                 }\r
 627                 rst.set(new_width, new_height);\r
 628                 for(int i=0; i<itr; i++) {\r
 629                         src.set(source_buf[i]);\r
 630                         src.fft();\r
 631                         std::cout << src.width_ << " " << src.height_ << std::endl;\r
 632                         copyBoxSpectrum(src, s_right_b, s_top_b, rst, t_right_b, t_top_b, right_length, top_length);\r
 633                         copyBoxSpectrum(src, s_left_b, s_top_b, rst, t_left_b, t_top_b, left_length, top_length);\r
 634                         copyBoxSpectrum(src, s_right_b, s_bottom_b, rst, t_right_b, t_bottom_b, right_length, bottom_length);\r
 635                         copyBoxSpectrum(src, s_left_b, s_bottom_b, rst, t_left_b, t_bottom_b, left_length, bottom_length);\r
 636                         rst.ifft();\r
 637                         rst.getImage(result_buf[i]);\r
 638                         result_buf[i] = result_buf[i];\r
 639                 }\r
 640                 switch (source.getComponentKind()) {\r
 641                         case Image::GRAY:\r
 642                                 result.from(result_buf[0]);\r
 643                                 break;\r
 644                         case Image::RGB:\r
 645                                 result.from(result_buf[0], result_buf[1], result_buf[2]);\r
 646                                 break;\r
 647                         case Image::RGBA:\r
 648                                 result.from(result_buf[0], result_buf[1], result_buf[2], result_buf[3]);\r
 649                                 break;\r
 650                         default:\r
 651                                 throw Exception("FFT2::resizeImage");\r
 652                 }\r
 653         }\r
 654 \r
 655 \r
 656         int FFT2::originFor(int size)\r
 657         {\r
 658                 return (size)/2+1;\r
 659         }\r
 660 \r
 661 /*\r
 662         Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double slope)\r
 663         {\r
 664                 Matrix filter(height, width);\r
 665 \r
 666                 double originY = (height-1)/2+1;\r
 667                 double originX = (width -1)/2+1;\r
 668                 double modulus = 0.0;\r
 669                 double cutoffL = Math::log2(cutoff_lambda1);\r
 670                 double cutoffH = Math::log2(cutoff_lambda2);\r
 671 \r
 672                 if(cutoffL<cutoffH) {\r
 673                         for(int y=1; y<=height; y++) {\r
 674                                 for(int x=1; x<=width; x++) {\r
 675                                         modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));\r
 676                                         if(modulus!=0) modulus = Math::log2(modulus);\r
 677                                         if(modulus<cutoffL) filter(y, x) = 1.0-slope*(cutoffL-modulus);\r
 678                                         else if(modulus>cutoffH) filter(y, x) = 1.0-slope*(-cutoffH+modulus);\r
 679                                         else\r
 680                                                 filter(y, x) = 1.0;\r
 681                                         //filter(y, x) = sigmoid(-modulus, -frequency, 1);\r
 682                                         if(filter(y,x)<0)filter(y,x)=0;\r
 683                                 }\r
 684                         }\r
 685                 } else {\r
 686                         for(int y=1; y<=height; y++) {\r
 687                                 for(int x=1; x<=width; x++) {\r
 688                                         modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));\r
 689                                         if(modulus!=0) modulus = Math::log2(modulus);\r
 690                                         if(modulus<cutoffL) filter(y, x) = slope*(cutoffL-modulus);\r
 691                                         else if(modulus>cutoffH) filter(y, x) = slope*(-cutoffH+modulus);\r
 692                                         else\r
 693                                                 filter(y, x) = 0.0;\r
 694                                         //filter(y, x) = sigmoid(-modulus, -frequency, 1);\r
 695                                         if(filter(y,x)<0)filter(y,x)=0;\r
 696                                 }\r
 697                         }\r
 698                 }\r
 699                 filter(originY, originX) = 1.0;\r
 700                 return filter;\r
 701         }\r
 702         */\r
 703 \r
 704 \r
 705         void FFT2::makeNoise()\r
 706         {\r
 707                 double num = height_*width_;\r
 708                 for (int i=0; i<num; i++) {\r
 709                         frq_spc[i][REAL] = random(0.0,1.0);\r
 710                         frq_spc[i][IMAGINARY] = 0;\r
 711                 }\r
 712         }\r
 713 \r
 714 \r
 715         FFTW3D::FFTW3D()\r
 716         {\r
 717                 img_spc = 0;\r
 718                 frq_spc = 0;\r
 719         }\r
 720         FFTW3D::FFTW3D(int wid, int hei, int dep)\r
 721         {\r
 722                 set(wid, hei, dep);\r
 723         }\r
 724         FFTW3D::FFTW3D(const Image *source, int framsize)\r
 725         {\r
 726                 set(source, framsize);\r
 727         }\r
 728         FFTW3D::FFTW3D(const Matrix *source, int framsize)\r
 729         {\r
 730                 set(source, framsize);\r
 731         }\r
 732         FFTW3D::~FFTW3D()\r
 733         {\r
 734                 if(img_spc==0) free(img_spc);\r
 735                 if(frq_spc==0) free(frq_spc);\r
 736         }\r
 737         void FFTW3D::set(int wid, int hei, int dep)\r
 738         {\r
 739                 width_  = wid;\r
 740                 height_ = hei;\r
 741                 depth_  = dep;\r
 742                 image_size_ = width_*height_;\r
 743                 img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei * dep);\r
 744                 frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei * dep);\r
 745                 x_zero = wid + 1;\r
 746                 y_zero = hei + 1;\r
 747                 z_zero = dep + 1;\r
 748                 left = wid/2;\r
 749                 top = hei/2;\r
 750                 front = dep/2;\r
 751         }\r
 752         void FFTW3D::set(const Image *source, int framsize)\r
 753         {\r
 754                 set(source[0].getWidth(), source[0].getHeight(), framsize);\r
 755                 for(int z=0; z<depth_; z++) {\r
 756                         for(int y=0; y<height_; y++) {\r
 757                                 for(int x=0; x<width_; x++) {\r
 758                                         img_spc[x + width_*y + image_size_*z][REAL] = source[z].getPix(x, y).getR();\r
 759                                         img_spc[x + width_*y + image_size_*z][IMAGINARY] = 0.0;\r
 760                                 }\r
 761                         }\r
 762                 }\r
 763         }\r
 764         void FFTW3D::set(const Matrix *source, int framsize)\r
 765         {\r
 766                 set(source[0].getCols(), source[0].getRows(), framsize);\r
 767                 for(int z=0; z<depth_; z++) {\r
 768                         for(int y=0; y<height_; y++) {\r
 769                                 for(int x=0; x<width_; x++) {\r
 770                                         img_spc[x + width_*y + image_size_*z][REAL] = source[z](y+1, x+1);\r
 771                                         img_spc[x + width_*y + image_size_*z][IMAGINARY] = 0.0;\r
 772                                 }\r
 773                         }\r
 774                 }\r
 775         }\r
 776 \r
 777         int FFTW3D::freqX(int x) const\r
 778         {\r
 779                 return 0;//imgs[0].freqX(x);\r
 780         }\r
 781         int FFTW3D::freqY(int y) const\r
 782         {\r
 783                 return 0;//imgs[0].freqY(y);\r
 784         }\r
 785         int FFTW3D::freqT(int t) const\r
 786         {\r
 787                 return 0;//t<0 ? t_zero+t : t;\r
 788         }\r
 789         double FFTW3D::pix(int x, int y, int z, double l)\r
 790         {\r
 791                 return img_spc[x + width_*y + image_size_*z][REAL] = l;\r
 792         }\r
 793         double FFTW3D::pix(int x, int y, int z, const Color &c)\r
 794         {\r
 795                 return img_spc[x + width_*y + image_size_*z][REAL] = c.getR();\r
 796         }\r
 797         double FFTW3D::getPix(int x, int y, int z)\r
 798         {\r
 799                 return img_spc[x + width_*y + image_size_*z][REAL];\r
 800         }\r
 801 \r
 802         double FFTW3D::getDC()\r
 803         {\r
 804                 return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);\r
 805         }\r
 806         double FFTW3D::setDC(double l)\r
 807         {\r
 808                 frq_spc[0][REAL] = l;\r
 809                 frq_spc[0][IMAGINARY] = 0;\r
 810                 return l;\r
 811         }\r
 812 \r
 813 \r
 814         void FFTW3D::fft()\r
 815         {\r
 816                 fftw_plan plan = fftw_plan_dft_3d(depth_, height_, width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);\r
 817                 fftw_execute(plan);\r
 818                 normalizeFFT();\r
 819         }\r
 820         void FFTW3D::ifft()\r
 821         {\r
 822                 fftw_plan plan = fftw_plan_dft_3d(depth_, height_, width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);\r
 823                 fftw_execute(plan);\r
 824         }\r
 825         void FFTW3D::normalizeFFT()\r
 826         {\r
 827                 double num = depth_*height_*width_;\r
 828                 for (int i=0; i<num; i++) {\r
 829                         frq_spc[i][REAL] /= num;\r
 830                         frq_spc[i][IMAGINARY] /= num;\r
 831                 }\r
 832         }\r
 833 \r
 834 \r
 835         void FFTW3D::drawCloudImage(Drawable &target)\r
 836         {\r
 837                 double rl, im;\r
 838                 Color col;\r
 839                 Point p;\r
 840                 for (int z=0; z<depth_; z++) {\r
 841                         p.z = z;\r
 842                         for (int y=0; y<height_; y++) {\r
 843                                 p.y = y;\r
 844                                 for (int x=0; x<width_; x++) {\r
 845                                         p.x = x;\r
 846                                         rl = img_spc[x + width_*y + image_size_*z][REAL];\r
 847                                         //im = img_spc[x + width_*y + image_size_*z][IMAGINARY];\r
 848                                         col.set(rl);\r
 849 //                                      target.pix(p, col);\r
 850                                 }\r
 851                         }\r
 852                 }\r
 853         }\r
 854         void FFTW3D::drawCloudSpectrum(Point c, double gamma, Drawable &target)\r
 855         {\r
 856                 double rl, im;\r
 857                 Color col;\r
 858                 int h, v, d;\r
 859                 Point p;\r
 860                 for (int z=0; z<depth_; z++) {\r
 861                         d = z<front ? z+front : z-front;\r
 862                         p.z = d;\r
 863                         for (int y=0; y<height_; y++) {\r
 864                                 v = y<top ? y+top : y-top;\r
 865                                 p.y = v;\r
 866                                 for (int x=0; x<left; x++) {\r
 867                                         h = x+left;\r
 868                                         p.z = h;\r
 869                                         rl = frq_spc[x + width_*y + image_size_*z][REAL];\r
 870                                         im = frq_spc[x + width_*y + image_size_*z][IMAGINARY];\r
 871                                         col.set(pow( sqrt(rl*rl+im*im), gamma ));\r
 872 //                                      target.pix(p, col);\r
 873                                 }\r
 874                                 for (int x=left; x<width_; x++) {\r
 875                                         h = x-left;\r
 876                                         p.z = h;\r
 877                                         rl = frq_spc[x + width_*y + image_size_*z][REAL];\r
 878                                         im = frq_spc[x + width_*y + image_size_*z][IMAGINARY];\r
 879                                         col.set(pow( sqrt(rl*rl+im*im), gamma ));\r
 880 //                                      target.pix(p, col);\r
 881                                 }\r
 882                         }\r
 883                 }\r
 884         }\r
 885 \r
 886 }       /*      <- namespace Psycholops         */\r