RoboidControl-cpp/LinearAlgebra/Polar.h

// 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 https ://mozilla.org/MPL/2.0/.

#ifndef POLAR_H
#define POLAR_H

#include "Angle.h"

namespace LinearAlgebra {

struct Vector2;
template <typename T>
class SphericalOf;

/// @brief A polar vector using an angle in various representations
/// @tparam T The implementation type used for the representation of the angle
template <typename T>
class PolarOf {
 public:
  /// @brief The distance in meters
  /// @remark The distance shall never be negative
  float distance;
  /// @brief The angle in degrees clockwise rotation
  /// @remark The angle shall be between -180 .. 180
  AngleOf<T> angle;

  /// @brief A new vector with polar coordinates with zero degrees and
  /// distance
  PolarOf();
  /// @brief A new vector with polar coordinates
  /// @param distance The distance in meters
  /// @param angle The angle in degrees, clockwise rotation
  /// @note The distance is automatically converted to a positive value.
  /// @note The angle is automatically normalized to -180 .. 180
  PolarOf(float distance, AngleOf<T> angle);

  /// @brief Create polar vector without using AngleOf type. All given angles
  /// are in degrees
  /// @param distance The distance in meters
  /// @param degrees The angle in degrees
  /// @return The polar vector
  static PolarOf<T> Degrees(float distance, float degrees);
  /// @brief Short-hand Deg alias for the Degrees function
  constexpr static auto Deg = Degrees;
  /// @brief Create polar vector without using AngleOf type. All given angles
  /// are in radians.
  /// @param distance The distance in meters
  /// @param radians The angle in radians
  /// @return The polar vector
  static PolarOf<T> Radians(float distance, float radians);
  /// @brief Short-hand Rad alias for the Radians function
  constexpr static auto Rad = Radians;

  /// @brief Convert a vector from 2D carthesian coordinates to polar
  /// coordinates
  /// @param v The vector to convert
  static PolarOf<T> FromVector2(Vector2 v);
  /// @brief Convert a vector from spherical coordinates to polar coordinates
  /// @param s The vector to convert
  /// @note The resulting vector will be projected on the horizontal plane
  static PolarOf<T> FromSpherical(SphericalOf<T> v);

  /// @brief A polar vector with zero degrees and distance
  const static PolarOf zero;
  /// @brief A normalized forward-oriented vector
  const static PolarOf forward;
  /// @brief A normalized back-oriented vector
  const static PolarOf back;
  /// @brief A normalized right-oriented vector
  const static PolarOf right;
  /// @brief A normalized left-oriented vector
  const static PolarOf left;

  /// @brief Equality test to another vector
  /// @param v The vector to check against
  /// @return true: if it is identical to the given vector
  /// @note This uses float comparison to check equality which may have
  /// strange effects. Equality on floats should be avoided.
  bool operator==(const PolarOf& v) const;

  /// @brief The vector length
  /// @param v The vector for which you need the length
  /// @return The vector length;
  inline static float Magnitude(const PolarOf& v) { return v.distance; }
  /// @brief The vector length
  /// @return The vector length
  inline float magnitude() const { return this->distance; }

  /// @brief Convert the vector to a length of 1
  /// @param v The vector to convert
  /// @return The vector normalized to a length of 1
  static PolarOf Normalize(const PolarOf& v);
  /// @brief Convert the vector to a length of a
  /// @return The vector normalized to a length of 1
  PolarOf normalized() const;

  /// @brief Negate the vector
  /// @return The negated vector
  /// This will rotate the vector by 180 degrees. Distance will stay the same.
  PolarOf operator-() const;

  /// @brief Subtract a polar vector from this vector
  /// @param v The vector to subtract
  /// @return The result of the subtraction
  PolarOf operator-(const PolarOf& v) const;
  PolarOf operator-=(const PolarOf& v);
  /// @brief Add a polar vector to this vector
  /// @param v The vector to add
  /// @return The result of the addition
  PolarOf operator+(const PolarOf& v) const;
  PolarOf operator+=(const PolarOf& v);

  /// @brief Scale the vector uniformly up
  /// @param f The scaling factor
  /// @return The scaled vector
  /// @remark This operation will scale the distance of the vector. The angle
  /// will be unaffected.
  friend PolarOf operator*(const PolarOf& v, float f) {
    return PolarOf(v.distance * f, v.angle);
  }
  friend PolarOf operator*(float f, const PolarOf& v) {
    return PolarOf(f * v.distance, v.angle);
  }
  PolarOf operator*=(float f);
  /// @brief Scale the vector uniformly down
  /// @param f The scaling factor
  /// @return The scaled factor
  /// @remark This operation will scale the distance of the vector. The angle
  /// will be unaffected.
  friend PolarOf operator/(const PolarOf& v, float f) {
    return PolarOf(v.distance / f, v.angle);
  }
  friend PolarOf operator/(float f, const PolarOf& v) {
    return PolarOf(f / v.distance, v.angle);
  }
  PolarOf operator/=(float f);

  /// @brief The distance between two vectors
  /// @param v1 The first vector
  /// @param v2 The second vector
  /// @return The distance between the two vectors
  static float Distance(const PolarOf& v1, const PolarOf& v2);

  /// @brief Rotate a vector
  /// @param v The vector to rotate
  /// @param a The angle in degreesto rotate
  /// @return The rotated vector
  static PolarOf Rotate(const PolarOf& v, AngleOf<T> a);
};

using PolarSingle = PolarOf<float>;
using Polar16 = PolarOf<signed short>;
// using Polar = PolarSingle;

}  // namespace LinearAlgebra
using namespace LinearAlgebra;

#include "Spherical.h"
#include "Vector2.h"

#endif