MathSup.h

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering   
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob3_common
 * ROS package name: cob3_utilities
 * Description:
 *                                                              
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *                      
 * Author: Christian Connette, email:christian.connette@ipa.fhg.de
 * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
 *
 * Date of creation: Feb 2010
 * ToDo:
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Fraunhofer Institute for Manufacturing 
 *       Engineering and Automation (IPA) nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as 
 * published by the Free Software Foundation, either version 3 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 Lesser General Public License LGPL for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public 
 * License LGPL along with this program. 
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

#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