merge changes from the defunct ubot

ext/crlib/cr-math.h

@@ -0,0 +1,143 @@
+//
+// CRLib - Simple library for STL replacement in private projects.
+// Copyright © 2020 YaPB Development Team <team@yapb.ru>.
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+// 
+
+#pragma once
+
+#include <crlib/cr-basic.h>
+
+#if defined (CR_HAS_SSE)
+#  include <pmmintrin.h>
+#endif
+
+#include <math.h>
+
+CR_NAMESPACE_BEGIN
+
+constexpr float kFloatEpsilon = 0.01f;
+constexpr float kFloatEqualEpsilon = 0.001f;
+constexpr float kFloatCmpEpsilon = 1.192092896e-07f;
+
+constexpr float kMathPi = 3.141592653589793115997963468544185161590576171875f;
+constexpr float kMathPiHalf = kMathPi * 0.5f;
+
+constexpr float kDegreeToRadians = kMathPi / 180.0f;
+constexpr float kRadiansToDegree = 180.0f / kMathPi;
+
+constexpr bool fzero (const float e) {
+   return cr::abs (e) < kFloatEpsilon;
+}
+
+constexpr bool fequal (const float a, const float b) {
+   return cr:: abs (a - b) < kFloatEqualEpsilon;
+}
+
+constexpr float rad2deg (const float r) {
+   return r * kRadiansToDegree;
+}
+
+constexpr float deg2rad (const float d) {
+   return d * kDegreeToRadians;
+}
+
+constexpr float modAngles (const float a) {
+   return 360.0f / 65536.0f * (static_cast <int> (a * (65536.0f / 360.0f)) & 65535);
+}
+
+constexpr float normalizeAngles (const float a) {
+   return 360.0f / 65536.0f * (static_cast <int> ((a + 180.0f) * (65536.0f / 360.0f)) & 65535) - 180.0f;
+}
+
+constexpr float anglesDifference (const float a, const float b) {
+   return normalizeAngles (a - b);
+}
+
+inline float sinf (const float value) {
+   return ::sinf (value);
+}
+
+inline float cosf (const float value) {
+   return ::cosf (value);
+}
+
+inline float atanf (const float value) {
+   return ::atanf (value);
+}
+
+inline float atan2f (const float y, const float x) {
+   return ::atan2f (y, x);
+}
+
+inline float powf (const float x, const float y) {
+   return ::powf (x, y);
+}
+
+inline float sqrtf (const float value) {
+   return ::sqrtf (value);
+}
+
+inline float tanf (const float value) {
+   return ::tanf (value);
+}
+
+inline float ceilf (const float value) {
+   return ::ceilf (value);
+}
+
+inline void sincosf (const float x, const float y, const float z, float *sines, float *cosines) {
+#if defined (CR_HAS_SSE)
+   auto set = _mm_set_ps (x, y, z, 0.0f);
+
+   auto _mm_sin = [] (__m128 rad) -> __m128 {
+      static auto pi2 = _mm_set_ps1 (kMathPi * 2);
+      static auto rp1 = _mm_set_ps1 (4.0f / kMathPi);
+      static auto rp2 = _mm_set_ps1 (-4.0f / (kMathPi * kMathPi));
+      static auto val = _mm_cmpnlt_ps (rad, _mm_set_ps1 (kMathPi));
+      static auto csi = _mm_castsi128_ps (_mm_set1_epi32 (0x80000000));
+
+      val = _mm_and_ps (val, pi2);
+      rad = _mm_sub_ps (rad, val);
+      val = _mm_cmpngt_ps (rad, _mm_set_ps1 (-kMathPi));
+      val = _mm_and_ps (val, pi2);
+      rad = _mm_add_ps (rad, val);
+      val = _mm_mul_ps (_mm_andnot_ps (csi, rad), rp2);
+      val = _mm_add_ps (val, rp1);
+
+      auto si = _mm_mul_ps (val, rad);
+
+      val = _mm_mul_ps (_mm_andnot_ps (csi, si), si);
+      val = _mm_sub_ps (val, si);
+      val = _mm_mul_ps (val, _mm_set_ps1 (0.225f));
+
+      return _mm_add_ps (val, si);
+   };
+   static auto hpi = _mm_set_ps1 (kMathPiHalf);
+
+   auto s = _mm_sin (set);
+   auto c = _mm_sin (_mm_add_ps (set, hpi));
+
+   _mm_store_ps (sines, _mm_shuffle_ps (s, s, _MM_SHUFFLE (0, 1, 2, 3)));
+   _mm_store_ps (cosines, _mm_shuffle_ps (c, c, _MM_SHUFFLE (0, 1, 2, 3)));
+#else
+   sines[0] = cr::sinf (x);
+   sines[1] = cr::sinf (y);
+   sines[2] = cr::sinf (z);
+
+   cosines[0] = cr::cosf (x);
+   cosines[1] = cr::cosf (y);
+   cosines[2] = cr::cosf (z);
+#endif
+}
+
+CR_NAMESPACE_END