g2o
misc.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #ifndef G2O_STUFF_MISC_H
28 #define G2O_STUFF_MISC_H
29 
30 #include "macros.h"
31 #include <cmath>
32 
33 #ifndef M_PI
34 #define M_PI 3.14159265358979323846
35 #endif
36 
38 // @{
39 
46 namespace g2o {
47 
51 template <typename T>
52 inline T square(T x)
53 {
54  return x*x;
55 }
56 
60 template <typename T>
61 inline T hypot(T x, T y)
62 {
63  return (T) (sqrt(x*x + y*y));
64 }
65 
69 template <typename T>
70 inline T hypot_sqr(T x, T y)
71 {
72  return x*x + y*y;
73 }
74 
78 inline double deg2rad(double degree)
79 {
80  return degree * 0.01745329251994329576;
81 }
82 
86 inline double rad2deg(double rad)
87 {
88  return rad * 57.29577951308232087721;
89 }
90 
94 inline double normalize_theta(double theta)
95 {
96  if (theta >= -M_PI && theta < M_PI)
97  return theta;
98 
99  double multiplier = floor(theta / (2*M_PI));
100  theta = theta - multiplier*2*M_PI;
101  if (theta >= M_PI)
102  theta -= 2*M_PI;
103  if (theta < -M_PI)
104  theta += 2*M_PI;
105 
106  return theta;
107 }
108 
112 inline double inverse_theta(double th)
113 {
114  return normalize_theta(th + M_PI);
115 }
116 
120 inline double average_angle(double theta1, double theta2)
121 {
122  double x, y;
123 
124  x = cos(theta1) + cos(theta2);
125  y = sin(theta1) + sin(theta2);
126  if(x == 0 && y == 0)
127  return 0;
128  else
129  return std::atan2(y, x);
130 }
131 
136 template <typename T>
137 inline int sign(T x)
138 {
139  if (x > 0)
140  return 1;
141  else if (x < 0)
142  return -1;
143  else
144  return 0;
145 }
146 
150 template <typename T>
151 inline T clamp(T l, T x, T u)
152 {
153  if (x < l)
154  return l;
155  if (x > u)
156  return u;
157  return x;
158 }
159 
163 template <typename T>
164 inline T wrap(T l, T x, T u)
165 {
166  T intervalWidth = u - l;
167  while (x < l)
168  x += intervalWidth;
169  while (x > u)
170  x -= intervalWidth;
171  return x;
172 }
173 
177 inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0)
178 {
179  for (int i = 0; i < size; ++i)
180  if (g2o_isnan(array[i])) {
181  if (nanIndex)
182  *nanIndex = i;
183  return true;
184  }
185  return false;
186 }
187 
191 extern "C"
192 {
193  typedef void (* ForceLinkFunction) (void);
194 }
195 
197 {
198  ForceLinker(ForceLinkFunction function) { (function)(); }
199 };
200 
201 
202 } // end namespace
203 
204 // @}
205 
206 #endif
T square(T x)
Definition: misc.h:52
void(* ForceLinkFunction)(void)
Definition: misc.h:193
double deg2rad(double degree)
Definition: misc.h:78
T clamp(T l, T x, T u)
Definition: misc.h:151
double average_angle(double theta1, double theta2)
Definition: misc.h:120
T hypot_sqr(T x, T y)
Definition: misc.h:70
bool arrayHasNaN(const double *array, int size, int *nanIndex=0)
Definition: misc.h:177
int sign(T x)
Definition: misc.h:137
double rad2deg(double rad)
Definition: misc.h:86
T wrap(T l, T x, T u)
Definition: misc.h:164
double normalize_theta(double theta)
Definition: misc.h:94
#define g2o_isnan(x)
Definition: macros.h:99
ForceLinker(ForceLinkFunction function)
Definition: misc.h:198
double inverse_theta(double th)
Definition: misc.h:112
T hypot(T x, T y)
Definition: misc.h:61
#define M_PI
Definition: misc.h:34