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 "Angle.h"
#include "Polar.h"

namespace Passer {
namespace LinearAlgebra {

struct Vector3;

template <typename T>
class SphericalOf {
 public:
  /// @brief The distance in meters
  /// @remark The distance should never be negative
  float distance;
  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
  /// @details The angle is automatically normalized to -180 .. 180
  AngleOf<T> horizontal;
  /// @brief The angle in the vertical plane in degrees. Positive is upward.
  /// @details The angle is automatically normalized to -180 .. 180
  AngleOf<T> vertical;

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

  static SphericalOf<T> FromPolar(Polar v);

  static SphericalOf<T> FromVector3(Vector3 v);
  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 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);
};

using SphericalSingle = SphericalOf<float>;
using Spherical16 = SphericalOf<signed short>;

/// @brief A spherical vector
/// @details This is a vector in 3D space using a spherical coordinate system.
/// It consists of a distance and the polar and elevation angles from a
/// reference direction. The reference direction is typically thought of
/// as a forward direction.
struct Spherical {
 public:
  /// @brief The distance in meters
  /// @remark The distance should never be negative
  float distance;
  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
  /// @details The angle is automatically normalized to -180 .. 180
  Angle horizontalAngle;
  /// @brief The angle in the vertical plane in degrees. Positive is upward.
  /// @details The angle is automatically normalized to -180 .. 180
  Angle verticalAngle;

  /// @brief Create a new spherical vector with zero degrees and distance
  Spherical();
  /// @brief Create a new spherical vector
  /// @param distance The distance in meters
  /// @param horizontalAngle The angle in the horizontal plane in degrees,
  /// clockwise rotation
  /// @param verticalAngle The angle in the vertical plan in degrees,
  /// zero is forward, positive is upward
  Spherical(float distance, Angle horizontalAngle, Angle verticalAngle);
  /// @brief Convert polar coordinates to spherical coordinates
  /// @param polar The polar coordinate
  Spherical(Polar polar);
  /// @brief Convert 3D carthesian coordinates to spherical coordinates
  /// @param v Vector in 3D carthesian coordinates;
  Spherical(Vector3 v);

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

  /// @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 Spherical& 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 Spherical& 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 Spherical Normalize(const Spherical& v);
  /// @brief Convert the vector to a length of a
  /// @return The vector normalized to a length of 1
  Spherical normalized() const;

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

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

  /// <summary>
  /// The distance between two vectors
  /// </summary>
  /// <param name="v1">The first vector</param>
  /// <param name="v2">The second vector</param>
  /// <returns>The distance between the two vectors</returns>
  // static float Distance(const Spherical &s1, const Spherical &s2);

  static Angle AngleBetween(const Spherical& v1, const Spherical& v2);

  static Spherical Rotate(const Spherical& v,
                          Angle horizontalAngle,
                          Angle verticalAngle);
  static Spherical RotateHorizontal(const Spherical& v, Angle angle);
  static Spherical RotateVertical(const Spherical& v, Angle angle);
};

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

#include "Vector3.h"

#endif