2
0

irrMath.h 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477
  1. // Copyright (C) 2002-2012 Nikolaus Gebhardt
  2. // This file is part of the "Irrlicht Engine".
  3. // For conditions of distribution and use, see copyright notice in irrlicht.h
  4. #pragma once
  5. #include "irrTypes.h"
  6. #include <cmath>
  7. #include <cfloat>
  8. #include <cstdlib> // for abs() etc.
  9. #include <climits> // For INT_MAX / UINT_MAX
  10. #include <type_traits>
  11. namespace irr
  12. {
  13. namespace core
  14. {
  15. //! Rounding error constant often used when comparing f32 values.
  16. const f32 ROUNDING_ERROR_f32 = 0.000001f;
  17. const f64 ROUNDING_ERROR_f64 = 0.00000001;
  18. #ifdef PI // make sure we don't collide with a define
  19. #undef PI
  20. #endif
  21. //! Constant for PI.
  22. const f32 PI = 3.14159265359f;
  23. //! Constant for reciprocal of PI.
  24. const f32 RECIPROCAL_PI = 1.0f / PI;
  25. //! Constant for half of PI.
  26. const f32 HALF_PI = PI / 2.0f;
  27. #ifdef PI64 // make sure we don't collide with a define
  28. #undef PI64
  29. #endif
  30. //! Constant for 64bit PI.
  31. const f64 PI64 = 3.1415926535897932384626433832795028841971693993751;
  32. //! Constant for 64bit reciprocal of PI.
  33. const f64 RECIPROCAL_PI64 = 1.0 / PI64;
  34. //! 32bit Constant for converting from degrees to radians
  35. const f32 DEGTORAD = PI / 180.0f;
  36. //! 32bit constant for converting from radians to degrees (formally known as GRAD_PI)
  37. const f32 RADTODEG = 180.0f / PI;
  38. //! 64bit constant for converting from degrees to radians (formally known as GRAD_PI2)
  39. const f64 DEGTORAD64 = PI64 / 180.0;
  40. //! 64bit constant for converting from radians to degrees
  41. const f64 RADTODEG64 = 180.0 / PI64;
  42. //! Utility function to convert a radian value to degrees
  43. /** Provided as it can be clearer to write radToDeg(X) than RADTODEG * X
  44. \param radians The radians value to convert to degrees.
  45. */
  46. inline f32 radToDeg(f32 radians)
  47. {
  48. return RADTODEG * radians;
  49. }
  50. //! Utility function to convert a radian value to degrees
  51. /** Provided as it can be clearer to write radToDeg(X) than RADTODEG * X
  52. \param radians The radians value to convert to degrees.
  53. */
  54. inline f64 radToDeg(f64 radians)
  55. {
  56. return RADTODEG64 * radians;
  57. }
  58. //! Utility function to convert a degrees value to radians
  59. /** Provided as it can be clearer to write degToRad(X) than DEGTORAD * X
  60. \param degrees The degrees value to convert to radians.
  61. */
  62. inline f32 degToRad(f32 degrees)
  63. {
  64. return DEGTORAD * degrees;
  65. }
  66. //! Utility function to convert a degrees value to radians
  67. /** Provided as it can be clearer to write degToRad(X) than DEGTORAD * X
  68. \param degrees The degrees value to convert to radians.
  69. */
  70. inline f64 degToRad(f64 degrees)
  71. {
  72. return DEGTORAD64 * degrees;
  73. }
  74. //! returns minimum of two values. Own implementation to get rid of the STL (VS6 problems)
  75. template <class T>
  76. inline const T &min_(const T &a, const T &b)
  77. {
  78. return a < b ? a : b;
  79. }
  80. //! returns minimum of three values. Own implementation to get rid of the STL (VS6 problems)
  81. template <class T>
  82. inline const T &min_(const T &a, const T &b, const T &c)
  83. {
  84. return a < b ? min_(a, c) : min_(b, c);
  85. }
  86. //! returns maximum of two values. Own implementation to get rid of the STL (VS6 problems)
  87. template <class T>
  88. inline const T &max_(const T &a, const T &b)
  89. {
  90. return a < b ? b : a;
  91. }
  92. //! returns maximum of three values. Own implementation to get rid of the STL (VS6 problems)
  93. template <class T>
  94. inline const T &max_(const T &a, const T &b, const T &c)
  95. {
  96. return a < b ? max_(b, c) : max_(a, c);
  97. }
  98. //! returns abs of two values. Own implementation to get rid of STL (VS6 problems)
  99. template <class T>
  100. inline T abs_(const T &a)
  101. {
  102. return a < (T)0 ? -a : a;
  103. }
  104. //! returns linear interpolation of a and b with ratio t
  105. //! \return: a if t==0, b if t==1, and the linear interpolation else
  106. template <class T>
  107. inline T lerp(const T &a, const T &b, const f32 t)
  108. {
  109. return (T)(a * (1.f - t)) + (b * t);
  110. }
  111. //! clamps a value between low and high
  112. template <class T>
  113. inline const T clamp(const T &value, const T &low, const T &high)
  114. {
  115. return min_(max_(value, low), high);
  116. }
  117. //! swaps the content of the passed parameters
  118. // Note: We use the same trick as boost and use two template arguments to
  119. // avoid ambiguity when swapping objects of an Irrlicht type that has not
  120. // it's own swap overload. Otherwise we get conflicts with some compilers
  121. // in combination with stl.
  122. template <class T1, class T2>
  123. inline void swap(T1 &a, T2 &b)
  124. {
  125. T1 c(a);
  126. a = b;
  127. b = c;
  128. }
  129. template <class T>
  130. inline T roundingError();
  131. template <>
  132. inline f32 roundingError()
  133. {
  134. return ROUNDING_ERROR_f32;
  135. }
  136. template <>
  137. inline f64 roundingError()
  138. {
  139. return ROUNDING_ERROR_f64;
  140. }
  141. template <class T>
  142. inline T relativeErrorFactor()
  143. {
  144. return 1;
  145. }
  146. template <>
  147. inline f32 relativeErrorFactor()
  148. {
  149. return 4;
  150. }
  151. template <>
  152. inline f64 relativeErrorFactor()
  153. {
  154. return 8;
  155. }
  156. //! returns if a equals b, for types without rounding errors
  157. template <class T, std::enable_if_t<std::is_integral<T>::value, bool> = true>
  158. inline bool equals(const T a, const T b)
  159. {
  160. return a == b;
  161. }
  162. //! returns if a equals b, taking possible rounding errors into account
  163. template <class T, std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
  164. inline bool equals(const T a, const T b, const T tolerance = roundingError<T>())
  165. {
  166. return std::abs(a - b) <= tolerance;
  167. }
  168. //! returns if a equals b, taking relative error in form of factor
  169. //! this particular function does not involve any division.
  170. template <class T>
  171. inline bool equalsRelative(const T a, const T b, const T factor = relativeErrorFactor<T>())
  172. {
  173. // https://eagergames.wordpress.com/2017/04/01/fast-parallel-lines-and-vectors-test/
  174. const T maxi = max_(a, b);
  175. const T mini = min_(a, b);
  176. const T maxMagnitude = max_(maxi, -mini);
  177. return (maxMagnitude * factor + maxi) == (maxMagnitude * factor + mini); // MAD Wise
  178. }
  179. union FloatIntUnion32
  180. {
  181. FloatIntUnion32(float f1 = 0.0f) :
  182. f(f1) {}
  183. // Portable sign-extraction
  184. bool sign() const { return (i >> 31) != 0; }
  185. irr::s32 i;
  186. irr::f32 f;
  187. };
  188. //! We compare the difference in ULP's (spacing between floating-point numbers, aka ULP=1 means there exists no float between).
  189. //\result true when numbers have a ULP <= maxUlpDiff AND have the same sign.
  190. inline bool equalsByUlp(f32 a, f32 b, int maxUlpDiff)
  191. {
  192. // Based on the ideas and code from Bruce Dawson on
  193. // http://www.altdevblogaday.com/2012/02/22/comparing-floating-point-numbers-2012-edition/
  194. // When floats are interpreted as integers the two nearest possible float numbers differ just
  195. // by one integer number. Also works the other way round, an integer of 1 interpreted as float
  196. // is for example the smallest possible float number.
  197. const FloatIntUnion32 fa(a);
  198. const FloatIntUnion32 fb(b);
  199. // Different signs, we could maybe get difference to 0, but so close to 0 using epsilons is better.
  200. if (fa.sign() != fb.sign()) {
  201. // Check for equality to make sure +0==-0
  202. if (fa.i == fb.i)
  203. return true;
  204. return false;
  205. }
  206. // Find the difference in ULPs.
  207. const int ulpsDiff = abs_(fa.i - fb.i);
  208. if (ulpsDiff <= maxUlpDiff)
  209. return true;
  210. return false;
  211. }
  212. //! returns if a equals zero, taking rounding errors into account
  213. inline bool iszero(const f64 a, const f64 tolerance = ROUNDING_ERROR_f64)
  214. {
  215. return fabs(a) <= tolerance;
  216. }
  217. //! returns if a equals zero, taking rounding errors into account
  218. inline bool iszero(const f32 a, const f32 tolerance = ROUNDING_ERROR_f32)
  219. {
  220. return fabsf(a) <= tolerance;
  221. }
  222. //! returns if a equals not zero, taking rounding errors into account
  223. inline bool isnotzero(const f32 a, const f32 tolerance = ROUNDING_ERROR_f32)
  224. {
  225. return fabsf(a) > tolerance;
  226. }
  227. //! returns if a equals zero, taking rounding errors into account
  228. inline bool iszero(const s32 a, const s32 tolerance = 0)
  229. {
  230. return (a & 0x7ffffff) <= tolerance;
  231. }
  232. //! returns if a equals zero, taking rounding errors into account
  233. inline bool iszero(const u32 a, const u32 tolerance = 0)
  234. {
  235. return a <= tolerance;
  236. }
  237. //! returns if a equals zero, taking rounding errors into account
  238. inline bool iszero(const s64 a, const s64 tolerance = 0)
  239. {
  240. return abs_(a) <= tolerance;
  241. }
  242. inline s32 s32_min(s32 a, s32 b)
  243. {
  244. return min_(a, b);
  245. }
  246. inline s32 s32_max(s32 a, s32 b)
  247. {
  248. return max_(a, b);
  249. }
  250. inline s32 s32_clamp(s32 value, s32 low, s32 high)
  251. {
  252. return clamp(value, low, high);
  253. }
  254. /*
  255. float IEEE-754 bit representation
  256. 0 0x00000000
  257. 1.0 0x3f800000
  258. 0.5 0x3f000000
  259. 3 0x40400000
  260. +inf 0x7f800000
  261. -inf 0xff800000
  262. +NaN 0x7fc00000 or 0x7ff00000
  263. in general: number = (sign ? -1:1) * 2^(exponent) * 1.(mantissa bits)
  264. */
  265. typedef union
  266. {
  267. u32 u;
  268. s32 s;
  269. f32 f;
  270. } inttofloat;
  271. #define F32_AS_S32(f) (*((s32 *)&(f)))
  272. #define F32_AS_U32(f) (*((u32 *)&(f)))
  273. #define F32_AS_U32_POINTER(f) (((u32 *)&(f)))
  274. #define F32_VALUE_0 0x00000000
  275. #define F32_VALUE_1 0x3f800000
  276. //! code is taken from IceFPU
  277. //! Integer representation of a floating-point value.
  278. inline u32 IR(f32 x)
  279. {
  280. inttofloat tmp;
  281. tmp.f = x;
  282. return tmp.u;
  283. }
  284. //! Floating-point representation of an integer value.
  285. inline f32 FR(u32 x)
  286. {
  287. inttofloat tmp;
  288. tmp.u = x;
  289. return tmp.f;
  290. }
  291. inline f32 FR(s32 x)
  292. {
  293. inttofloat tmp;
  294. tmp.s = x;
  295. return tmp.f;
  296. }
  297. #define F32_LOWER_0(n) ((n) < 0.0f)
  298. #define F32_LOWER_EQUAL_0(n) ((n) <= 0.0f)
  299. #define F32_GREATER_0(n) ((n) > 0.0f)
  300. #define F32_GREATER_EQUAL_0(n) ((n) >= 0.0f)
  301. #define F32_EQUAL_1(n) ((n) == 1.0f)
  302. #define F32_EQUAL_0(n) ((n) == 0.0f)
  303. #define F32_A_GREATER_B(a, b) ((a) > (b))
  304. #ifndef REALINLINE
  305. #ifdef _MSC_VER
  306. #define REALINLINE __forceinline
  307. #else
  308. #define REALINLINE inline
  309. #endif
  310. #endif
  311. // NOTE: This is not as exact as the c99/c++11 round function, especially at high numbers starting with 8388609
  312. // (only low number which seems to go wrong is 0.49999997 which is rounded to 1)
  313. // Also negative 0.5 is rounded up not down unlike with the standard function (p.E. input -0.5 will be 0 and not -1)
  314. inline f32 round_(f32 x)
  315. {
  316. return floorf(x + 0.5f);
  317. }
  318. // calculate: sqrt ( x )
  319. REALINLINE f32 squareroot(const f32 f)
  320. {
  321. return sqrtf(f);
  322. }
  323. // calculate: sqrt ( x )
  324. REALINLINE f64 squareroot(const f64 f)
  325. {
  326. return sqrt(f);
  327. }
  328. // calculate: sqrt ( x )
  329. REALINLINE s32 squareroot(const s32 f)
  330. {
  331. return static_cast<s32>(squareroot(static_cast<f32>(f)));
  332. }
  333. // calculate: sqrt ( x )
  334. REALINLINE s64 squareroot(const s64 f)
  335. {
  336. return static_cast<s64>(squareroot(static_cast<f64>(f)));
  337. }
  338. // calculate: 1 / sqrt ( x )
  339. REALINLINE f64 reciprocal_squareroot(const f64 x)
  340. {
  341. return 1.0 / sqrt(x);
  342. }
  343. // calculate: 1 / sqrtf ( x )
  344. REALINLINE f32 reciprocal_squareroot(const f32 f)
  345. {
  346. return 1.f / sqrtf(f);
  347. }
  348. // calculate: 1 / sqrtf( x )
  349. REALINLINE s32 reciprocal_squareroot(const s32 x)
  350. {
  351. return static_cast<s32>(reciprocal_squareroot(static_cast<f32>(x)));
  352. }
  353. // calculate: 1 / x
  354. REALINLINE f32 reciprocal(const f32 f)
  355. {
  356. return 1.f / f;
  357. }
  358. // calculate: 1 / x
  359. REALINLINE f64 reciprocal(const f64 f)
  360. {
  361. return 1.0 / f;
  362. }
  363. // calculate: 1 / x, low precision allowed
  364. REALINLINE f32 reciprocal_approxim(const f32 f)
  365. {
  366. return 1.f / f;
  367. }
  368. REALINLINE s32 floor32(f32 x)
  369. {
  370. return (s32)floorf(x);
  371. }
  372. REALINLINE s32 ceil32(f32 x)
  373. {
  374. return (s32)ceilf(x);
  375. }
  376. // NOTE: Please check round_ documentation about some inaccuracies in this compared to standard library round function.
  377. REALINLINE s32 round32(f32 x)
  378. {
  379. return (s32)round_(x);
  380. }
  381. inline f32 f32_max3(const f32 a, const f32 b, const f32 c)
  382. {
  383. return a > b ? (a > c ? a : c) : (b > c ? b : c);
  384. }
  385. inline f32 f32_min3(const f32 a, const f32 b, const f32 c)
  386. {
  387. return a < b ? (a < c ? a : c) : (b < c ? b : c);
  388. }
  389. inline f32 fract(f32 x)
  390. {
  391. return x - floorf(x);
  392. }
  393. } // end namespace core
  394. } // end namespace irr
  395. using irr::core::FR;
  396. using irr::core::IR;