RoboidControl-cpp/Quaternion.cpp

// 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/.

#include "Quaternion.h"
#include <float.h>
#include <math.h>
#include "Angle.h"
#include "Vector3.h"

void CopyQuat(const Quat& q1, Quat& q2) {
  q2.x = q1.x;
  q2.y = q1.y;
  q2.z = q1.z;
  q2.w = q1.w;
}

const float Deg2Rad = 0.0174532924F;
const float Rad2Deg = 57.29578F;

Quaternion::Quaternion() {
  x = 0;
  y = 0;
  z = 0;
  w = 1;
}

Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
  x = _x;
  y = _y;
  z = _z;
  w = _w;
}

Quaternion::Quaternion(Quat q) {
  x = q.x;
  y = q.y;
  z = q.z;
  w = q.w;
}

Quaternion::~Quaternion() {}

const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);

Vector3 Quaternion::xyz() const {
  return Vector3(x, y, z);
}

float Quaternion::GetLength() const {
  return sqrtf(x * x + y * y + z * z + w * w);
}

float Quaternion::GetLengthSquared() const {
  return x * x + y * y + z * z + w * w;
}
float Quaternion::GetLengthSquared(const Quaternion& q) {
  return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
}

void Quaternion::Normalize() {
  float length = GetLength();
  x /= length;
  y /= length;
  z /= length;
  w /= length;
}

Quaternion Quaternion::Normalize(const Quaternion& q) {
  Quaternion result;
  float length = q.GetLength();
  result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
  return result;
};

float Quaternion::Dot(Quaternion a, Quaternion b) {
  return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

Vector3 Quaternion::ToAngles(const Quaternion& q1) {
  float test = q1.x * q1.y + q1.z * q1.w;
  if (test > 0.499f) {  // singularity at north pole
    return Vector3(0, 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, 90);
  } else if (test < -0.499f) {  // singularity at south pole
    return Vector3(0, -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, -90);
  } else {
    float sqx = q1.x * q1.x;
    float sqy = q1.y * q1.y;
    float sqz = q1.z * q1.z;

    return Vector3(
        atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) *
            Rad2Deg,
        atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) *
            Rad2Deg,
        asinf(2 * test) * Rad2Deg);
  }
}

Quaternion Quaternion::operator*(const Quaternion& r2) const {
  return Quaternion(
      this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,
      -this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y,
      this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z,
      -this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w);
};

Vector3 Quaternion::operator*(const Vector3& p) const {
  float num = this->x * 2;
  float num2 = this->y * 2;
  float num3 = this->z * 2;
  float num4 = this->x * num;
  float num5 = this->y * num2;
  float num6 = this->z * num3;
  float num7 = this->x * num2;
  float num8 = this->x * num3;
  float num9 = this->y * num3;
  float num10 = this->w * num;
  float num11 = this->w * num2;
  float num12 = this->w * num3;

  float px = p.Right();
  float py = p.Up();
  float pz = p.Forward();
  // Vector3 result = Vector3::zero;
  //  result.x =
  float rx =
      (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
  // result.y =
  float ry =
      (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
  // result.z =
  float rz =
      (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
  Vector3 result = Vector3(rx, ry, rz);
  return result;
}

bool Quaternion::operator==(const Quaternion& q) const {
  return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
}

Quaternion Quaternion::Inverse(Quaternion r) {
  float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w);
  return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
}

Quaternion Quaternion::LookRotation(const Vector3& forward) {
  Vector3 up = Vector3(0, 1, 0);
  return LookRotation(forward, up);
}
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
  Vector3 nForward = Vector3::Normalize(forward);
  Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
  Vector3 nUp = Vector3::Cross(nForward, nRight);
  float m00 = nRight.Right();      // x;
  float m01 = nRight.Up();         // y;
  float m02 = nRight.Forward();    // z;
  float m10 = nUp.Right();         // x;
  float m11 = nUp.Up();            // y;
  float m12 = nUp.Forward();       // z;
  float m20 = nForward.Right();    // x;
  float m21 = nForward.Up();       // y;
  float m22 = nForward.Forward();  // z;

  float num8 = (m00 + m11) + m22;
  Quaternion quaternion = Quaternion();
  if (num8 > 0) {
    float num = sqrtf(num8 + 1);
    quaternion.w = num * 0.5f;
    num = 0.5f / num;
    quaternion.x = (m12 - m21) * num;
    quaternion.y = (m20 - m02) * num;
    quaternion.z = (m01 - m10) * num;
    return quaternion;
  }
  if ((m00 >= m11) && (m00 >= m22)) {
    float num7 = sqrtf(((1 + m00) - m11) - m22);
    float num4 = 0.5F / num7;
    quaternion.x = 0.5f * num7;
    quaternion.y = (m01 + m10) * num4;
    quaternion.z = (m02 + m20) * num4;
    quaternion.w = (m12 - m21) * num4;
    return quaternion;
  }
  if (m11 > m22) {
    float num6 = sqrtf(((1 + m11) - m00) - m22);
    float num3 = 0.5F / num6;
    quaternion.x = (m10 + m01) * num3;
    quaternion.y = 0.5F * num6;
    quaternion.z = (m21 + m12) * num3;
    quaternion.w = (m20 - m02) * num3;
    return quaternion;
  }
  float num5 = sqrtf(((1 + m22) - m00) - m11);
  float num2 = 0.5F / num5;
  quaternion.x = (m20 + m02) * num2;
  quaternion.y = (m21 + m12) * num2;
  quaternion.z = 0.5F * num5;
  quaternion.w = (m01 - m10) * num2;
  return quaternion;
}

Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
                                      Vector3 toDirection) {
  Vector3 axis = Vector3::Cross(fromDirection, toDirection);
  axis = Vector3::Normalize(axis);
  AngleOf<float> angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
  Quaternion rotation = Quaternion::AngleAxis(angle.InDegrees(), axis);
  return rotation;
}

Quaternion Quaternion::RotateTowards(const Quaternion& from,
                                     const Quaternion& to,
                                     float maxDegreesDelta) {
  float num = Quaternion::Angle(from, to);
  if (num == 0) {
    return to;
  }
  float t = (float)fmin(1, maxDegreesDelta / num);
  return SlerpUnclamped(from, to, t);
}

Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
  if (Vector3::SqrMagnitude(axis) == 0.0f)
    return Quaternion();

  Quaternion result = Quaternion();
  float radians = angle * Deg2Rad;
  radians *= 0.5f;

  Vector3 axis2 = axis * (float)sin(radians);
  result.x = axis2.Right();    // x;
  result.y = axis2.Up();       // y;
  result.z = axis2.Forward();  // z;
  result.w = (float)cos(radians);

  return Quaternion::Normalize(result);
}

float Quaternion::Angle(Quaternion a, Quaternion b) {
  float f = Quaternion::Dot(a, b);
  return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
}

void Quaternion::ToAngleAxis(float* angle, Vector3* axis) {
  Quaternion::ToAxisAngleRad(*this, axis, angle);
  *angle *= Rad2Deg;
}

void Quaternion::ToAxisAngleRad(const Quaternion& q,
                                Vector3* const axis,
                                float* angle) {
  Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
  *angle = 2.0f * acosf(q1.w);  // angle
  float den = sqrtf(1.0F - q1.w * q1.w);
  if (den > 0.0001f) {
    *axis = Vector3::Normalize(q1.xyz() / den);
  } else {
    // This occurs when the angle is zero.
    // Not a problem: just set an arbitrary normalized axis.
    *axis = Vector3(1, 0, 0);
  }
}
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a,
                                      const Quaternion& b,
                                      float t) {
  // if either input is zero, return the other.
  if (Quaternion::GetLengthSquared(a) == 0.0f) {
    if (Quaternion::GetLengthSquared(b) == 0.0f) {
      return Quaternion();
    }
    return b;
  } else if (Quaternion::GetLengthSquared(b) == 0.0f) {
    return a;
  }

  const Vector3 axyz = a.xyz();
  const Vector3 bxyz = b.xyz();
  float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);

  Quaternion b2 = b;
  if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
    // angle = 0.0f, so just return one input.
    return a;
  } else if (cosHalfAngle < 0.0f) {
    b2.x = -b.x;
    b2.y = -b.y;
    b2.z = -b.z;
    b2.w = -b.w;
    cosHalfAngle = -cosHalfAngle;
  }

  float blendA;
  float blendB;
  if (cosHalfAngle < 0.99f) {
    // do proper slerp for big angles
    float halfAngle = acosf(cosHalfAngle);
    float sinHalfAngle = sinf(halfAngle);
    float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
    blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
    blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
  } else {
    // do lerp if angle is really small.
    blendA = 1.0f - t;
    blendB = t;
  }
  Vector3 v = axyz * blendA + b2.xyz() * blendB;
  Quaternion result =
      Quaternion(v.Right(), v.Up(), v.Forward(), blendA * a.w + blendB * b2.w);
  if (result.GetLengthSquared() > 0.0f)
    return Quaternion::Normalize(result);
  else
    return Quaternion();
}

Quaternion Quaternion::Slerp(const Quaternion& a,
                             const Quaternion& b,
                             float t) {
  if (t > 1)
    t = 1;
  if (t < 0)
    t = 0;
  return Quaternion::SlerpUnclamped(a, b, t);
}

Quaternion Quaternion::Euler(float x, float y, float z) {
  return Quaternion::Euler(Vector3(x, y, z));
}
Quaternion Quaternion::Euler(Vector3 euler) {
  return Quaternion::FromEulerRad(euler * Deg2Rad);
}

Quaternion Quaternion::FromEulerRad(Vector3 euler) {
  float yaw = euler.Right();
  float pitch = euler.Up();
  float roll = euler.Forward();
  float rollOver2 = roll * 0.5f;
  float sinRollOver2 = (float)sin((float)rollOver2);
  float cosRollOver2 = (float)cos((float)rollOver2);
  float pitchOver2 = pitch * 0.5f;
  float sinPitchOver2 = (float)sin((float)pitchOver2);
  float cosPitchOver2 = (float)cos((float)pitchOver2);
  float yawOver2 = yaw * 0.5f;
  float sinYawOver2 = (float)sin((float)yawOver2);
  float cosYawOver2 = (float)cos((float)yawOver2);
  Quaternion result;
  result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
             sinYawOver2 * sinPitchOver2 * sinRollOver2;
  result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
             cosYawOver2 * sinPitchOver2 * sinRollOver2;
  result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
             sinYawOver2 * cosPitchOver2 * sinRollOver2;
  result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
             sinYawOver2 * sinPitchOver2 * cosRollOver2;
  return result;
}

Quaternion Quaternion::EulerXYZ(float x, float y, float z) {
  return Quaternion::EulerXYZ(Vector3(x, y, z));
}
Quaternion Quaternion::EulerXYZ(Vector3 euler) {
  return Quaternion::FromEulerRadXYZ(euler * Deg2Rad);
}
Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) {
  float yaw = euler.Right();     // x;
  float pitch = euler.Up();      // y;
  float roll = euler.Forward();  // z;
  float rollOver2 = roll * 0.5f;
  float sinRollOver2 = (float)sin((float)rollOver2);
  float cosRollOver2 = (float)cos((float)rollOver2);
  float pitchOver2 = pitch * 0.5f;
  float sinPitchOver2 = (float)sin((float)pitchOver2);
  float cosPitchOver2 = (float)cos((float)pitchOver2);
  float yawOver2 = yaw * 0.5f;
  float sinYawOver2 = (float)sin((float)yawOver2);
  float cosYawOver2 = (float)cos((float)yawOver2);
  Quaternion result;
  result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
             sinYawOver2 * sinPitchOver2 * sinRollOver2;
  result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 -
             cosYawOver2 * sinPitchOver2 * sinRollOver2;
  result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 +
             sinYawOver2 * cosPitchOver2 * sinRollOver2;
  result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
             sinYawOver2 * sinPitchOver2 * cosRollOver2;
  return result;
}

float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
  Quaternion secondaryRotation = GetRotationAround(axis, rotation);
  float rotationAngle;
  Vector3 rotationAxis;
  secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);

  // Do the axis point in opposite directions?
  if (Vector3::Dot(axis, rotationAxis) < 0)
    rotationAngle = -rotationAngle;

  return rotationAngle;
}

Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
  Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z);  // rotation axis
  Vector3 p = Vector3::Project(
      ra, axis);  // return projection ra on to axis  (parallel component)
  Quaternion twist = Quaternion(p.Right(), p.Up(), p.Forward(), rotation.w);
  twist = Quaternion::Normalize(twist);
  return twist;
}

void Quaternion::GetSwingTwist(Vector3 axis,
                               Quaternion rotation,
                               Quaternion* swing,
                               Quaternion* twist) {
  *twist = GetRotationAround(axis, rotation);
  *swing = rotation * Quaternion::Inverse(*twist);
}