MathSup.h
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob3_common
00012  * ROS package name: cob3_utilities
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Feb 2010
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #ifndef MATHSUP_INCLUDEDEFX_H
00055 #define MATHSUP_INCLUDEDEFX_H
00056 
00057 #include <math.h>
00058 
00059 
00060 //-----------------------------------------------
00065 class MathSup
00066 {
00067 public:
00068         // -------- Constants
00070         static const double PI;
00071 
00073         static const double TWO_PI;
00074 
00076         static const double HALF_PI;
00077 
00078         // -------- Angle conversion
00082         static double convRadToDeg(const double& dAngRad)
00083         {
00084                 return (dAngRad * 180.0 / PI);
00085         }
00086 
00090         static double convDegToRad(const double& dAngDeg)
00091         {
00092                 return ( dAngDeg * PI / 180.0 );
00093         }
00094 
00098         static void normalize2Pi(double& angle)
00099         {
00100                 angle-=floor(angle/(2 * PI))* 2 * PI;
00101         }
00102 
00106         static void normalizePi(double& angle)
00107         {
00108                 normalize2Pi(angle);
00109                 if ( angle> PI ) angle-=2 * PI;
00110         }
00111 
00115         static void normalizePiHalf(double& angle)
00116         {
00117                 normalize2Pi(angle);
00118                 if (angle > PI) angle-=2*PI;
00119                 if (angle > PI/2.0) angle -= PI;
00120                 if (angle <= -PI/2.0) angle += PI;
00121         }
00122 
00126         static double sign(const double& x)
00127         {
00128                 if (x<0)
00129                         return -1.0;
00130                 else
00131                         return 1.0;
00132         }
00133 
00137         static double getMin(const double& a, const double& b)
00138         {
00139                 if (a < b)
00140                         return a;
00141                 else
00142                         return b;
00143         }
00144 
00148         static double getMax(const double& a, const double& b)
00149         {
00150                 if (a > b)
00151                         return a;
00152                 else
00153                         return b;
00154         }
00155 
00160         static double calcDeltaAng(const double& a, const double& b)
00161         {
00162                 double c = a-b;
00163                 normalizePi(c);
00164                 return c;
00165         }
00166 
00167 
00171         static double atan4quad(double y, double x)
00172         {
00173                 double result;
00174 
00175                 if((x==0.0) && (y==0.0))
00176                         result = 0.0;
00177                 else if((x==0.0) && (y > 0.0))
00178                         result = HALF_PI;
00179                 else if((x==0.0) && (y < 0.0))
00180                         result = -HALF_PI;
00181                 else if((y==0.0) && (x > 0.0))
00182                         result = 0.0;
00183                 else if((y==0.0) && (x < 0.0))
00184                         result = PI;
00185                 else
00186                 {
00187                         result = atan(y/x);
00188                         if(x<0.0)
00189                                 {
00190                                         if(y>0.0)               // Quadrant 2 -> correct
00191                                                 result += PI;
00192                                         else                    // Quadrant 3 -> correct
00193                                                 result -= PI;
00194                                 }               
00195                 }       
00196                 normalizePi(result);
00197                 return result;
00198         }
00199 
00200 
00204         static double distance(double x1, double y1, double x2, double y2)
00205         {
00206                 return sqrt( distanceSq( x1, y1, x2, y2 ) );
00207         }
00208 
00212         static double distanceSq(double x1, double y1, double x2, double y2)
00213         {
00214                 double dx = x2 - x1;
00215                 double dy = y2 - y1;
00216 
00217                 return dx*dx + dy*dy;
00218         }
00219 
00223         static bool isBitSet(int iVal, int iNrBit)
00224         {
00225                 if( (iVal & (1 << iNrBit)) == 0)
00226                         return false;
00227                 else
00228                         return true;
00229         }
00230         
00235         static double convFloatToInt4Byte(double dVal)
00236         {
00237                 //Todo 
00238                 return 1.0;
00239         }
00240 
00245         static double convInt4ByteToFloat(int iVal)
00246         {
00247                 unsigned char c[4];
00248                 double dSign;
00249                 double dFraction;
00250                 double dExp;
00251                 double dVal;
00252 
00253                 c[0] = iVal >> 24;
00254                 c[1] = iVal >> 16;
00255                 c[2] = iVal >> 8;
00256                 c[3] = iVal;
00257 
00258                 if( (c[0] & 0x80) == 0)
00259                         dSign = 1;
00260                 else
00261                         dSign = -1;
00262 
00263                 dFraction = (iVal & 0xFFFFFF) | 0x800000;
00264                 dFraction = dFraction * pow(2., -23);
00265 
00266                 dExp = ((c[0] << 1) | (c[1] >> 7)) - 127;
00267 
00268                 dVal = dSign * dFraction * pow(10., dExp);
00269                 
00270                 return dVal;
00271         }
00272 
00280         static int limit(double* pdToLimit, double dLimit)
00281         {
00282                 int iRet = 0;
00283 
00284                 if(*pdToLimit < -dLimit)
00285                 {
00286                         *pdToLimit = -dLimit;
00287                         iRet = 1;
00288                 }
00289                 if(*pdToLimit > dLimit)
00290                 {
00291                         *pdToLimit = dLimit;
00292                         iRet = 2;
00293                 }
00294 
00295                 return iRet;
00296         }
00297 
00305         static int limit(int* piToLimit, int iLimit)
00306         {
00307                 int iRet = 0;
00308 
00309                 if(*piToLimit < -iLimit)
00310                 {
00311                         *piToLimit = -iLimit;
00312                         iRet = 1;
00313                 }
00314                 if(*piToLimit > iLimit)
00315                 {
00316                         *piToLimit = iLimit;
00317                         iRet = 2;
00318                 }
00319 
00320                 return iRet;
00321         }
00322 
00330         static bool isInInterval(double dLow, double dHigh, double dVal)
00331         {
00332                 if( (dVal >= dLow) && (dVal <= dHigh) )
00333                         return true;
00334 
00335                 else
00336                         return false;
00337         }
00338 
00339 };
00340 
00341 
00342 //-----------------------------------------------
00343 #endif


cob_utilities
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:31