RoboidControl-cpp/Spherical.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 SPHERICAL_H
#define SPHERICAL_H

#include "Direction.h"

namespace LinearAlgebra {

struct Vector3;
template <typename T>
class PolarOf;

/// @brief A spherical vector using angles in various representations
/// @tparam T The implementation type used for the representations of the agles
template <typename T>
class SphericalOf {
 public:
  /// @brief The distance in meters
  /// @remark The distance should never be negative
  float distance;
  /// @brief The direction of the vector
  DirectionOf<T> direction;

  SphericalOf();
  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
  SphericalOf(float distance, DirectionOf<T> direction);

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

  /// @brief Create a Spherical coordinate from a Polar coordinate
  /// @param v The polar coordinate
  /// @return The spherical coordinate with the vertical angle set to zero.
  static SphericalOf<T> FromPolar(PolarOf<T> v);

  /// @brief Create a Spherical coordinate from a Vector3 coordinate
  /// @param v The vector coordinate
  /// @return The spherical coordinate
  static SphericalOf<T> FromVector3(Vector3 v);
  /// @brief Convert the spherical coordinate to a Vector3 coordinate
  /// @return The vector coordinate
  Vector3 ToVector3() const;

  /// @brief A spherical vector with zero degree angles and distance
  const static SphericalOf<T> zero;
  /// @brief A normalized forward-oriented vector
  const static SphericalOf<T> forward;
  /// @brief A normalized back-oriented vector
  const static SphericalOf<T> back;
  /// @brief A normalized right-oriented vector
  const static SphericalOf<T> right;
  /// @brief A normalized left-oriented vector
  const static SphericalOf<T> left;
  /// @brief A normalized up-oriented vector
  const static SphericalOf<T> up;
  /// @brief A normalized down-oriented vector
  const static SphericalOf<T> down;

  /// @brief Update the distance component of the spherical coordinate
  /// @param distance The new distance
  /// @return The updated coordinate
  SphericalOf<T> WithDistance(float distance);

  /// @brief Negate the vector
  /// @return The negated vector
  /// This will rotate the vector by 180 degrees horizontally and
  /// vertically. Distance will stay the same.
  SphericalOf<T> operator-() const;

  /// @brief Subtract a spherical vector from this vector
  /// @param v The vector to subtract
  /// @return The result of the subtraction
  SphericalOf<T> operator-(const SphericalOf<T>& v) const;
  SphericalOf<T> operator-=(const SphericalOf<T>& v);
  /// @brief Add a spherical vector to this vector
  /// @param v The vector to add
  /// @return The result of the addition
  SphericalOf<T> operator+(const SphericalOf<T>& v) const;
  SphericalOf<T> operator+=(const SphericalOf<T>& 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 SphericalOf<T> operator*(const SphericalOf<T>& v, float f) {
    return SphericalOf<T>(v.distance * f, v.direction);
  }
  friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) {
    return SphericalOf<T>(f * v.distance, v.direction);
  }
  SphericalOf<T> 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 SphericalOf<T> operator/(const SphericalOf<T>& v, float f) {
    return SphericalOf<T>(v.distance / f, v.direction);
  }
  friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) {
    return SphericalOf<T>(f / v.distance, v.direction);
  }
  SphericalOf<T> operator/=(float f);

  /// @brief Calculate the distance between two spherical coordinates
  /// @param v1 The first coordinate
  /// @param v2 The second coordinate
  /// @return The distance between the coordinates in meters
  static float DistanceBetween(const SphericalOf<T>& v1,
                               const SphericalOf<T>& v2);
  /// @brief Calculate the unsigned angle between two spherical vectors
  /// @param v1 The first vector
  /// @param v2 The second vector
  /// @return The unsigned angle between the vectors
  static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
                                 const SphericalOf<T>& v2);
  /// @brief Calculate the signed angle between two spherical vectors
  /// @param v1 The first vector
  /// @param v2 The second vector
  /// @param axis The axis are which the angle is calculated
  /// @return The signed angle between the vectors
  static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
                                       const SphericalOf<T>& v2,
                                       const SphericalOf<T>& axis);

  /// @brief Rotate a spherical vector
  /// @param v The vector to rotate
  /// @param horizontalAngle The horizontal rotation angle in local space
  /// @param verticalAngle The vertical rotation angle in local space
  /// @return The rotated vector
  static SphericalOf<T> Rotate(const SphericalOf& v,
                               AngleOf<T> horizontalAngle,
                               AngleOf<T> verticalAngle);
  /// @brief Rotate a spherical vector horizontally
  /// @param v The vector to rotate
  /// @param angle The horizontal rotation angle in local space
  /// @return The rotated vector
  static SphericalOf<T> RotateHorizontal(const SphericalOf<T>& v,
                                         AngleOf<T> angle);
  /// @brief Rotate a spherical vector vertically
  /// @param v The vector to rotate
  /// @param angle The vertical rotation angle in local space
  /// @return The rotated vector
  static SphericalOf<T> RotateVertical(const SphericalOf<T>& v,
                                       AngleOf<T> angle);
};

/// @brief Shorthand notation for a spherical vector using single precision
/// floats for the angles This is the fastest implementation on devices with
/// floating point harware
using SphericalSingle = SphericalOf<float>;
/// @brief Shorthand notation for a spherical vector using signed 16-bit words
/// for the angles
/// @note This is the fastest implementation on devices without floating point
/// hardware
using Spherical16 = SphericalOf<signed short>;

#if defined(ARDUINO)
using Spherical = Spherical16;
#else
using Spherical = SphericalSingle;
#endif

}  // namespace LinearAlgebra

#include "Polar.h"
#include "Vector3.h"

#endif