Merge branch 'main' into fix/formatting

common/cpp/robocin/geometry/CMakeLists.txt

@@ -2,6 +2,8 @@ robocin_cpp_library(
   NAME geometry
   HDRS point2d.h
        point3d.h
+       mathematics.h
   SRCS point2d.cpp
        point3d.cpp
+       mathematics.cpp
 )

common/cpp/robocin/geometry/mathematics.cpp

@@ -0,0 +1 @@
+#include "robocin/geometry/mathematics.h"

common/cpp/robocin/geometry/mathematics.h

@@ -0,0 +1,46 @@
+#ifndef ROBOCIN_MATHEMATICS_H
+#define ROBOCIN_MATHEMATICS_H
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+#include <type_traits>
+
+namespace mathematics {
+/*!
+ * @tparam T arithmetic type.
+ * @return Returns true if T is float and the absolute value of f is within 0.00001f of 0.0.
+ * @return Returns true if T is double and the absolute value of d is within 0.000000000001 of
+ * 0.0.
+ * @return Returns true if T is an integer type and equals to 0.
+ */
+template <class T>
+[[maybe_unused]] constexpr bool isNull(const T& value) noexcept {
+  if constexpr (std::is_floating_point_v<T>) {
+    return (fabsf(value - 1.0F) < 0.00001F);
+  } else {
+    return value == 0;
+  }
+}
+
+/*!
+ * @tparam T arithmetic type.
+ * @param value value to map.
+ * @param l_lower lower value from l-range.
+ * @param l_higher higher value from l-range.
+ * @param r_lower lower value from r-range.
+ * @param r_higher higher value from r-range.
+ * @return Returns value mapped from l-range to r-range.
+ */
+template <class T>
+[[maybe_unused]] constexpr T
+map(const T& value, const T& l_lower, const T& l_higher, const T& r_lower, const T& r_higher) {
+  if (isNull(l_higher - l_lower)) {
+    throw std::runtime_error("'l_lower' equals to 'l_higher'.");
+  }
+  return (value - l_lower) * (r_higher - r_lower) / (l_higher - l_lower) + r_lower;
+}
+
+} // namespace mathematics
+
+#endif // ROBOCIN_MATHEMATICS_H

common/cpp/robocin/utility/CMakeLists.txt

@@ -54,3 +54,17 @@ robocin_cpp_test(
   SRCS fuzzy_compare_test.cpp
   DEPS utility
 )
+
+robocin_cpp_library(
+  NAME angular
+  HDRS angular.h
+  SRCS angular.cpp
+  DEPS utility
+)
+
+robocin_cpp_test(
+  NAME angular_test
+  HDRS internal/test/epsilon_injector.h
+  SRCS angular_test.cpp
+  DEPS angular
+)

common/cpp/robocin/utility/angular.cpp

@@ -0,0 +1 @@
+#include "robocin/utility/angular.h"

common/cpp/robocin/utility/angular.h

@@ -0,0 +1,63 @@
+#ifndef ROBOCIN_UTILITY_ANGULAR_H
+#define ROBOCIN_UTILITY_ANGULAR_H
+
+#include "robocin/utility/concepts.h"
+
+#include <cmath>
+#include <numbers>
+
+namespace robocin {
+
+template <arithmetic T>
+constexpr auto degreesToRadians(T degrees) {
+  using F = std::conditional_t<std::floating_point<T>, T, double>;
+
+  constexpr F kDegreesToRadiansFactor = std::numbers::pi_v<F> / 180;
+
+  return degrees * kDegreesToRadiansFactor;
+}
+
+template <arithmetic T>
+constexpr auto radiansToDegrees(T radians) {
+  using F = std::conditional_t<std::floating_point<T>, T, double>;
+
+  constexpr F kRadiansToDegreesFactor = 180 / std::numbers::pi_v<F>;
+
+  return radians * kRadiansToDegreesFactor;
+}
+
+template <arithmetic T>
+constexpr auto normalizeAngle(T angle) {
+  using F = std::conditional_t<std::floating_point<T>, T, double>;
+
+  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;
+  } else if (result > kPi) {
+    result -= k2Pi;
+  }
+  return result;
+}
+
+template <arithmetic T, arithmetic U>
+constexpr auto smallestAngleDiff(T lhs, U rhs) {
+  using F = std::conditional_t<std::floating_point<std::common_type_t<T, U>>, T, double>;
+
+  return normalizeAngle<F>(rhs - lhs);
+}
+
+template <arithmetic T, arithmetic U>
+constexpr auto absSmallestAngleDiff(T lhs, U rhs) {
+  return std::abs(smallestAngleDiff(lhs, rhs));
+}
+
+} // namespace robocin
+
+#endif // ROBOCIN_UTILITY_ANGULAR_H

common/cpp/robocin/utility/angular_test.cpp

@@ -0,0 +1,156 @@
+#include "robocin/utility/angular.h"
+#include "robocin/utility/internal/test/epsilon_injector.h"
+
+#include <gtest/gtest.h>
+#include <numbers>
+#include <ranges>
+
+namespace robocin {
+
+using ::testing::Test;
+using ::testing::Types;
+
+using FloatingPointTestTypes = Types<float, double, long double>;
+
+template <class>
+class FloatingPointTest : public Test {};
+TYPED_TEST_SUITE(FloatingPointTest, FloatingPointTestTypes);
+
+TYPED_TEST(FloatingPointTest, DegreesToRadiansGivenPiFractions) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+  static constexpr T kPiInDegrees = 180;
+
+  for (const T kRatio : std::views::iota(0, 7)) {
+    EXPECT_NEAR(degreesToRadians<T>(kRatio * kPiInDegrees), kRatio * kPi, kEpsilon);
+  }
+}
+
+TYPED_TEST(FloatingPointTest, RadiansToDegreesGivenPiFractions) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+  static constexpr T kPiInDegrees = 180;
+
+  for (const T kRatio : std::views::iota(0, 7)) {
+    EXPECT_NEAR(radiansToDegrees<T>(kRatio * kPi), kRatio * kPiInDegrees, kEpsilon);
+  }
+}
+
+TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesBetweenPiAndMinusPi) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  EXPECT_NEAR(normalizeAngle<T>(0.0), 0.0, kEpsilon);           // 0.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(kPi / 2), kPi / 2, kEpsilon);   // 90.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-kPi / 2), -kPi / 2, kEpsilon); // -90.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(kPi), kPi, kEpsilon);           // 180.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-kPi), -kPi, kEpsilon);         // -180.0 degrees
+}
+
+TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesGreaterThanPi) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  EXPECT_NEAR(normalizeAngle<T>(5 * kPi / 4), -3 * kPi / 4, kEpsilon); // 225.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-5 * kPi / 4), 3 * kPi / 4, kEpsilon); // -225.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(3 * kPi / 2), -kPi / 2, kEpsilon);     // 270.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-3 * kPi / 2), kPi / 2, kEpsilon);     // -270.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(7 * kPi / 3), kPi / 3, kEpsilon);      // 420.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-7 * kPi / 3), -kPi / 3, kEpsilon);    // -420.0 degrees
+}
+
+TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesLessThanMinusPi) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  EXPECT_NEAR(normalizeAngle<T>(-9 * kPi / 4), -kPi / 4, kEpsilon); // -405.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(9 * kPi / 4), kPi / 4, kEpsilon);   // 405.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-5 * kPi / 2), -kPi / 2, kEpsilon); // -450.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(5 * kPi / 2), kPi / 2, kEpsilon);   // 450.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(-11 * kPi / 3), kPi / 3, kEpsilon); // -660.0 degrees
+  EXPECT_NEAR(normalizeAngle<T>(11 * kPi / 3), -kPi / 3, kEpsilon); // 660.0 degrees
+}
+
+TYPED_TEST(FloatingPointTest, SmallestAngleDiffGivenAnglesBetweenPiAndMinusPiRange) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  // 0.0 and 90.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(0, kPi / 2)), kPi / 2, kEpsilon);
+  // 90.0 and 0.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(kPi / 2, 0)), -kPi / 2, kEpsilon);
+  // -45.0 and 45.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(-kPi / 4, kPi / 4)), kPi / 2, kEpsilon);
+  // 45.0 and -45.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(kPi / 4, -kPi / 4)), -kPi / 2, kEpsilon);
+  // -45.0 and 135.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(-kPi / 4, kPi / 2)), 3 * kPi / 4, kEpsilon);
+  // 135.0 and -45.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(kPi / 2, -kPi / 4)), -3 * kPi / 4, kEpsilon);
+}
+
+TYPED_TEST(FloatingPointTest, SmallestAngleDiffGivenAnglesOutsidePiAndMinusPiRange) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  // -450.0 to 360.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(-5 * kPi / 2, 2 * kPi)), kPi / 2, kEpsilon);
+  // 360.0 to -450.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(2 * kPi, -5 * kPi / 2)), -kPi / 2, kEpsilon);
+  // -450.0 to 270.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(-5 * kPi / 2, 3 * kPi / 2)), 0.0, kEpsilon);
+  // -450.0 to 270.0 degrees
+  EXPECT_NEAR((smallestAngleDiff<T, T>(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon);
+}
+
+TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesBetweenPiAndMinusPiRange) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  // 0.0 and 90.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(0, kPi / 2)), kPi / 2, kEpsilon);
+  // 90.0 and 0.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(kPi / 2, 0)), kPi / 2, kEpsilon);
+  // -45.0 and 45.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(-kPi / 4, kPi / 4)), kPi / 2, kEpsilon);
+  // 45.0 and -45.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(kPi / 4, -kPi / 4)), kPi / 2, kEpsilon);
+  // -45.0 and 135.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(-kPi / 4, kPi / 2)), 3 * kPi / 4, kEpsilon);
+  // 135.0 and -45.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(kPi / 2, -kPi / 4)), 3 * kPi / 4, kEpsilon);
+}
+
+TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesOutsidePiAndMinusPiRange) {
+  using T = TypeParam;
+
+  static constexpr T kEpsilon = epsilon_v<T>;
+  static constexpr T kPi = std::numbers::pi_v<T>;
+
+  // -450.0 to 360.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(-5 * kPi / 2, 2 * kPi)), kPi / 2, kEpsilon);
+  // 360.0 to -450.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(2 * kPi, -5 * kPi / 2)), kPi / 2, kEpsilon);
+  // -450.0 to 270.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(-5 * kPi / 2, 3 * kPi / 2)), 0.0, kEpsilon);
+  // -450.0 to 270.0 degrees
+  EXPECT_NEAR((absSmallestAngleDiff<T, T>(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon);
+}
+
+} // namespace robocin

common/cpp/robocin/utility/iproto_convertible.h

@@ -15,8 +15,9 @@ class IProtoConvertible {
 
   virtual ~IProtoConvertible() = default;
 
+
   virtual T toProto() const = 0;
-  virtual void fromProto(T proto) = 0;
+  virtual void fromProto(const T& proto) = 0;
 };
 
 } // namespace robocin

decision-ms/decision-guardiola/decision/processing/CMakeLists.txt

@@ -3,4 +3,5 @@ robocin_cpp_library(
     HDRS decision_processor.h
     SRCS decision_processor.cpp
     DEPS common::output
+         messages
 )

decision-ms/decision-guardiola/decision/processing/decision_processor.cpp

@@ -1,21 +1,24 @@
 #include "decision/processing/decision_processor.h"
 
 #include "decision/messaging/receiver/payload.h"
+#include "decision/processing/entities/world.h"
+#include "decision/processing/messages/behavior/behavior_message.h"
+#include "decision/processing/messages/decision/decision_message.h"
+#include "decision/processing/messages/perception/detection/detection_message.h"
 
 #include <protocols/common/robot_id.pb.h>
 #include <protocols/decision/decision.pb.h>
 #include <protocols/perception/detection.pb.h>
 #include <protocols/referee/game_status.pb.h>
 #include <ranges>
+#include <robocin/output/log.h>
 
 namespace decision {
 
 namespace parameters = ::robocin::parameters;
 
 namespace {
 
-using ::robocin::ilog;
-
 namespace rc {
 
 using ::protocols::common::RobotId;
@@ -65,25 +68,7 @@ std::optional<rc::Decision> DecisionProcessor::process(std::span<const Payload>
 
   const rc::Detection last_detection = detections.back();
 
-  ///////////////////////////////////////////////////////////////////////////////////
-
-  // TODO: Implement the logic to generate the decision based on the last detection and the last
-  // game status.
-  for (const auto& robot : last_detection.robots()) {
-    rc::Behavior* behavior = decision_output.add_behavior();
-
-    behavior->set_id(0);
-    behavior->mutable_robot_id()->CopyFrom(robot.robot_id());
-  }
-
-  rc::TacticalPlan* tplan = decision_output.mutable_plan();
-
-  rc::OffensivePlan* ofPlan = tplan->mutable_offensive();
-  rc::DefensivePlan* dfPlan = tplan->mutable_defensive();
-
-  ///////////////////////////////////////////////////////////////////////////////////
-
-  return decision_output;
+  return rc::Decision{};
 }
 
 } // namespace decision

decision-ms/decision-guardiola/decision/processing/entities/CMakeLists.txt

@@ -0,0 +1,5 @@
+robocin_cpp_library(
+  NAME entities
+  HDRS world.h
+  SRCS world.cpp
+)