RoboidControl-cpp/Quaternion.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 QUATERNION_H
#define QUATERNION_H

#include "Vector3.h"

extern "C" {
/// <summary>
/// A quaternion (C-style)
/// </summary>
/// This is a C-style implementation
typedef struct Quat {
  /// <summary>
  /// The x component
  /// </summary>
  float x;
  /// <summary>
  /// The y component
  /// </summary>
  float y;
  /// <summary>
  /// The z component
  /// </summary>
  float z;
  /// <summary>
  /// The w component
  /// </summary>
  float w;
} Quat;
}

namespace LinearAlgebra {

class Matrix2;

/// <summary>
/// A quaternion
/// </summary>
struct Quaternion : Quat {
 public:
  /// <summary>
  /// Create a new identity quaternion
  /// </summary>
  Quaternion();
  /// <summary>
  /// create a new quaternion with the given values
  /// </summary>
  /// <param name="_x">x component</param>
  /// <param name="_y">y component</param>
  /// <param name="_z">z component</param>
  /// <param name="_w">w component</param>
  Quaternion(float _x, float _y, float _z, float _w);
  /// <summary>
  /// Create a quaternion from C-style Quat
  /// </summary>
  /// <param name="q"></param>
  Quaternion(Quat q);
  /// <summary>
  /// Quaternion destructor
  /// </summary>
  ~Quaternion();

  /// <summary>
  /// An identity quaternion
  /// </summary>
  const static Quaternion identity;

  /// <summary>
  /// Convert to unit quaternion
  /// </summary>
  /// This will preserve the orientation,
  /// but ensures that it is a unit quaternion.
  void Normalize();
  /// <summary>
  /// Convert to unity quaternion
  /// </summary>
  /// <param name="q">The quaternion to convert</param>
  /// <returns>A unit quaternion</returns>
  /// This will preserve the orientation,
  /// but ensures that it is a unit quaternion.
  static Quaternion Normalize(const Quaternion& q);

  /// <summary>
  /// Convert to euler angles
  /// </summary>
  /// <param name="q">The quaternion to convert</param>
  /// <returns>A vector containing euler angles</returns>
  /// The euler angles performed in the order: Z, X, Y
  static Vector3 ToAngles(const Quaternion& q);

  Matrix2 ToRotationMatrix();

  /// <summary>
  /// Rotate a vector using this quaterion
  /// </summary>
  /// <param name="vector">The vector to rotate</param>
  /// <returns>The rotated vector</returns>
  Vector3 operator*(const Vector3& vector) const;
  /// <summary>
  /// Multiply this quaternion with another quaternion
  /// </summary>
  /// <param name="rotation">The quaternion to multiply with</param>
  /// <returns>The resulting rotation</returns>
  /// The result will be this quaternion rotated according to
  /// the give rotation.
  Quaternion operator*(const Quaternion& rotation) const;

  /// <summary>
  /// Check the equality of two quaternions
  /// </summary>
  /// <param name="quaternion">The quaternion to compare to</param>
  /// <returns>True when the components of the quaternions are
  /// identical</returns> Note that this does not compare the rotations
  /// themselves. Two quaternions with the same rotational effect may have
  /// different components. Use Quaternion::Angle to check if the rotations are
  /// the same.
  bool operator==(const Quaternion& quaternion) const;

  /// <summary>
  /// The inverse of quaterion
  /// </summary>
  /// <param name="quaternion">The quaternion for which the inverse is
  /// needed</param> <returns>The inverted quaternion</returns>
  static Quaternion Inverse(Quaternion quaternion);

  /// <summary>
  /// A rotation which looks in the given direction
  /// </summary>
  /// <param name="forward">The look direction</param>
  /// <param name="upwards">The up direction</param>
  /// <returns>The look rotation</returns>
  static Quaternion LookRotation(const Vector3& forward,
                                 const Vector3& upwards);
  /// <summary>
  /// Creates a quaternion with the given forward direction with up =
  /// Vector3::up
  /// </summary>
  /// <param name="forward">The look direction</param>
  /// <returns>The rotation for this direction</returns>
  /// For the rotation, Vector::up is used for the up direction.
  /// Note: if the forward direction == Vector3::up, the result is
  /// Quaternion::identity
  static Quaternion LookRotation(const Vector3& forward);

  /// <summary>
  /// Calculat the rotation from on vector to another
  /// </summary>
  /// <param name="fromDirection">The from direction</param>
  /// <param name="toDirection">The to direction</param>
  /// <returns>The rotation from the first to the second vector</returns>
  static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);

  /// <summary>
  /// Rotate form one orientation to anther with a maximum amount of degrees
  /// </summary>
  /// <param name="from">The from rotation</param>
  /// <param name="to">The destination rotation</param>
  /// <param name="maxDegreesDelta">The maximum amount of degrees to
  /// rotate</param> <returns>The possibly limited rotation</returns>
  static Quaternion RotateTowards(const Quaternion& from,
                                  const Quaternion& to,
                                  float maxDegreesDelta);

  /// <summary>
  /// Convert an angle/axis representation to a quaternion
  /// </summary>
  /// <param name="angle">The angle</param>
  /// <param name="axis">The axis</param>
  /// <returns>The resulting quaternion</returns>
  static Quaternion AngleAxis(float angle, const Vector3& axis);
  /// <summary>
  /// Convert this quaternion to angle/axis representation
  /// </summary>
  /// <param name="angle">A pointer to the angle for the result</param>
  /// <param name="axis">A pointer to the axis for the result</param>
  void ToAngleAxis(float* angle, Vector3* axis);

  /// <summary>
  /// Get the angle between two orientations
  /// </summary>
  /// <param name="orientation1">The first orientation</param>
  /// <param name="orientation2">The second orientation</param>
  /// <returns>The smallest angle in degrees between the two
  /// orientations</returns>
  static float Angle(Quaternion orientation1, Quaternion orientation2);
  /// <summary>
  /// Sherical lerp between two rotations
  /// </summary>
  /// <param name="rotation1">The first rotation</param>
  /// <param name="rotation2">The second rotation</param>
  /// <param name="factor">The factor between 0 and 1.</param>
  /// <returns>The resulting rotation</returns>
  /// A factor 0 returns rotation1, factor1 returns rotation2.
  static Quaternion Slerp(const Quaternion& rotation1,
                          const Quaternion& rotation2,
                          float factor);
  /// <summary>
  /// Unclamped sherical lerp between two rotations
  /// </summary>
  /// <param name="rotation1">The first rotation</param>
  /// <param name="rotation2">The second rotation</param>
  /// <param name="factor">The factor</param>
  /// <returns>The resulting rotation</returns>
  /// A factor 0 returns rotation1, factor1 returns rotation2.
  /// Values outside the 0..1 range will result in extrapolated rotations
  static Quaternion SlerpUnclamped(const Quaternion& rotation1,
                                   const Quaternion& rotation2,
                                   float factor);

  /// <summary>
  /// Create a rotation from euler angles
  /// </summary>
  /// <param name="x">The angle around the right axis</param>
  /// <param name="y">The angle around the upward axis</param>
  /// <param name="z">The angle around the forward axis</param>
  /// <returns>The resulting quaternion</returns>
  /// Rotation are appied in the order Z, X, Y.
  static Quaternion Euler(float x, float y, float z);
  /// <summary>
  /// Create a rotation from a vector containing euler angles
  /// </summary>
  /// <param name="eulerAngles">Vector with the euler angles</param>
  /// <returns>The resulting quaternion</returns>
  /// Rotation are appied in the order Z, X, Y.
  static Quaternion Euler(Vector3 eulerAngles);

  /// <summary>
  /// Create a rotation from euler angles
  /// </summary>
  /// <param name="x">The angle around the right axis</param>
  /// <param name="y">The angle around the upward axis</param>
  /// <param name="z">The angle around the forward axis</param>
  /// <returns>The resulting quaternion</returns>
  /// Rotation are appied in the order X, Y, Z.
  static Quaternion EulerXYZ(float x, float y, float z);
  /// <summary>
  /// Create a rotation from a vector containing euler angles
  /// </summary>
  /// <param name="eulerAngles">Vector with the euler angles</param>
  /// <returns>The resulting quaternion</returns>
  /// Rotation are appied in the order X, Y, Z.
  static Quaternion EulerXYZ(Vector3 eulerAngles);

  /// <summary>
  /// Returns the angle of around the give axis for a rotation
  /// </summary>
  /// <param name="axis">The axis around which the angle should be
  /// computed</param> <param name="rotation">The source rotation</param>
  /// <returns>The signed angle around the axis</returns>
  static float GetAngleAround(Vector3 axis, Quaternion rotation);
  /// <summary>
  /// Returns the rotation limited around the given axis
  /// </summary>
  /// <param name="axis">The axis which which the rotation should be
  /// limited</param> <param name="rotation">The source rotation</param>
  /// <returns>The rotation around the given axis</returns>
  static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
  /// <summary>
  /// Swing-twist decomposition of a rotation
  /// </summary>
  /// <param name="axis">The base direction for the decomposition</param>
  /// <param name="rotation">The source rotation</param>
  /// <param name="swing">A pointer to the quaternion for the swing
  /// result</param> <param name="twist">A pointer to the quaternion for the
  /// twist result</param>
  static void GetSwingTwist(Vector3 axis,
                            Quaternion rotation,
                            Quaternion* swing,
                            Quaternion* twist);

  /// <summary>
  /// Calculate the dot product of two quaternions
  /// </summary>
  /// <param name="rotation1">The first rotation</param>
  /// <param name="rotation2">The second rotation</param>
  /// <returns></returns>
  static float Dot(Quaternion rotation1, Quaternion rotation2);

 private:
  float GetLength() const;
  float GetLengthSquared() const;
  static float GetLengthSquared(const Quaternion& q);

  void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
  static Quaternion FromEulerRad(Vector3 euler);
  static Quaternion FromEulerRadXYZ(Vector3 euler);

  Vector3 xyz() const;
};

}  // namespace LinearAlgebra
using namespace LinearAlgebra;

#endif