OwnMath.h

Go to the documentation of this file.

// this is a -*- C++ -*- file

// -- BEGIN LICENSE BLOCK ----------------------------------------------
//
// You received this file as part of MCA2
// Modular Controller Architecture Version 2
//
// Copyright (C) FZI Forschungszentrum Informatik Karlsruhe
//
// This program is free software; you mild_base_driving redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
//----------------------------------------------------------------------

#ifndef _OwnMath_h_
#define _OwnMath_h_
//----------------------------------------------------------------------
// Includes
//----------------------------------------------------------------------
#include "KernelMath.h"

#ifdef _SYSTEM_WIN32_
# define M_E  2.7182818284590452354 /* e */
# define M_LOG2E 1.4426950408889634074 /* log_2 e */
# define M_LOG10E 0.43429448190325182765 /* log_10 e */
# define M_LN2  0.69314718055994530942 /* log_e 2 */
# define M_LN10  2.30258509299404568402 /* log_e 10 */
# define M_PI  3.14159265358979323846 /* pi */
# define M_PI_2  1.57079632679489661923 /* pi/2 */
# define M_PI_4  0.78539816339744830962 /* pi/4 */
# define M_1_PI  0.31830988618379067154 /* 1/pi */
# define M_2_PI  0.63661977236758134308 /* 2/pi */
# define M_2_SQRTPI 1.12837916709551257390 /* 2/sqrt(pi) */
# define M_SQRT2 1.41421356237309504880 /* sqrt(2) */
# define M_SQRT1_2 0.70710678118654752440 /* 1/sqrt(2) */
#endif

#include <stdlib.h>
#include <math.h>

double FastSin(double x);
double FastCos(double x);

//--------------------------------------------------------------------------------------------------
//                                Mathematical Function
//--------------------------------------------------------------------------------------------------



inline double Mod(double a, double b)
{
    double c = fmod(a, b);
    return c >= 0 ? c : c + b;
}

inline double Mod(double a, int b)
{
    return Mod(a, double(b));
}

inline int Mod(int a, int b)
{
    int c = a % b;
    return c >= 0 ? c : c + b;
}

inline long int Mod(long int a, long int b)
{
    int c = a % b;
    return c >= 0 ? c : c + b;
}

inline int Sgn(double a)
{
    return a >= 0 ? 1 : -1;
}

inline int Sgn(long double a)
{
    return a >= 0 ? 1 : -1;
}

inline int Sgn0(double a)
{
    return a > 0 ? 1 : a < 0 ? -1 : 0;
}

inline int Sgn0(long double a)
{
    return a > 0 ? 1 : a < 0 ? -1 : 0;
}

inline int Sgn(long a)
{
    return a >= 0 ? 1 : -1;
}

inline int Sgn0(long a)
{
    return a > 0 ? 1 : a < 0 ? -1 : 0;
}

inline double Rad2Deg(double a)
{
    static double factor = 180 / M_PI;
    return a*factor;
}

inline double Deg2Rad(double a)
{
    static double factor = M_PI / 180;
    return a*factor;
}

inline double NormalizeAngleUnsigned(const double angle)
{
    return Mod(angle, 2*M_PI);
};

inline double NormalizeAngleSigned(const double angle)
{
    return Mod(angle + M_PI, 2*M_PI) - M_PI;
};


//@Zacharias: inherente Annahme angle>-180
inline double NormalizeAngleInDegreeSigned(const double angle)
{
    return Mod(angle + 180, 360) - 180;
};

inline int Pow(int a, int b)
{
    int ret;
    for (ret = 1; b > 0; --b)
        ret *= a;
    return ret;
}
inline double Round(double value, int precision = 0)
{
    double fact = pow(1e1, precision);
    return floor(((value*fact) + 0.5)) / fact;
}

inline double Ceil(double value, int precision = 0)
{
    double fact = pow(1e1, precision);
    return ceil(value*fact) / fact;
}

inline double Floor(double value, int precision = 0)
{
    double fact = pow(1e1, precision);
    return floor(value*fact) / fact;
}

inline int* Add(int *a, int*b, int size)
{
    int i;
    for (i = 0; i < size; i++)
        a[i] += b[i];
    return a;
}

inline int* Sum(int *a, int*b, int*c, int size)
{
    int i;
    for (i = 0; i < size; i++)
        c[i] = a[i] + b[i];
    return c;
}

inline int* Div(int* a, int b, int size)
{
    int i;
    for (i = 0; i < size; i++)
        a[i] /= b;
    return a;
}

inline int* Div(int* a, int b, int*c, int size)
{
    int i;
    for (i = 0; i < size; i++)
        c[i] = a[i] / b;
    return c;
}

template<typename T>
inline T Sqr(T a)
{
    return a*a;
}

inline double Sqrt(double a)
{
    return sqrt(a);
}

inline double RandomUniformAbs()
{
    return float(rand()) / RAND_MAX;
}

inline double RandomUniform()
{
    return 2*float(rand()) / RAND_MAX - 1;
}

inline double RandomNormal(double variance)
{
    double r1 = (float((rand()) + 1) / (float(RAND_MAX) + 1)); // intervall ]0,1] wegen log(0)
    double r2 = float(rand()) / RAND_MAX;    // intervall [0,1]
#ifdef MATH_USE_TABLE_LOOKUP
    return variance*sqrt(-2*log(r1))*FastSin(2*M_PI*r2);
#else
    return variance*sqrt(-2*log(r1))*sin(2*M_PI*r2);
#endif
    // Übrigens ergibt (man beachte den cosinus anstelle des sinus)
    // eine weitere Zufallsvariable, die von der ersten unabhängig ist:
    // sqrt(-2*variance*log(r1))*cos(2*M_PI*r2);
}



inline int ipow(int base, int exponent)
{
    if (base <= 0)
        return 0;
    if (exponent < 0)
        return 0;
    else
    {
        int ret = 1;
        for (int i = 1; i <= exponent; i++)
            ret *= base;
        return ret;
    }
}


inline int log2upw(int value)
{
    int res = 1;
    while (value > 2)
    {
        value = value / 2;
        res++;
    }
    return res;
}


inline int log8upw(int value)
{
    int res = 1;
    while (value > 8)
    {
        value = value / 8;
        res++;
    }
    return res;
}

inline void SinCos(const double x, double &sx, double &cx)
{
#if defined _SYSTEM_LINUX_ && ! defined _SYSTEM_DARWIN_
    sincos(x, &sx, &cx);
#else
    sx = sin(x);
    cx = cos(x);
#endif
}

class tSinCosLookupTable
{
public:
    tSinCosLookupTable()
    {
        int i = 0;
        double x = 0;
        double increment = 2. * M_PI / double(cTABLE_SIZE);
        for (; i < cTABLE_SIZE; ++i, x += increment)
        {
            m_sin_table[i] = ::sin(x);
            m_cos_table[i] = ::cos(x);
        }
    }

    double Sin(int index)
    {
        return m_sin_table[index & cTABLE_INDEX_MASK];
    }

    double Cos(int index)
    {
        return m_cos_table[index & cTABLE_INDEX_MASK];
    }

    static const int cTABLE_SIZE = 16384;
    static const int cTABLE_INDEX_MASK;
    static const double cSCALE_FACTOR;

private:
    double m_sin_table[cTABLE_SIZE];
    double m_cos_table[cTABLE_SIZE];
};

extern tSinCosLookupTable sin_cos_lookup_table;

inline double FastSin(double x)
{
    double scaled_x = sin_cos_lookup_table.cSCALE_FACTOR * x;
    double table_index = ::floor(scaled_x);
    double y1 = sin_cos_lookup_table.Sin(int(table_index));
    double y2 = sin_cos_lookup_table.Sin(int(table_index) + 1);
    return y1 + (y2 - y1) * (scaled_x - table_index);
}

inline double FastCos(double x)
{
    double scaled_x = sin_cos_lookup_table.cSCALE_FACTOR * x;
    double table_index = ::floor(scaled_x);
    double y1 = sin_cos_lookup_table.Cos(int(table_index));
    double y2 = sin_cos_lookup_table.Cos(int(table_index) + 1);
    return y1 + (y2 - y1) * (scaled_x - table_index);
}

inline bool FloatIsEqual(const double a, const double b, const double epsilon = 1e-10)
{
    return fabs(a -b) < epsilon;
}


#endif