1 /* Copyright (C) 2013-2016, The Regents of The University of Michigan.
2 All rights reserved.
3 This software was developed in the APRIL Robotics Lab under the
4 direction of Edwin Olson, ebolson@umich.edu. This software may be
5 available under alternative licensing terms; contact the address above.
6 Redistribution and use in source and binary forms, with or without
7 modification, are permitted provided that the following conditions are met:
8 1. Redistributions of source code must retain the above copyright notice, this
9 list of conditions and the following disclaimer.
10 2. Redistributions in binary form must reproduce the above copyright notice,
11 this list of conditions and the following disclaimer in the documentation
12 and/or other materials provided with the distribution.
13 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
14 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
16 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
17 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
18 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
20 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
21 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
22 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 The views and conclusions contained in the software and documentation are those
24 of the authors and should not be interpreted as representing official policies,
25 either expressed or implied, of the Regents of The University of Michigan.
26 */
27
28 #pragma once
29
30 #include <math.h>
31 #include <float.h>
32 #include <stdlib.h>
33 #include <stdint.h>
34 #include <assert.h>
35 #include <string.h> // memcpy
36
37 #ifdef __cplusplus
38 //extern "C" {
39 #endif
40
41 #ifndef M_TWOPI
42 # define M_TWOPI 6.2831853071795862319959 /* 2*pi */
43 #endif
44
45 #ifndef M_PI
46 # define M_PI 3.141592653589793238462643383279502884196
47 #endif
48
49 #define to_radians(x) ( (x) * (M_PI / 180.0 ))
50 #define to_degrees(x) ( (x) * (180.0 / M_PI ))
51
52 #ifndef max
53 #define max(A, B) (A < B ? B : A)
54 #endif
55 #ifndef min
56 #define min(A, B) (A < B ? A : B)
57 #endif
58
59 /* DEPRECATE, threshold meaningless without context.
60 static inline int dequals(double a, double b)
61 {
62 double thresh = 1e-9;
63 return (fabs(a-b) < thresh);
64 }
65 */
66
dequals_mag(double a,double b,double thresh)67 static inline int dequals_mag(double a, double b, double thresh)
68 {
69 return (fabs(a-b) < thresh);
70 }
71
isq(int v)72 static inline int isq(int v)
73 {
74 return v*v;
75 }
76
fsq(float v)77 static inline float fsq(float v)
78 {
79 return v*v;
80 }
81
sq(double v)82 static inline double sq(double v)
83 {
84 return v*v;
85 }
86
sgn(double v)87 static inline double sgn(double v)
88 {
89 return (v>=0) ? 1 : -1;
90 }
91
92 // random number between [0, 1)
randf()93 static inline float randf()
94 {
95 return (float)(rand() / (static_cast<float>(RAND_MAX) + 1.0f));
96 }
97
98
signed_randf()99 static inline float signed_randf()
100 {
101 return randf()*2 - 1;
102 }
103
104 // return a random integer between [0, bound)
irand(int bound)105 static inline int irand(int bound)
106 {
107 int v = (int) (randf()*bound);
108 if (v == bound)
109 return (bound-1);
110 //assert(v >= 0);
111 //assert(v < bound);
112 return v;
113 }
114
115 /** Map vin to [0, 2*PI) **/
mod2pi_positive(double vin)116 static inline double mod2pi_positive(double vin)
117 {
118 return vin - M_TWOPI * floor(vin / M_TWOPI);
119 }
120
121 /** Map vin to [-PI, PI) **/
mod2pi(double vin)122 static inline double mod2pi(double vin)
123 {
124 return mod2pi_positive(vin + M_PI) - M_PI;
125 }
126
127 /** Return vin such that it is within PI degrees of ref **/
mod2pi_ref(double ref,double vin)128 static inline double mod2pi_ref(double ref, double vin)
129 {
130 return ref + mod2pi(vin - ref);
131 }
132
133 /** Map vin to [0, 360) **/
mod360_positive(double vin)134 static inline double mod360_positive(double vin)
135 {
136 return vin - 360 * floor(vin / 360);
137 }
138
139 /** Map vin to [-180, 180) **/
mod360(double vin)140 static inline double mod360(double vin)
141 {
142 return mod360_positive(vin + 180) - 180;
143 }
144
mod_positive(int vin,int mod)145 static inline int mod_positive(int vin, int mod) {
146 return (vin % mod + mod) % mod;
147 }
148
theta_to_int(double theta,int max)149 static inline int theta_to_int(double theta, int max)
150 {
151 theta = mod2pi_ref(M_PI, theta);
152 int v = (int) (theta / M_TWOPI * max);
153
154 if (v == max)
155 v = 0;
156
157 assert (v >= 0 && v < max);
158
159 return v;
160 }
161
imin(int a,int b)162 static inline int imin(int a, int b)
163 {
164 return (a < b) ? a : b;
165 }
166
imax(int a,int b)167 static inline int imax(int a, int b)
168 {
169 return (a > b) ? a : b;
170 }
171
imin64(int64_t a,int64_t b)172 static inline int64_t imin64(int64_t a, int64_t b)
173 {
174 return (a < b) ? a : b;
175 }
176
imax64(int64_t a,int64_t b)177 static inline int64_t imax64(int64_t a, int64_t b)
178 {
179 return (a > b) ? a : b;
180 }
181
iclamp(int v,int minv,int maxv)182 static inline int iclamp(int v, int minv, int maxv)
183 {
184 return imax(minv, imin(v, maxv));
185 }
186
dclamp(double a,double min,double max)187 static inline double dclamp(double a, double min, double max)
188 {
189 if (a < min)
190 return min;
191 if (a > max)
192 return max;
193 return a;
194 }
195
fltcmp(float f1,float f2)196 static inline int fltcmp (float f1, float f2)
197 {
198 float epsilon = f1-f2;
199 if (epsilon < 0.0)
200 return -1;
201 else if (epsilon > 0.0)
202 return 1;
203 else
204 return 0;
205 }
206
dblcmp(double d1,double d2)207 static inline int dblcmp (double d1, double d2)
208 {
209 double epsilon = d1-d2;
210 if (epsilon < 0.0)
211 return -1;
212 else if (epsilon > 0.0)
213 return 1;
214 else
215 return 0;
216 }
217
double_pos_inf()218 static inline double double_pos_inf() {
219 //https://developer.arm.com/docs/dui0475/g/floating-point-support/sample-double-precision-floating-point-values-for-ieee-754-arithmetic
220 union {double d; int64_t i;} u;
221 u.i = 0x7FF0000000000000;
222 return u.d;
223 }
224
double_neg_inf()225 static inline double double_neg_inf() {
226 //https://developer.arm.com/docs/dui0475/g/floating-point-support/sample-double-precision-floating-point-values-for-ieee-754-arithmetic
227 union {double d; uint64_t i;} u;
228 u.i = 0xFFF0000000000000;
229 return u.d;
230 }
231
float_pos_inf()232 static inline float float_pos_inf() {
233 //https://developer.arm.com/docs/dui0475/g/floating-point-support/sample-single-precision-floating-point-values-for-ieee-754-arithmetic
234 union {float f; int32_t i;} u;
235 u.i = 0x7F800000;
236 return u.f;
237 }
238
float_neg_inf()239 static inline float float_neg_inf() {
240 //https://developer.arm.com/docs/dui0475/g/floating-point-support/sample-single-precision-floating-point-values-for-ieee-754-arithmetic
241 union {float f; uint32_t i;} u;
242 u.i = 0xFF800000;
243 return u.f;
244 }
245
246 #ifdef __cplusplus
247 //}
248 #endif
249