Initial commit

include/okapi/api/odometry/odomMath.hpp

@@ -0,0 +1,95 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/odometry/odomState.hpp"
+#include "okapi/api/odometry/point.hpp"
+#include "okapi/api/util/logging.hpp"
+#include <tuple>
+
+namespace okapi {
+class OdomMath {
+  public:
+  /**
+   * Computes the distance from the given Odometry state to the given point. The point and the
+   * OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The distance between the Odometry state and the point.
+   */
+  static QLength computeDistanceToPoint(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the angle from the given Odometry state to the given point. The point and the
+   * OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The angle between the Odometry state and the point.
+   */
+  static QAngle computeAngleToPoint(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the distance and angle from the given Odometry state to the given point. The point and
+   * the OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The distance and angle between the Odometry state and the point.
+   */
+  static std::pair<QLength, QAngle> computeDistanceAndAngleToPoint(const Point &ipoint,
+                                                                   const OdomState &istate);
+
+  /**
+   * Constraints the angle to [0,360] degrees.
+   *
+   * @param angle The input angle.
+   * @return The angle normalized to [0,360] degrees.
+   */
+  static QAngle constrainAngle360(const QAngle &angle);
+
+  /**
+   * Constraints the angle to [-180,180) degrees.
+   *
+   * @param angle The input angle.
+   * @return The angle normalized to [-180,180) degrees.
+   */
+  static QAngle constrainAngle180(const QAngle &angle);
+
+  private:
+  OdomMath();
+  ~OdomMath();
+
+  /**
+   * Computes the x and y diffs in meters between the points.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The diffs in the order `{xDiff, yDiff}`.
+   */
+  static std::pair<double, double> computeDiffs(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the distance between the points.
+   *
+   * @param xDiff The x-axis diff in meters.
+   * @param yDiff The y-axis diff in meters.
+   * @return The cartesian distance in meters.
+   */
+  static double computeDistance(double xDiff, double yDiff);
+
+  /**
+   * Compites the angle between the points.
+   *
+   * @param xDiff The x-axis diff in meters.
+   * @param yDiff The y-axis diff in meters.
+   * @param theta The current robot's theta in radians.
+   * @return The angle in radians.
+   */
+  static double computeAngle(double xDiff, double yDiff, double theta);
+};
+} // namespace okapi