INTRODUCTION
Overview
Download and Install
Documentation
Publications

REPOSITORY
Libraries

DEVELOPER
Dev Guide
Dashboard

PEOPLE
Contributors
Users

SourceForge.net Logo
Project
Download
Mailing lists

 

         
mathdefs.h
1 /*
2  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
3  * http://gearbox.sf.net/
4  * Copyright (c) 2004-2010 Alex Brooks, Alexei Makarenko, Tobias Kaupp
5  *
6  * This distribution is licensed to you under the terms described in
7  * the LICENSE file included in this distribution.
8  *
9  */
10 
11 /*dox
12  @file
13  @brief Mathematical macros
14  @ingroup orca_library_orcaiceutil
15  */
16 
17 #ifndef GBXUTILACFR_MATH_DEFINITIONS_H
18 #define GBXUTILACFR_MATH_DEFINITIONS_H
19 
20 #if defined (WIN32)
21  #if defined (GBXUTILACFR_STATIC)
22  #define GBXUTILACFR_EXPORT
23  #elif defined (GBXUTILACFR_EXPORTS)
24  #define GBXUTILACFR_EXPORT __declspec (dllexport)
25  #else
26  #define GBXUTILACFR_EXPORT __declspec (dllimport)
27  #endif
28 #else
29  #define GBXUTILACFR_EXPORT
30 #endif
31 
32 #include <assert.h>
33 
34 /*****************************************************************************
35  * INCLUDE THE RELEVANT MATHS LIBRARIES
36  *****************************************************************************/
37 
38 // MSVC compiler requires this symbol before exposing the (apparently)
39 // non-standard symbols M_PI, etc...
40 #ifdef WIN32
41 #define _USE_MATH_DEFINES
42 #endif
43 
44 #include <cmath>
45 
46 /*****************************************************************************
47  * CONSTANTS
48  *****************************************************************************/
49 // M_PI is not defined after including cmath for the MS visual studio compiler?
50 #ifndef M_PI
51 //dox Defines number Pi
52 #define M_PI 3.14159265358979323846
53 #endif
54 
55 #ifndef NAN
56 //dox Defines not-a-number
57 #define NAN (__builtin_nanf(""))
58 #endif
59 
60 #ifndef INF
61 //dox Defines infinity
62 #define INF (__builtin_inff())
63 #endif
64 
65 /*****************************************************************************
66  * CONVERSION MACROS
67  *****************************************************************************/
68 #ifndef DEG2RAD_RATIO
69 //dox Convertion factor from degrees to radians.
70 #define DEG2RAD_RATIO (M_PI/180.0)
71 #endif
72 
73 //dox Converts from degrees to radians.
74 #define DEG2RAD(deg) ((deg)*DEG2RAD_RATIO)
75 //dox Converts from radians to degrees.
76 #define RAD2DEG(rad) ((rad)/DEG2RAD_RATIO)
77 
78 //dox Normalises the angle [rad] to the range [-pi,pi)
79 //dox Don't return the normalised angle, because it's easy to make the
80 //dox mistake of doing: 'NORMALISE_ANGLE( myAngle )', ignoring the return value.
81 GBXUTILACFR_EXPORT inline void NORMALISE_ANGLE( double &theta )
82 {
83  double multiplier;
84 
85  if (theta >= -M_PI && theta < M_PI)
86  return;
87 
88  multiplier = std::floor(theta / (2*M_PI));
89  theta -= multiplier*2*M_PI;
90  if (theta >= M_PI)
91  theta -= 2*M_PI;
92  else if (theta < -M_PI)
93  theta += 2*M_PI;
94 }
95 
96 //dox Normalises the angle [rad] to the range [-pi,pi)
97 //dox Don't return the normalised angle, because it's easy to make the
98 //dox mistake of doing: 'NORMALISE_ANGLE( myAngle )', ignoring the return value.
99 GBXUTILACFR_EXPORT inline void NORMALISE_ANGLE( float &theta )
100 {
101  double thDouble = theta;
102  NORMALISE_ANGLE( thDouble );
103  theta = (float)thDouble;
104 }
105 
106 /*****************************************************************************
107  * MATH MACROS
108  *****************************************************************************/
109 // for compatability retain this old one
110 //#ifndef ABS
111 //#define ABS(x) (std::abs(x))
112 //#endif
113 #ifndef MIN
114 //dox Minimum of two numbers
115 #define MIN(x, y) (((x) < (y)) ? (x) : (y))
116 #endif
117 #ifndef MAX
118 //dox Maximum of two numbers
119 #define MAX(x, y) (((x) > (y)) ? (x) : (y))
120 #endif
121 #ifndef APPLY_LIMITS
122 //dox Limits x to an interval between min_x and max_x.
123 //dox x must be a variable which can be assigned a value;
124 //dox Compares using less_than and greater_than.
125 #define APPLY_LIMITS(max_x, x, min_x) \
126  if((x)>(max_x)) x=(max_x); if((x)<(min_x)) x=(min_x);
127 #endif
128 #ifndef NORM2
129 //dox Norm of a 2D vector
130 #define NORM2(x, y) (sqrt((x)*(x)+(y)*(y)))
131 #endif
132 #ifndef NORM3
133 //dox Norm of a 3D vector
134 #define NORM3(x, y, z) (sqrt((x)*(x)+(y)*(y)+(z)*(z)))
135 #endif
136 #ifndef ROUND
137 //dox Rounds double or float to the nearest integer.
138 #define ROUND(x) ((int)(x+0.5))
139 #endif
140 #ifndef ROUND_TO
141 //dox Rounds @c n to the nearest whole number of multiples of @c d.
142 //dox For example, ROUND_TO(8,5) and ROUND_TO(12,5) will both result in 10.
143 #define ROUND_TO(n,d) (d*rint(n/d))
144 #endif
145 #ifndef SIGN
146 //dox Sign of a number.
147 #define SIGN(A) ((A)<0?(-1):(1))
148 #endif
149 #ifndef COS_LAW
150 //dox Law of cosines.
151 #define COS_LAW(side1, side2, theta) \
152  (sqrt(SQR(side1)+SQR(side2)-2.0*(side1)*(side2)*cos(theta)))
153 #endif
154 #ifndef INV_COS_LAW
155 //dox Inverse law of cosines..
156 #define INV_COS_LAW(oppSide, side1, side2) \
157  (acos((SQR(side1)+SQR(side2)-SQR(oppSide))/(2.0*(side1)*(side2))))
158 #endif
159 
160 // isinf not defined on all systems (eg Solaris)
161 #if defined (__SVR4) && defined (__sun)
162 #define isfinite(x) \
163  __extension__ ({ __typeof (x) __x_f = (x); \
164  __builtin_expect(!isnan(__x_f - __x_f), 1); })
165 
166 #define isinf(x) \
167  __extension__ ({ __typeof (x) __x_i = (x); \
168  __builtin_expect(!isnan(__x_i) && !isfinite(__x_i), 0); })
169 #endif
170 
171 
172 /*****************************************************************************
173  * COMPARISON MACROS
174  *****************************************************************************/
175 #ifndef NEAR
176 //dox Check that two numbers are sufficiently close.
177 //dox Compares using less_than and greater_than.
178 #define NEAR(x,y,epsilon) (((x) > (y)-(epsilon)) && ((x) < (y)+(epsilon)))
179 #endif
180 
181 //dox Modifies x to lie within [x_min,x_max]
182 //dox
183 template<typename T>
184 GBXUTILACFR_EXPORT void
185 CLIP_TO_LIMITS( const T &min_x, T &x, const T &max_x )
186 {
187  assert( min_x <= max_x );
188  if ( x > max_x )
189  x = max_x;
190  else if ( x < min_x )
191  x = min_x;
192 }
193 
194 #endif
 

Generated for GearBox by  doxygen 1.4.5