biped-stabilizer 1.5.0
Stabilizer for Biped Locomotion
Loading...
Searching...
No Matches
wykobi_math.hpp
Go to the documentation of this file.
1/*
2(***********************************************************************)
3(* *)
4(* Wykobi Computational Geometry Library *)
5(* Release Version 0.0.5 *)
6(* http://www.wykobi.com *)
7(* Copyright (c) 2005-2017 Arash Partow, All Rights Reserved. *)
8(* *)
9(* The Wykobi computational geometry library and its components are *)
10(* supplied under the terms of the open source MIT License. *)
11(* The contents of the Wykobi computational geometry library and its *)
12(* components may not be copied or disclosed except in accordance with *)
13(* the terms of the MIT License. *)
14(* *)
15(* URL: https://opensource.org/licenses/MIT *)
16(* *)
17(***********************************************************************)
18*/
19
20#ifndef INCLUDE_WYKOBI_MATH
21#define INCLUDE_WYKOBI_MATH
22
23#include <cmath>
24#include <limits>
25
26namespace wykobi {
27#define WYKOBI_DOUBLE_PRECISION
28
29#ifdef WYKOBI_SINGLE_PRECISION
30typedef float Float;
31#endif
32
33#ifdef WYKOBI_DOUBLE_PRECISION
34typedef double Float;
35#endif
36
37#ifdef WYKOBI_EXTENDED_PRECISION
38typedef long double Float;
39#endif
40
41/*************[ Epsilon constants ]*************/
42static const Float Epsilon_High = 1.0E-16;
43static const Float Epsilon_Medium = 1.0E-10;
44static const Float Epsilon_Low = 1.0E-07;
45static const Float Epsilon = Epsilon_Medium;
46static const Float Infinity = std::numeric_limits<Float>::infinity();
47
48/********[ Random resolution constants ]********/
49static const std::size_t RANDOM_RESOLUTION_INT = 1000000000;
50static const double RANDOM_RESOLUTION_FLT = RANDOM_RESOLUTION_INT * 1.0;
51
52/********[ ]********/
53static const Float PI = Float(3.141592653589793238462643383279500);
54static const Float PI2 = Float(6.283185307179586476925286766559000);
55static const Float PIDiv180 = Float(0.017453292519943295769236907684886);
56static const Float _180DivPI = Float(57.295779513082320876798154814105000);
57
58template <typename T>
59inline T sqr(const T& val);
60template <typename T>
61inline T sqrt(const T& val);
62template <typename T>
63inline T abs(const T& value);
64template <typename T>
65inline T max(const T& value1, const T& value2);
66template <typename T>
67inline T min(const T& value1, const T& value2);
68template <typename T>
69inline T max(const T& value1, const T& value2, const T& value3);
70template <typename T>
71inline T min(const T& value1, const T& value2, const T& value3);
72template <typename T>
73inline T infinity();
74
75template <typename T>
76inline T sin(const T& value);
77template <typename T>
78inline T cos(const T& value);
79template <typename T>
80inline T tan(const T& value);
81template <typename T>
82inline T asin(const T& value);
83template <typename T>
84inline T acos(const T& value);
85template <typename T>
86inline T atan(const T& value);
87template <typename T>
88inline T approx_sin(T angle);
89template <typename T>
90inline T approx_cos(T angle);
91template <typename T>
92inline T approx_tan(T angle);
93
94template <typename T>
95inline T clamp(const T& value, const T& low, const T& high);
96
97template <typename T>
98inline T sqr(const T& val) {
99 return val * val;
100}
101
102template <typename T>
103inline T sqrt(const T& val) {
104 return std::sqrt(val);
105}
106
107template <typename T>
108inline T abs(const T& value) {
109 return std::abs(value);
110}
111
112template <typename T>
113inline T max(const T& value1, const T& value2) {
114 return std::max<T>(value1, value2);
115}
116
117template <typename T>
118inline T min(const T& value1, const T& value2) {
119 return std::min<T>(value1, value2);
120}
121
122template <typename T>
123inline T max(const T& value1, const T& value2, const T& value3) {
124 return max(value1, max(value2, value3));
125}
126
127template <typename T>
128inline T min(const T& value1, const T& value2, const T& value3) {
129 return min(value1, min(value2, value3));
130}
131
132template <typename T>
133inline T infinity() {
134 return std::numeric_limits<T>::infinity();
135}
136
137template <typename T>
138inline T sin(const T& value) {
139 return std::sin(value);
140}
141
142template <typename T>
143inline T cos(const T& value) {
144 return std::cos(value);
145}
146
147template <typename T>
148inline T tan(const T& value) {
149 return std::tan(value);
150}
151
152template <typename T>
153inline T asin(const T& value) {
154 return std::asin(value);
155}
156
157template <typename T>
158inline T acos(const T& value) {
159 return std::acos(value);
160}
161
162template <typename T>
163inline T atan(const T& value) {
164 return std::atan(value);
165}
166
167template <typename T>
168inline T approx_sin(T angle) {
169 T final_sign = T(1.0);
170
171 if ((angle <= T(180.0)) && (angle > 90.0)) {
172 angle = T(180.0) - angle;
173 final_sign = T(1.0);
174 } else if ((angle <= T(270.0)) && (angle > T(180.0))) {
175 angle = angle - T(180.0);
176 final_sign = T(-1.0);
177 } else if ((angle <= T(360.0)) && (angle > T(270.0))) {
178 angle = T(360.0) - angle;
179 final_sign = T(-1.0);
180 }
181
182 angle *= T(PI / 180.0);
183 T asqr = angle * angle;
184 T result = T(-2.39e-08);
185 result *= asqr;
186 result += T(2.7526e-06);
187 result *= asqr;
188 result -= T(1.98409e-04);
189 result *= asqr;
190 result += T(8.3333315e-03);
191 result *= asqr;
192 result -= T(1.666666664e-01);
193 result *= asqr;
194 result += T(1.0);
195 result *= angle;
196
197 return result * final_sign;
198}
199
200template <typename T>
201inline T approx_cos(T angle) {
202 T final_sign = T(1.0);
203
204 if ((angle <= T(180.0)) && (angle > 90.0)) {
205 angle = T(180.0) - angle;
206 final_sign = T(-1.0);
207 } else if ((angle <= T(270.0)) && (angle > T(180.0))) {
208 angle = angle - T(180.0);
209 final_sign = T(-1.0);
210 } else if ((angle <= T(360.0)) && (angle > T(270.0))) {
211 angle = T(360.0) - angle;
212 final_sign = T(1.0);
213 }
214
215 angle *= T(PI / 180.0);
216 T asqr = angle * angle;
217 T result = T(-2.605e-07);
218 result *= asqr;
219 result += T(2.47609e-05);
220 result *= asqr;
221 result -= T(1.3888397e-03);
222 result *= asqr;
223 result += T(4.16666418e-02);
224 result *= asqr;
225 result -= T(4.999999963e-01);
226 result *= asqr;
227 result += T(1.0);
228
229 return result * final_sign;
230}
231
232template <typename T>
233inline T approx_tan(T angle) {
234 T final_sign = T(1.0);
235
236 if ((angle <= T(180.0)) && (angle > 90.0)) {
237 angle = T(180.0) - angle;
238 final_sign = T(-1.0);
239 } else if ((angle <= T(270.0)) && (angle > T(180.0))) {
240 angle = angle - T(180.0);
241 final_sign = T(1.0);
242 } else if ((angle <= T(360.0)) && (angle > T(270.0))) {
243 angle = T(360.0) - angle;
244 final_sign = T(-1.0);
245 }
246
247 angle *= T(PI / 180.0);
248 T asqr = angle * angle;
249 T result = T(9.5168091e-03);
250 result *= asqr;
251 result += T(2.900525e-03);
252 result *= asqr;
253 result += T(2.45650893e-02);
254 result *= asqr;
255 result += T(5.33740603e-02);
256 result *= asqr;
257 result += T(1.333923995e-01);
258 result *= asqr;
259 result += T(3.333314036e-01);
260 result *= asqr;
261 result += T(1.0);
262 result *= angle;
263
264 return result * final_sign;
265}
266
267template <typename T>
268inline T clamp(const T& value, const T& low_end, const T& high_end) {
269 if (value < low_end) return low_end;
270 if (value > high_end) return high_end;
271
272 return value;
273}
274
275} // namespace wykobi
276
277#endif
wykobi
Definition wykobi.hpp:32
wykobi::infinity
T infinity()
Definition wykobi_math.hpp:133
wykobi::acos
T acos(const T &value)
Definition wykobi_math.hpp:158
wykobi::approx_tan
T approx_tan(T angle)
Definition wykobi_math.hpp:233
wykobi::sin
T sin(const T &value)
Definition wykobi_math.hpp:138
wykobi::cos
T cos(const T &value)
Definition wykobi_math.hpp:143
wykobi::atan
T atan(const T &value)
Definition wykobi_math.hpp:163
wykobi::tan
T tan(const T &value)
Definition wykobi_math.hpp:148
wykobi::Float
double Float
Definition wykobi_math.hpp:34
wykobi::clamp
T clamp(const T &value, const T &low, const T &high)
Definition wykobi_math.hpp:268
wykobi::asin
T asin(const T &value)
Definition wykobi_math.hpp:153
wykobi::max
T max(const T &value1, const T &value2)
Definition wykobi_math.hpp:113
wykobi::sqrt
T sqrt(const T &val)
Definition wykobi_math.hpp:103
wykobi::sqr
T sqr(const T &val)
Definition wykobi_math.hpp:98
wykobi::min
T min(const T &value1, const T &value2)
Definition wykobi_math.hpp:118
wykobi::abs
T abs(const T &value)
Definition wykobi_math.hpp:108
wykobi::approx_sin
T approx_sin(T angle)
Definition wykobi_math.hpp:168
wykobi::approx_cos
T approx_cos(T angle)
Definition wykobi_math.hpp:201