RoboidControl-cpp/Spherical16.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 SPHERICAL16_H
#define SPHERICAL16_H

#include "Angle16.h"
#include "Polar.h"

namespace Passer {
namespace LinearAlgebra {

struct Vector3;

/// @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.
/// In contrast to the normal Spherical type, this version uses 16 bit integers
/// for the angles
struct Spherical16 {
 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
  Angle16 horizontalAngle;
  /// @brief The angle in the vertical plane in degrees. Positive is upward.
  /// @details The angle is automatically normalized to -180 .. 180
  Angle16 verticalAngle;

  /// @brief Create a new spherical vector with zero degrees and distance
  Spherical16();
  /// @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
  Spherical16(float distance, Angle16 horizontalAngle, Angle16 verticalAngle);
  /// @brief Convert polar coordinates to spherical coordinates
  /// @param polar The polar coordinate
  Spherical16(Polar polar);
  /// @brief Convert 3D carthesian coordinates to spherical coordinates
  /// @param v Vector in 3D carthesian coordinates;
  Spherical16(Vector3 v);

  Vector3 ToVector3();

  /// @brief A spherical vector with zero degree angles and distance
  const static Spherical16 zero;
  /// @brief A normalized forward-oriented vector
  const static Spherical16 forward;
  /// @brief A normalized back-oriented vector
  const static Spherical16 back;
  /// @brief A normalized right-oriented vector
  const static Spherical16 right;
  /// @brief A normalized left-oriented vector
  const static Spherical16 left;
  /// @brief A normalized up-oriented vector
  const static Spherical16 up;
  /// @brief A normalized down-oriented vector
  const static Spherical16 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 Spherical16& 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 Spherical16& 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 Spherical16 Normalize(const Spherical16& v);
  /// @brief Convert the vector to a length of a
  /// @return The vector normalized to a length of 1
  Spherical16 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.
  Spherical16 operator-() const;

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

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

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

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

#include "Vector3.h"

#endif
*/