Feat/ball analyzer methods (#93)

This PR adds helper functions used throughout the code to derive ball
information.

common/cpp/robocin/geometry/mathematics.h

@@ -41,6 +41,63 @@ map(const T& value, const T& l_lower, const T& l_higher, const T& r_lower, const
   return (value - l_lower) * (r_higher - r_lower) / (l_higher - l_lower) + r_lower;
 }
 
+static constexpr bool isNullable(double f) { return isNull(f) or not std::isnormal(f); }
+
+/*!
+ * @tparam PT Requires '.x' and '.y' members and '.cross()' member function.
+ * @param a, b, c, d lines (a, b) and (c, d).
+ * @return Determines if lines through (a, b) and (c, d) are parallel.
+ */
+template <class PT>
+constexpr bool linesParallel(const PT& a, const PT& b, const PT& c, const PT& d) {
+  return isNullable((b - a).cross(c - d));
+}
+/*!
+ * @tparam PT Requires '.x' and '.y' members and '.cross()' member function.
+ * @param a, b, c, d lines (a, b) and (c, d).
+ * @return Determines if lines through (a, b) and (c, d) are collinear.
+ */
+template <class PT>
+constexpr bool linesCollinear(const PT& a, const PT& b, const PT& c, const PT& d) {
+  if (!linesParallel<PT>(a, b, c, d)) {
+    return false;
+  }
+  if (!isNullable((a - b).cross(a - c))) {
+    return false;
+  }
+  if (!isNullable((c - d).cross(c - a))) {
+    return false;
+  }
+  return true;
+}
+
+/*!
+ * @tparam PT Requires '.x' and '.y' members and '.cross()', '.distanceSquaredTo()' and '.dot()'
+ * member functions.
+ * @param a, b, c, d lines (a, b) and (c, d).
+ * @return Determines if line segment from a to b intersects with line segment from c to d.
+ */
+template <class PT>
+constexpr bool segmentsIntersect(const PT& a, const PT& b, const PT& c, const PT& d) {
+  if (linesCollinear<PT>(a, b, c, d)) {
+    if (isNullable(a.distanceSquaredTo(c)) || isNullable(a.distanceSquaredTo(d))
+        || isNullable(b.distanceSquaredTo(c)) || isNullable(b.distanceSquaredTo(d))) {
+      return true;
+    }
+    if ((c - a).dot(c - b) > 0 && (d - a).dot(d - b) > 0 && (c - b).dot(d - b) > 0) {
+      return false;
+    }
+    return true;
+  }
+  if ((d - a).cross(b - a) * (c - a).cross(b - a) > 0) {
+    return false;
+  }
+  if ((a - c).cross(d - c) * (b - c).cross(d - c) > 0) {
+    return false;
+  }
+  return true;
+}
+
 } // namespace mathematics
 
 #endif // ROBOCIN_MATHEMATICS_H

decision-ms/decision-guardiola/decision/parameters/parameters.h

@@ -14,6 +14,9 @@ constinit const auto pDecisionPollerTimeoutMs
 constinit const auto pAllyColor = decision::RobotIdMessage::COLOR_YELLOW;
 
 constinit const auto isAttackingToRight = ::robocin::parameters::View<1>::asBool(true);
+
+constinit const auto pBallIsMovingVelocity = ::robocin::parameters::View<1>::asInt32(300);
+constinit const auto pBallIsMovingFastVelocity = ::robocin::parameters::View<1>::asInt32(850);
 // NOLINTEND(*comment*)
 } // namespace decision
 

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

@@ -1,6 +1,8 @@
 robocin_cpp_library(
   NAME analyzers
   HDRS field_analyzer.h
+       ball_analyzer.h
   SRCS field_analyzer.cpp
+       ball_analyzer.cpp
   DEPS common::geometry
 )

decision-ms/decision-guardiola/decision/processing/analyzer/ball_analyzer.cpp

@@ -0,0 +1,108 @@
+#include "decision/processing/analyzer/ball_analyzer.h"
+
+#include "decision/parameters/parameters.h"
+
+#include <robocin/geometry/mathematics.h>
+#include <robocin/geometry/point2d.h>
+
+namespace decision {
+
+bool BallAnalyzer::isBallStopped(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() < pBallIsMovingVelocity();
+}
+
+bool BallAnalyzer::isBallMovingFast(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > pBallIsMovingFastVelocity();
+}
+
+bool BallAnalyzer::isBallMoving(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > pBallIsMovingVelocity();
+}
+
+bool BallAnalyzer::isBallMovingSlowly(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > pBallIsMovingVelocity()
+         && ball_velocity.length() < pBallIsMovingFastVelocity();
+}
+
+bool BallAnalyzer::isBallMovingWithVelocity(double min_velocity, BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > min_velocity;
+}
+
+bool BallAnalyzer::isBallMovingTowards(const robocin::Point2Df& target,
+                                       BallMessage& ball,
+                                       double max_angle_difference) {
+  if (!isBallMoving(ball)) {
+    return false;
+  }
+
+  robocin::Point2Df ball_position = robocin::Point2Df{ball.position->x, ball.position->y};
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  robocin::Point2Df ball_to_target_vector_direction = (target - ball_position).normalized();
+
+  double angle_difference
+      = std::abs(ball_to_target_vector_direction.angleTo(ball_velocity.normalized()));
+  return angle_difference < max_angle_difference;
+}
+
+bool BallAnalyzer::isBallMovingAway(const robocin::Point2Df& target, BallMessage& ball) {
+  bool is_ball_moving_away_from_target = isBallMoving(ball) && !isBallMovingTowards(target, ball);
+  return is_ball_moving_away_from_target;
+}
+
+bool BallAnalyzer::isBallMovingAwayWithVelocity(const robocin::Point2Df& target,
+                                                double velocity,
+                                                BallMessage& ball) {
+  bool is_ball_moving_away_with_velocity
+      = isBallMovingWithVelocity(velocity, ball) && !isBallMovingTowards(target, ball);
+  return is_ball_moving_away_with_velocity;
+}
+
+bool BallAnalyzer::isBallMovingToOffensiveGoal(const FieldMessage& field, BallMessage& ball) {
+  if (isBallStopped(ball)) {
+    return false;
+  }
+
+  robocin::Point2Df ball_position = robocin::Point2Df{ball.position->x, ball.position->y};
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  robocin::Point2Df offset_from_goal_center = robocin::Point2Df{0.0, *(field.width) / 2};
+
+  bool moving_to_field_bottom_line
+      = mathematics::segmentsIntersect(field.enemyGoalOutsideCenter() + offset_from_goal_center,
+                                       field.enemyGoalOutsideCenter() - offset_from_goal_center,
+                                       ball_position,
+                                       ball_position + ball_velocity.resized(*(field.length)));
+  return moving_to_field_bottom_line;
+}
+
+bool BallAnalyzer::isBallMovingToDefensiveGoal(const FieldMessage& field, BallMessage& ball) {
+  if (isBallStopped(ball)) {
+    return false;
+  }
+
+  robocin::Point2Df ball_position = robocin::Point2Df{ball.position->x, ball.position->y};
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  float offset_y = (*field.penalty_area_width) * sqrt(2) / 2;
+  robocin::Point2Df offset_from_goal_center = {0, offset_y};
+  bool is_moving_to_defensive_goal
+      = mathematics::segmentsIntersect(field.allyGoalOutsideCenter() + offset_from_goal_center,
+                                       field.allyGoalOutsideCenter() - offset_from_goal_center,
+                                       ball_position,
+                                       ball_position + ball_velocity.resized(*(field.length)));
+  return is_moving_to_defensive_goal;
+}
+
+bool BallAnalyzer::isBallMovingToEnemySide(const FieldMessage& field, BallMessage& ball) {
+  if (isBallStopped(ball)) {
+    return false;
+  }
+
+  bool is_moving_to_enemy_side = (ball.velocity->x * field.attackDirection().x) > 0;
+  return is_moving_to_enemy_side;
+}
+
+} // namespace decision

decision-ms/decision-guardiola/decision/processing/analyzer/ball_analyzer.h

@@ -0,0 +1,33 @@
+#ifndef DECISION_PROCESSING_ANALYZER_BALL_ANALYZER_H
+#define DECISION_PROCESSING_ANALYZER_BALL_ANALYZER_H
+
+#include "decision/processing/messages/perception/ball/ball_message.h"
+#include "decision/processing/messages/perception/field/field_message.h"
+
+#include <robocin/geometry/point2d.h>
+
+namespace decision {
+
+class BallAnalyzer {
+ public:
+  BallAnalyzer();
+
+  static bool isBallStopped(BallMessage& ball);
+  static bool isBallMovingFast(BallMessage& ball);
+  static bool isBallMoving(BallMessage& ball);
+  static bool isBallMovingSlowly(BallMessage& ball);
+  static bool isBallMovingWithVelocity(double min_velocity, BallMessage& ball);
+  static bool isBallMovingTowards(const robocin::Point2Df& target,
+                                  BallMessage& ball,
+                                  double max_angle_difference = 1.047);
+  static bool isBallMovingAway(const robocin::Point2Df& target, BallMessage& ball);
+  static bool
+  isBallMovingAwayWithVelocity(const robocin::Point2Df& target, double velocity, BallMessage& ball);
+  static bool isBallMovingToOffensiveGoal(const FieldMessage& field, BallMessage& ball);
+  static bool isBallMovingToDefensiveGoal(const FieldMessage& field, BallMessage& ball);
+  static bool isBallMovingToEnemySide(const FieldMessage& field, BallMessage& ball);
+};
+
+} // namespace decision
+
+#endif /* DECISION_PROCESSING_ANALYZER_BALL_ANALYZER_H */