Add fsin and fcos functions to angular library

robocin/utility/CMakeLists.txt

@@ -36,7 +36,7 @@ robocin_cpp_test(
 
 robocin_cpp_library(
         NAME angular
-        HDRS angular.h
+        HDRS angular.h internal/angular_internal.h
         SRCS angular.cpp
         DEPS concepts
 )

robocin/utility/README.md

@@ -21,6 +21,10 @@ The [angular](angular.h) header provides a set of functions to perform operation
 - `normalizeAngle`: normalize an angle to the range [-pi, pi];
 - `smallestAngleDiff`: calculate the smallest angle difference between two angles;
 - `absSmallestAngleDiff`: calculate the absolute value of the smallest angle difference between two angles;
+- `fsin`: get the sine of an angle in degrees from a lookup table (faster
+  than [`std::sin`](https://en.cppreference.com/w/cpp/numeric/math/sin), but less accurate);
+- `fcos`: get the cosine of an angle in degrees from a lookup table (faster
+  than [`std::cos`](https://en.cppreference.com/w/cpp/numeric/math/cos), but less accurate);
 
 > **Note**: As in the standard library, additional overloads are provided for all integer types, which are treated
 > as `double`.

robocin/utility/angular.h

@@ -7,30 +7,31 @@
 #define ROBOCIN_UTILITY_ANGULAR_H
 
 #include <cmath>
+#include <cstdint>
 #include <numbers>
 
 #include "robocin/utility/concepts.h"
 
+#include "robocin/utility/internal/angular_internal.h"
+
 namespace robocin {
 
 template <arithmetic T>
 constexpr auto degreesToRadians(T degrees) {
   using F = std::conditional_t<std::floating_point<T>, T, double>;
 
-  constexpr F kPi = std::numbers::pi_v<F>;
-  constexpr F kPiInDegrees = 180;
+  constexpr F kDegreesToRadiansFactor = std::numbers::pi_v<F> / 180;
 
-  return degrees * kPi / kPiInDegrees;
+  return degrees * kDegreesToRadiansFactor;
 }
 
 template <arithmetic T>
 constexpr auto radiansToDegrees(T radians) {
   using F = std::conditional_t<std::floating_point<T>, T, double>;
 
-  constexpr F kPi = std::numbers::pi_v<F>;
-  constexpr F kPiInDegrees = 180;
+  constexpr F kRadiansToDegreesFactor = 180 / std::numbers::pi_v<F>;
 
-  return radians * kPiInDegrees / kPi;
+  return radians * kRadiansToDegreesFactor;
 }
 
 template <arithmetic T>
@@ -40,6 +41,10 @@ constexpr auto normalizeAngle(T angle) {
   constexpr F kPi = std::numbers::pi_v<F>;
   constexpr F k2Pi = 2 * kPi;
 
+  if (-kPi <= angle && angle <= kPi) {
+    return static_cast<F>(angle);
+  }
+
   F result = std::fmod(static_cast<F>(angle), k2Pi);
   if (result < -kPi) {
     result += k2Pi;
@@ -61,6 +66,32 @@ constexpr auto absSmallestAngleDiff(T lhs, U rhs) {
   return std::abs(smallestAngleDiff(lhs, rhs));
 }
 
+template <arithmetic T>
+constexpr auto fsin(T radians) { // NOLINT(readability-identifier-naming)
+  using F = std::conditional_t<std::floating_point<T>, T, double>;
+
+  auto index = static_cast<int>(2 * radiansToDegrees(normalizeAngle(radians)));
+
+  // since sin is an odd function, i.e. sin(x) = -sin(-x), we can use the absolute value:
+  if (index < 0) {
+    return -angular_internal::kSinTable<F>[-index];
+  }
+  return angular_internal::kSinTable<F>[index];
+}
+
+template <arithmetic T>
+constexpr auto fcos(T radians) { // NOLINT(readability-identifier-naming)
+  using F = std::conditional_t<std::floating_point<T>, T, double>;
+
+  auto index = static_cast<int>(2 * radiansToDegrees(normalizeAngle(radians)));
+
+  // since cos is an even function, i.e. cos(x) = cos(-x), we can use the absolute value:
+  if (index < 0) {
+    return angular_internal::kCosTable<F>[-index];
+  }
+  return angular_internal::kCosTable<F>[index];
+}
+
 } // namespace robocin
 
 #endif // ROBOCIN_UTILITY_ANGULAR_H

robocin/utility/angular_test.cpp

@@ -166,5 +166,37 @@ TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesOutsidePiAndMinusPi
   EXPECT_NEAR((absSmallestAngleDiff<T, T>(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon);
 }
 
+// fsin --------------------------------------------------------------------------------------------
+TYPED_TEST(FloatingPointTest, Fsin) {
+  using T = TypeParam;
+
+  // the accuracy of standard trigonometric functions is not
+  // totally defined, and the approach to the floor or ceiling may contain
+  // inconsistencies, so we use fixed a small epsilon.
+  static constexpr T kEpsilon = 1e-2;
+
+  for (const T kDegs : std::views::iota(-360, 361)) { // from -pi to pi, stepped 0.5 degrees.
+    const T kRads = degreesToRadians(kDegs / 2);
+
+    EXPECT_NEAR(fsin(kRads), std::sin(kRads), kEpsilon);
+  }
+}
+
+// fcos --------------------------------------------------------------------------------------------
+TYPED_TEST(FloatingPointTest, Fcos) {
+  using T = TypeParam;
+
+  // the accuracy of standard trigonometric functions is not
+  // totally defined, and the approach to the floor or ceiling may contain
+  // inconsistencies, so we use fixed a small epsilon.
+  static constexpr T kEpsilon = 1e-2;
+
+  for (const T kDegs : std::views::iota(-360, 361)) { // from -pi to pi, with step of 0.5.
+    const T kRads = degreesToRadians(kDegs / 2);
+
+    EXPECT_NEAR(fcos(kRads), std::cos(kRads), kEpsilon);
+  }
+}
+
 } // namespace
 } // namespace robocin