stdalgorithm.cpp

Go to the documentation of this file.

#include "stdalgorithm.hpp"
#include "kube-gustavson-fft.hpp"
#include <fstream>
#include <iostream>
#include <cmath>

using namespace std;

const int Parabelzentrum::order=5;
// Anzahl Koeffizienten = 5

Parabelzentrum::Parabelzentrum(const bildfloat& bild_, int pktmax_) : 
 bild(bild_), pktmax(pktmax_), xmax(1), ymax(1), xmin(1), ymin(1) {
  reserve_2D(R, order, pktmax);
  f=new double[pktmax];
  koeff=new double[order];
  errors=new double[order];
  rel_errors=new double[order];
  pktzahl=0;
  x_estimate=0.0;
  y_estimate=0.0;
}  

Parabelzentrum::~Parabelzentrum() {
  delete_2D(R);
  delete[] f;
  delete[] koeff;
  delete[] errors;
  delete[] rel_errors;
}  

void Parabelzentrum::estimate(double x, double y) {
  if (pktzahl>0) STHROW("Error: Can set estimate only if there are no points");
  x_estimate=x;
  y_estimate=y;
}  

void Parabelzentrum::operator () (int x, int y) {
  if (pktzahl==pktmax) STHROW("Mist -- zuwenig Platz für die Punkte");
  
  // rechte Seite
  f[pktzahl]=bild(y,x);
  
  
  // Schätzwerte für x und y abziehen
  // für alle weiteren Werte
  double xn=x-x_estimate;
  double yn=y-y_estimate;
  R[pktzahl][0]=1.0;
  R[pktzahl][1]=xn;
  R[pktzahl][2]=yn;
  R[pktzahl][3]=xn*xn;
  R[pktzahl][4]=yn*yn;
  pktzahl++;
  xmax.put(xn);
  xmin.put(xn);
  ymax.put(yn);
  ymin.put(yn);
}

void Parabelzentrum::clear() {
  pktzahl=0;
  xmax.clear();
  xmin.clear();
  ymax.clear();
  ymin.clear();
  x_cent=0.0;
  y_cent=0.0;
  x_estimate=0.0;
  y_estimate=0.0;

}  

bool  Parabelzentrum::fit() {
  x_cent=0; y_cent=0;
  for (int i=0; i<pktzahl; i++) {
    x_cent+=R[i][1];
    y_cent+=R[i][2];
  }
  x_cent/=pktzahl;
  y_cent/=pktzahl;
  /* Berechne den Schwerpunkt der Punkte als Notlösung,
     falls der Fit versagt */
  // Fehler dazu als Fehler des Mittelwerts
  s_xcent=0; s_ycent=0;
  for (int i=0; i<pktzahl; i++) {
    s_xcent+=SQR(R[i][1]-x_cent);
    s_ycent+=SQR(R[i][2]-y_cent);
  }
  s_xcent/=pktzahl;
  s_ycent/=pktzahl;

  if (pktzahl==0) {
    std::cerr<<"Fit ohne Punkte !"<<std::endl;
    return false; //nix machen
  }  

  if (pktzahl < 2*order) {
    std::cerr<<"Sehr wenig Punkte !"<<std::endl;
    return true; /* Schwerpunkt zählt */
  }  
  // Gleichungssystem unterbestimmt bzw. schwach definiert 
  
  double *d=new double[order];
  // Löse das Fitproblem mit QR
  // || R x - f || minimal für x

  if(QR(pktzahl, order, R, d, f, 1e-10))  {
      std::cerr<<"Matrix singulär ????"<<std::endl;
     // Fitmatrix war singulär -- darf eigentlich nicht sein
     // Nimm Schwerpunkt
     return false;
  }
  
  // Jetzt löse nach den Koeffizienten auf
  rdsolve(order, R, d, f, koeff);
  double x=-koeff[1]/(2*koeff[3]);
  double y=-koeff[2]/(2*koeff[4]);

  // Berechne die statistischen Fehler der Koeffizenten
  compute_errors(order, pktzahl, R, d, f, errors);
  for (int o=0; o<order; o++)
    rel_errors[o]=errors[o]/koeff[o];
    
  /* Parabel falschherum gekrümmt -- das ist kein Peak ! 

  if ((koeff[3]<=0) || (koeff[4]<=0)) {
    std::cerr<<"Parabel falschherum"<<std::endl;
    return false;
  }  */
  
  /* Wenn der Parabelmittelpunkt außerhalb der Punkte liegt, nimm Schwerpunkt */
  if ((x>xmax()) || (x<xmin()) || (y>ymax()) || (y<ymin()))  {
    std::cerr<<"Danebengeschossen !"<<std::endl;
    return true;
  }  

  /* Endgültig Fit-Werte akzeptieren */
  x_cent=x;
  y_cent=y;

  
  // Fehler berechnen
  s_xcent=x_cent*sqrt(SQR(rel_errors[1]) + SQR (rel_errors[3]));
  s_ycent=y_cent*sqrt(SQR(rel_errors[2]) + SQR (rel_errors[4]));

  return true;
  delete[] d;
}  

bildcomplex::bildcomplex(const bildfloat &realvalue) :
 bildcomplex_base() 
{ MAP(realvalue) {
    (*this)(y,x).re=realvalue(y,x);
    (*this)(y,x).im=0.0;
  }  
}    

bildcomplex::bildcomplex(const bildfloat& real, const bildfloat& imag) :
 bildcomplex_base() 
{ MAP(real) {
    (*this)(y,x).re=real(y,x);
    (*this)(y,x).im=imag(y,x);
  }  
}    

void bildcomplex::decompose(bildfloat&real, bildfloat& imag) 
{
  MAP((*this)) {
    real(y,x)=(*this)(y,x).re;
    imag(y,x)=(*this)(y,x).im;
  }

}


bildcomplex& bildcomplex::FFT_forward() {
  forward_fft2f(bild[0], SIZEY, SIZEX);
  return *this;
}


bildcomplex& bildcomplex::FFT_backward() {
  inverse_fft2f(bild[0], SIZEY, SIZEX);
  return *this;
}

bildfloat& bildcomplex::norm(bildfloat &ret) {
  MAP (ret) {
    ret(y,x)=SQR((*this)(y,x).re)+SQR((*this)(y,x).im);
  }
  return ret;
}

bildcomplex& bildcomplex::Hamming_window(float width, float xm, float ym) {
  
  MAP((*this)) {        
   float xs=x-xm;
   float ys=y-ym;
   if (SQR(xs)+SQR(ys) < SQR(width))
     (*this)(y,x)*=SQR(0.54+0.46*cos(M_PI*sqrt(SQR(xs)+SQR(ys))/width))*2.36961606240022;
   else 
     (*this)(y,x)=0.0; 
  } 
  return *this;
}

bildcomplex& bildcomplex::Hamming_unwindow(float width, float xm, float ym) {
  
  MAP((*this)) {        
   float xs=x-xm;
   float ys=y-ym;
   if (SQR(xs)+SQR(ys) < SQR(width))
     (*this)(y,x)/=SQR(0.54+0.46*cos(M_PI*sqrt(SQR(xs)+SQR(ys))/width))*2.36961606240022;
   else 
     (*this)(y,x)=0.0; 
  } 
  return *this;
}

/*
 * Copyright notice to pnpoly, interior point testing of a polygon
 *
 *   Copyright (c) 1970-2003, Wm. Randolph Franklin
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is furnished to do
 * so, subject to the following conditions:
 * 
 *    1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimers.  2. Redistributions
 *    in binary form must reproduce the above copyright notice in the
 *    documentation and/or other materials provided with the distribution.  3. The
 *    name of W. Randolph Franklin may not be used to endorse or promote products
 *    derived from this Software without specific prior written permission. 
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE. 
 */

bool pnpoly(int npol, float *xp, float *yp, float x, float y)
    {
      int i, j;
      bool c = false;
      for (i = 0, j = npol-1; i < npol; j = i++) {
        if ((((yp[i]<=y) && (y<yp[j])) ||
             ((yp[j]<=y) && (y<yp[i]))) &&
            (x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i]))

          c = !c;
      }
      return c;
    }



Voronoischnitt::Voronoischnitt(bildfloat &bfl, float xmitte_, float ymitte_, const char *vorofile) :
  rb(bfl), xmitte(xmitte_), ymitte(ymitte_), r(0,0) 
{
  ifstream vf(vorofile);
  vector<Point> centerpoints;
  vector<cell> zellen;
  if (!vf) STHROW("Voronoidata "<<vorofile<<" not found"); 
          
  while (vf) {
    // lese Voronoi-Daten ein
    float x; float y; int anzahl;
    vf >>x; vf >>y; vf>>anzahl;
    
    int cellindex=centerpoints.size();
    centerpoints.push_back(make_pair(x,y));
    
    // Zentrum der Voronoi-Zelle
    zellen.resize(cellindex+1);
    for (int i=0; i<anzahl; i++) {
      vf >> x; vf >>y; 
      zellen[cellindex].push_back(make_pair(x,y));
    }
  }  
  vf.close();
  
  if (zellen.size()<2) STHROW("Could not read Voronoi data "<<vorofile);
  // suche die nächste Zelle
  int ind=0;
  float mindist=SQR(centerpoints[0].first-xmitte)+SQR(centerpoints[0].second-ymitte);
  for (size_t i=1; i<centerpoints.size(); i++) {
    float dist=SQR(centerpoints[i].first-xmitte)+SQR(centerpoints[i].second-ymitte);
    if (dist<mindist) {
      mindist=dist;
      ind=i;
    }
  }
  // Punkt nr. ind ist am nächsten
  cerr<<ind<<endl;
  zelle=zellen[ind];
  // cerr<<"Ende\n"<<flush;
  stddev=0;
  // Speichere Polygonecken in C-Array für pnpoly
  npol=zelle.size()+1;
  xp=new float[npol];
  yp=new float[npol];
  for (int i=0; i<npol-1; i++) {
    xp[i]=zelle[i].first;
    yp[i]=zelle[i].second;
  }
  xp[npol-1]=xp[0];
  yp[npol-1]=yp[0];
  // schließen
  
}

Voronoischnitt::~Voronoischnitt() {
  delete [] xp;
  delete [] yp;
}

void Voronoischnitt::compute_schnitt(int nr) {
  // Suche die längste Diagonale
  int anzahl=zelle.size();
  if (anzahl!=6) cerr<<"Warnung: Zelle hat nicht 6 Ecken, sondern "<<anzahl<<endl;
  int ind=0;
  maxdist=mindist=SQR(zelle[0].first-zelle[3].first)+SQR(zelle[0].second-zelle[3].second);
  for (int shift=1; shift < anzahl; shift++) {
    float aktdist=SQR(zelle[shift%anzahl].first-zelle[(shift+3)%anzahl].first)+
                  SQR(zelle[shift%anzahl].second-zelle[(shift+3)%anzahl].second);
    if (aktdist>maxdist) {
       maxdist=aktdist;
       ind=shift;
    }
    if (aktdist<mindist) mindist=aktdist;
  }
  
  mindist=sqrt(mindist);
  maxdist=sqrt(maxdist);
  
  cerr<<".c create line "<<zelle[ind%anzahl].first<<" "<<zelle[ind%anzahl].second<<" "<<
         zelle[(ind+3)%anzahl].first<<" "<<zelle[(ind+3)%anzahl].second<<endl;

  // Berechne "Ausgleichsgerade", die durch das Zentrum geht und den Winkel gleichmäßig verteilt
  vec2d M(xmitte, ymitte);
  vec2d P1(zelle[ind%anzahl].first, zelle[ind%anzahl].second);
  vec2d P2(zelle[(ind+3)%anzahl].first, zelle[(ind+3)%anzahl].second);

  vec2d v1=P1-M;
  vec2d v2=P2-M;
  
  r=v1/abs(v1)-v2/abs(v2);
  
  int stepstart, stepend;
  
  // r ist Richtungsvektor der gesuchten Geraden
  cerr<<"Richtungsvektor "<<r.x<<", "<<r.y<<endl;
  if (abs(r.x) >abs(r.y)) {
    r/=r.x;
    // stepstart=(r*1.0/abs(r))*((P1-M)*r).x;
    if (P1.x < P2.x) {
      stepstart=int(-abs(v1.x)); stepend=int(abs(v2.x));
    } else {
      stepstart=int(-abs(v2.x)); stepend=int(abs(v1.x));
    }  
  } else {
    r/=r.y;
    if (P1.y < P2.y) {
      stepstart=int(-abs(v1.y)); stepend=int(abs(v2.y));
    } else {
      stepstart=int(-abs(v2.y)); stepend=int(abs(v1.y));
    } 
  }

  // Normierung, so dass immer ein Schritt entlang x oder y gemacht wird 

  for (int step=stepstart; step<=stepend; step++) {
    int xpunkt= int(floor(r.x*step+M.x+0.5));
    int ypunkt= int(floor(r.y*step+M.y+0.5));
    // Genaue Stachelmitte ist mit Subpixelgenauigkeit in xmitte, ymitte
    // Berechne Punkte als Abstand dazu, Richtung aus Projektion auf r
    vec2d distvec(xpunkt-xmitte, ypunkt-ymitte);
    double dist=abs(distvec);
    if (r*distvec < 0) dist*=-1;
    
    cout<<nr<<"\t"<<dist<<"\t"<<rb(ypunkt, xpunkt)<<endl;
  } 
  cout<<endl;
         
}  

vec2d Voronoischnitt::direction() {
  return r;
}  


float Voronoischnitt::boundary_average() 
{
  int anzahl=zelle.size();
  if (anzahl!=6) cerr<<"Warnung: Zelle hat nicht 6 Ecken, sondern "<<anzahl<<endl;
  
  mean_avg avg(rb);
  for (int i=0; i<anzahl; i++) {
    /*vec2d P1(zelle[i].first, zelle[i].second);
    vec2d P2(zelle[(i+1)%anzahl].first, zelle[(i+1)%anzahl].second);
    line_iterate(P1, P2, avg); */
    avg(int(floor(zelle[i].first+0.5)), int(floor(zelle[i].second+0.5)));
  }

  double height=avg.sum/avg.pktzahl;
  stddev=sqrt((avg.sqrsum/avg.pktzahl) - SQR(height));
  cerr<<"Mittlere Höhe am Rand: "<<height<<" +/- "<<stddev<<endl;
  return avg.sum/avg.pktzahl;
}  


bool Voronoischnitt::contains(float x, float y) {
  return pnpoly(npol, xp, yp, x, y);
}  

---