MathExt.cpp

/*Projektionsformeln und Entfernungen auf Kugelkoordinaten*/

#include "MathExt.h"

float distance(double lat1, double lon1, double lat2, double lon2){
    if (lat1 == lat2 && lon1 == lon2) return 0;
    else{
        double aacos = cos(lat2/180*M_PI)*cos(lat1/180*M_PI)*
                      (cos((lon2-lon1)/180*M_PI))+sin(lat2/180*M_PI)
                      *sin(lat1/180*M_PI);
        if(aacos < 1)
            return (float) acos(aacos)*6371;
        else
            return 0.0;
    }
}
double** SymMatrixtoTriang(double** m, int dim, short returnlower){
    int i,j,k;
    double** tl = CreateMatrix(dim,dim);
    
    /*Bestimme untere Dreiecksmatrix*/
    float sum;
    for(j = 0; j < dim; j++)
        for(i = 0; i <=j; i++){
            sum = 0;
            for(k = 0;k < i; k++) sum = sum + float(tl[k][j]*tl[k][i]);
            if(i == j){
                if(m[i][j]-sum > 0)
                    tl[i][j] = sqrt(m[i][j]-sum);
                else{
                    /*Sonst Error, weil Matrix nicht mit reellen Zahlen in 
                    obere und untere Dreiecksmatrx aufspaltbar*/
                    DeleteMatrix(tl,dim);
                    return NULL;
                }
            }
            else
                tl[i][j] = (m[i][j]-sum)/tl[i][i];
        }
    if (returnlower)
        return tl;
    else{
        TranspondMatrix(tl,dim);
        return tl;
    }
}
double** CreateMatrix(int dimx, int dimy){
    int i,j;
    /*Anlegen*/
    double** t = new double*[dimx];
    for(i = 0; i < dimx; i++)
        t[i] = new double[dimy];
        
    /*mit Null inititalisieren*/
    for(i = 0; i < dimx; i++)
        for(j = 0; j < dimy; j++)
            t[i][j] = 0;

    return t;
}
double** CopyMatrix(double** m, int dimx,int dimy){
    double** t = CreateMatrix(dimx,dimy);
    for(int i = 0; i < dimx; i++)
        for(int j = 0; j < dimy; j++)
            t[i][j] = m[i][j];
    return t;
}
void DeleteMatrix(double** m, int dimx){
    for(int i= 0; i < dimx; i++)
        delete(m[i]);
    delete(m);
}
void TranspondMatrix(double** m, int dim){
    double buf;
    int i,j;
    for(i = 0; i < dim; i++)
        for(j = i+1; j < dim; j++){
            buf = m[i][j];
            m[i][j] = m[j][i];
            m[j][i] = buf;
        }
}
double** MatrixMatrix(double** m1, int dim1x, int dim1y,
                      double** m2, int dim2x, int dim2y){
    int i,j,k;
    double** r = CreateMatrix(dim2x,dim1y);

    for(int i = 0; i < dim2x; i++)
        for(int j = 0; j < dim1y; i++)
            r[i][j] = 0;
    
    for(i = 0; i < dim2x; i++)
        for(j = 0; j < dim1y; i++)
            for(k = 0; k < dim1x; k++)
                r[i][j] = r[i][j] + m1[k][j]*m2[i][k];
    
    return r;
}
double* SymLGS(double** A, double* y, int dim){
    /*Ein LGS ist dann durch das Cholesky-Verfahren loesbar, wenn A positiv
    definit ist, ansonsten gelingt die Aufspaltung in Dreiecksmatrizen nicht,
    da komplexe Zahlen auftreten. Die allermeisten natürlichen Probleme
    formulieren aber apriori LGS, die eine positiv definite A haben*/
    int i;
    double sum;
    short error = 0;

    double* b = new double[dim];
    double* x = new double[dim];

    /*untere Dreiecksmatrix erstellen*/
    double** tl = SymMatrixtoTriang(A,dim,1);
    if(tl == NULL) error++;

    if(!error)
        /*Falls Hauptdiagonale Nullen hat, ist das LGS nicht loesbar*/
        for(int j = 0; j < dim; j++)
            if(tl[j][j] == 0) error++;

    if(!error){
        /*Zwischenlösung b erstellen aus unterer Dreiecksmatrix*/
        for(int j = 0; j < dim; j++){
            sum = 0;
            for(i = j-1; i >= 0; i--) sum = sum + b[i]*tl[i][j];
            b[j] = (y[j]-sum)/tl[j][j];
        }
        
        /*obere Dreiecksmatrix erstellen*/
        TranspondMatrix(tl,dim);
        /*Endlösung erstellen*/
        for(int j = dim-1; j >= 0; j--){
            sum = 0;
            for(i = j+1; i < dim; i++) sum = sum + x[i]*tl[i][j];
            x[j] = (b[j]-sum)/tl[j][j];
        }
    }


    delete b;
    if(error != 1)
        DeleteMatrix(tl,dim);
    
    /*Wenn das System nicht mit Lösbar war, dann NULL ausgeben*/
    if(error){
        delete(x);
        return NULL;
    }
    else
        return x;
    
}
double* FitPolynom(QPointF* p, int pointcount, int grad){
    int i,j,xi,yi;
    double* y = new double[grad+1];
    for(i = 0; i <= grad; i++) y[i] = 0;
    double** A = CreateMatrix(grad+1,grad+1);
    double xn;

    for(i = 0; i < pointcount; i++){
        /*Koeffizientmatrix befüllen, fuer jeden Punkt in p ...*/
        xn = 1;
        for(j = 0; j <= grad*2; j++){
            xi =    qMax(0,grad-j);
            yi = 2*grad-j-xi;
            while(xi <= grad && yi >= 0){
                A[xi][yi] = A[xi][yi] + xn;
                xi++;
                yi--;
            }
            /*y befüllen*/
            if(j <= grad) y[grad-j] = y[grad-j] + xn * p[i].y();
            /*nächste Potenz vorbereiten*/
            xn = xn*p[i].x();
        }
    }
    /*LGS lösen lassen*/
    double* x = SymLGS(A,y,grad+1);
    DeleteMatrix(A,grad+1);
    delete(y);
    return x;
}
double fpln(double x, double* p, int grad){
    double result = 0;
    for(int k = 0; k <= grad; k++)
        result = result + p[k]*pow(x,grad-k);

    return result;
}

double upperscale(float f){
    int i1 = 0;
    if(f == 1) return 1;
    if(f < 1){
        while(pow(double(10), i1) > f) i1--;
    }
    else{
        while(pow(double(10), i1) < f) i1++;
        i1--;
    }
    double v = f/pow(double(10), i1);
    if(v < 2)   return pow(double(10), i1)*2;
    if(v < 2.5) return pow(double(10), i1)*2.5;
    if(v < 5)   return pow(double(10), i1)*5;
    else        return pow(double(10), i1+1);
}
double fermi(double x, double max, double T, double x0){
    if(T == 0)
        T = 1e-5;
    return max/(1+exp((x-x0)/T));
}
double lattomercator(double lat){
    double sinlat = sin(lat/180.*M_PI);
    if(lat > 0)
        return 1./2.*log((1+sinlat)/(1-sinlat))*180./M_PI;
    else
        return 1./2.*log((1-sinlat)/(1+sinlat))*180./M_PI;
}
double mercatortolat(double mlat){
    double explat = exp(2*mlat/180.*M_PI);
    return asin((explat-1)/(explat+1))*180./M_PI;
}