MathSup.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 

#ifndef MATHSUP_INCLUDEDEFX_H
#define MATHSUP_INCLUDEDEFX_H

#include <math.h>


//-----------------------------------------------
class MathSup
{
public:
        // -------- Constants
        static const double PI;

        static const double TWO_PI;

        static const double HALF_PI;

        // -------- Angle conversion
        static double convRadToDeg(const double& dAngRad)
        {
                return (dAngRad * 180.0 / PI);
        }

        static double convDegToRad(const double& dAngDeg)
        {
                return ( dAngDeg * PI / 180.0 );
        }

        static void normalize2Pi(double& angle)
        {
                angle-=floor(angle/(2 * PI))* 2 * PI;
        }

        static void normalizePi(double& angle)
        {
                normalize2Pi(angle);
                if ( angle> PI ) angle-=2 * PI;
        }

        static void normalizePiHalf(double& angle)
        {
                normalize2Pi(angle);
                if (angle > PI) angle-=2*PI;
                if (angle > PI/2.0) angle -= PI;
                if (angle <= -PI/2.0) angle += PI;
        }

        static double sign(const double& x)
        {
                if (x<0)
                        return -1.0;
                else
                        return 1.0;
        }

        static double getMin(const double& a, const double& b)
        {
                if (a < b)
                        return a;
                else
                        return b;
        }

        static double getMax(const double& a, const double& b)
        {
                if (a > b)
                        return a;
                else
                        return b;
        }

        static double calcDeltaAng(const double& a, const double& b)
        {
                double c = a-b;
                normalizePi(c);
                return c;
        }


        static double atan4quad(double y, double x)
        {
                double result;

                if((x==0.0) && (y==0.0))
                        result = 0.0;
                else if((x==0.0) && (y > 0.0))
                        result = HALF_PI;
                else if((x==0.0) && (y < 0.0))
                        result = -HALF_PI;
                else if((y==0.0) && (x > 0.0))
                        result = 0.0;
                else if((y==0.0) && (x < 0.0))
                        result = PI;
                else
                {
                        result = atan(y/x);
                        if(x<0.0)
                                {
                                        if(y>0.0)               // Quadrant 2 -> correct
                                                result += PI;
                                        else                    // Quadrant 3 -> correct
                                                result -= PI;
                                }
                }
                normalizePi(result);
                return result;
        }


        static double distance(double x1, double y1, double x2, double y2)
        {
                return sqrt( distanceSq( x1, y1, x2, y2 ) );
        }

        static double distanceSq(double x1, double y1, double x2, double y2)
        {
                double dx = x2 - x1;
                double dy = y2 - y1;

                return dx*dx + dy*dy;
        }

        static bool isBitSet(int iVal, int iNrBit)
        {
                if( (iVal & (1 << iNrBit)) == 0)
                        return false;
                else
                        return true;
        }

        static double convFloatToInt4Byte(double dVal)
        {
                //Todo
                return 1.0;
        }

        static double convInt4ByteToFloat(int iVal)
        {
                unsigned char c[4];
                double dSign;
                double dFraction;
                double dExp;
                double dVal;

                c[0] = iVal >> 24;
                c[1] = iVal >> 16;
                c[2] = iVal >> 8;
                c[3] = iVal;

                if( (c[0] & 0x80) == 0)
                        dSign = 1;
                else
                        dSign = -1;

                dFraction = (iVal & 0xFFFFFF) | 0x800000;
                dFraction = dFraction * pow(2., -23);

                dExp = ((c[0] << 1) | (c[1] >> 7)) - 127;

                dVal = dSign * dFraction * pow(10., dExp);

                return dVal;
        }

        static int limit(double* pdToLimit, double dLimit)
        {
                int iRet = 0;

                if(*pdToLimit < -dLimit)
                {
                        *pdToLimit = -dLimit;
                        iRet = 1;
                }
                if(*pdToLimit > dLimit)
                {
                        *pdToLimit = dLimit;
                        iRet = 2;
                }

                return iRet;
        }

        static int limit(int* piToLimit, int iLimit)
        {
                int iRet = 0;

                if(*piToLimit < -iLimit)
                {
                        *piToLimit = -iLimit;
                        iRet = 1;
                }
                if(*piToLimit > iLimit)
                {
                        *piToLimit = iLimit;
                        iRet = 2;
                }

                return iRet;
        }

        static bool isInInterval(double dLow, double dHigh, double dVal)
        {
                if( (dVal >= dLow) && (dVal <= dHigh) )
                        return true;

                else
                        return false;
        }

};


//-----------------------------------------------
#endif