RoboidControl-cpp/Angle.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 ANGLE_H
#define ANGLE_H

namespace Passer {
namespace LinearAlgebra {

static float pi = 3.1415927410125732421875F;

static float Rad2Deg = 360.0f / (pi * 2);
static float Deg2Rad = (pi * 2) / 360.0f;

/// @brief An angle in various representations.
/// @tparam T The internal type used for the representation of the angle.
/// The angle is internally limited to (-180..180] degrees or (-PI...PI]
/// radians. When an angle exceeds this range, it is normalized to a value
/// within the range.
template <typename T> class AngleOf {
public:
  /// @brief Create a new angle with a zero value
  AngleOf<T>();

  /// @brief An zero value angle
  const static AngleOf<T> zero;
  // const static AngleOf<T> deg90;
  // const static AngleOf<T> deg180;

  /// @brief Creates an angle in degrees
  /// @param degrees the angle in degrees
  /// @return The angle value
  static AngleOf<T> Degrees(float degrees);
  /// @brief Creates an angle in radians
  /// @param radians the angle in radians
  /// @return The angle value
  static AngleOf<T> Radians(float radians);

  /// @brief Creates an angle from a raw value
  /// @param rawValue the raw value to use for the angle
  /// @return The the angle
  static AngleOf<T> Binary(T rawValue);

  /// @brief Get the angle value in degrees
  /// @return The angle value in degrees
  float InDegrees() const;
  /// @brief Get the angle value in radians
  /// @return The angle value in radians
  float InRadians() const;

  /// @brief Get the raw value for the angle
  /// @return The raw value
  T GetBinary() const;
  /// @brief Set the raw value of the angle
  /// @param rawValue The raw value
  void SetBinary(T rawValue);

  /// @brief Tests whether this angle is equal to the given angle
  /// @param angle The angle to compare to
  /// @return True when the angles are equal, False otherwise
  /// @note The equality is determine within the limits of precision of the raw
  /// type T
  bool operator==(const AngleOf<T> angle) const;
  /// @brief Tests if this angle is greater than the given angle
  /// @param angle The given angle
  /// @return True when this angle is greater than the given angle, False
  /// otherwise
  bool operator>(AngleOf<T> angle) const;
  /// @brief Tests if this angle is greater than or equal to the given angle
  /// @param angle The given angle
  /// @return True when this angle is greater than or equal to the given angle.
  /// False otherwise.
  bool operator>=(AngleOf<T> angle) const;
  /// @brief Tests if this angle is less than the given angle
  /// @param angle The given angle
  /// @return True when this angle is less than the given angle. False
  /// otherwise.
  bool operator<(AngleOf<T> angle) const;
  /// @brief Tests if this angle is less than or equal to the given angle
  /// @param angle The given angle
  /// @return True when this angle is less than or equal to the given angle.
  /// False otherwise.
  bool operator<=(AngleOf<T> angle) const;

  /// @brief Returns the sign of the angle
  /// @param angle The angle
  /// @return -1 when the angle is negative, 1 when it is positive and 0
  /// otherwise.
  static signed int Sign(AngleOf<T> angle);
  /// @brief Returns the magnitude of the angle
  /// @param angle The angle
  /// @return The positive magitude of the angle.
  /// Negative values are negated to get a positive result
  static AngleOf<T> Abs(AngleOf<T> angle);

  /// @brief Negate the angle
  /// @return The negated angle
  AngleOf<T> operator-() const;
  /// @brief Substract another angle from this angle
  /// @param angle The angle to subtract from this angle
  /// @return The result of the subtraction
  AngleOf<T> operator-(const AngleOf<T> &angle) const;
  /// @brief Add another angle from this angle
  /// @param angle The angle to add to this angle
  /// @return The result of the addition
  AngleOf<T> operator+(const AngleOf<T> &angle) const;
  /// @brief Add another angle to this angle
  /// @param angle The angle to add to this angle
  /// @return The result of the addition
  AngleOf<T> operator+=(const AngleOf<T> &angle);

  /// @brief Mutliplies the angle
  /// @param angle The angle to multiply
  /// @param factor The factor by which the angle is multiplied
  /// @return The multiplied angle
  friend AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
    return AngleOf::Degrees((float)angle.InDegrees() * factor);
  }
  /// @brief Multiplies the angle
  /// @param factor The factor by which the angle is multiplies
  /// @param angle The angle to multiply
  /// @return The multiplied angle
  friend AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
    return AngleOf::Degrees((float)factor * angle.InDegrees());
  }

  /// @brief Normalizes the angle to (-180..180] or (-PI..PI]
  /// Should not be needed but available in case it is.
  void Normalize();
  /// @brief Normalizes the angle to (-180..180] or (-PI..PI]
  /// @param angle The angle to normalize
  /// @return The normalized angle;
  static AngleOf<T> Normalize(AngleOf<T> angle);
  /// @brief Clamps the angle value between the two given angles
  /// @param angle The angle to clamp
  /// @param min The minimum angle
  /// @param max The maximum angle
  /// @return The clamped value
  /// @remark When the min value is greater than the max value, angle is
  /// returned unclamped.
  static AngleOf<T> Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max);
  // static AngleOf<T> Difference(AngleOf<T> a, AngleOf<T> b) {
  //   AngleOf<T> r = Normalize(b.InDegrees() - a.InDegrees());
  //   return r;
  // };

  /// @brief Rotates an angle towards another angle with a max distance
  /// @param fromAngle The angle to start from
  /// @param toAngle The angle to rotate towards
  /// @param maxAngle The maximum angle to rotate
  /// @return The rotated angle
  static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
                                float maxAngle);

  /// @brief Calculates the cosine of an angle
  /// @param angle The given angle
  /// @return The cosine of the angle
  static float Cos(AngleOf<T> angle);
  /// @brief Calculates the sine of an angle
  /// @param angle The given angle
  /// @return The sine of the angle
  static float Sin(AngleOf<T> angle);
  /// @brief Calculates the tangent of an angle
  /// @param angle The given angle
  /// @return The tangent of the angle
  static float Tan(AngleOf<T> angle);

  /// @brief Calculates the arc cosine angle
  /// @param f The value
  /// @return The arc cosine for the given value
  static AngleOf<T> Acos(float f);
  /// @brief Calculates the arc sine angle
  /// @param f The value
  /// @return The arc sine for the given value
  static AngleOf<T> Asin(float f);
  /// @brief Calculates the arc tangent angle
  /// @param f The value
  /// @return The arc tangent for the given value
  static AngleOf<T> Atan(float f);
  /// @brief Calculates the tangent for the given values
  /// @param y The vertical value
  /// @param x The horizontal value
  /// @return The tanget for the given values
  /// Uses the y and x signs to compute the quadrant
  static AngleOf<T> Atan2(float y, float x);

  /// @brief Computes the length of a side using the rule of cosines
  /// @param a The length of side A
  /// @param b The length of side B
  /// @param gamma The angle of the corner opposing side C
  /// @return The length of side C
  static float CosineRuleSide(float a, float b, AngleOf<T> gamma);
  /// @brief Computes the angle of a corner using the rule of cosines
  /// @param a The length of side A
  /// @param b The length of side B
  /// @param c The length of side C
  /// @return The angle of the corner opposing side C
  static AngleOf<T> CosineRuleAngle(float a, float b, float c);

  /// @brief Computes the angle of a corner using the rule of sines
  /// @param a The length of side A
  /// @param beta the angle of the corner opposing side B
  /// @param c The length of side C
  /// @return The angle of the corner opposing side A
  static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);

private:
  T value;

  AngleOf<T>(T rawValue);
};

using AngleSingle = AngleOf<float>;
using Angle16 = AngleOf<signed short>;
using Angle8 = AngleOf<signed char>;

} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;

#endif