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.
81GBXUTILACFR_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.
99GBXUTILACFR_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
183template<typename T>
184GBXUTILACFR_EXPORT void
185CLIP_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