RoboidControl-cpp/LinearAlgebra/Vector3.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 "Vector3.h"
#include "Angle.h"
#include "Spherical.h"

#include <math.h>

const float Deg2Rad = 0.0174532924F;
const float Rad2Deg = 57.29578F;
const float epsilon = 1E-05f;

Vector3::Vector3() {
  this->x = 0;
  this->y = 0;
  this->z = 0;
}

Vector3::Vector3(float right, float up, float forward) {
  this->x = right;
  this->y = up;
  this->z = forward;
}

Vector3::Vector3(Vector2 v) {
  this->x = v.x;
  this->y = 0.0f;
  this->z = v.y;
}

Vector3::Vector3(SphericalOf<float> s) {
  float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad;
  float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
  float cosVertical = cosf(verticalRad);
  float sinVertical = sinf(verticalRad);
  float cosHorizontal = cosf(horizontalRad);
  float sinHorizontal = sinf(horizontalRad);

  x = s.distance * sinVertical * sinHorizontal;
  y = s.distance * cosVertical;
  z = s.distance * sinVertical * cosHorizontal;
  // Vector3 v = Vector3(s.distance * sinVertical * sinHorizontal,
  //                     s.distance * cosVertical,
  //                     );
  // return v;
}

Vector3::~Vector3() {}

const Vector3 Vector3::zero = Vector3(0, 0, 0);
const Vector3 Vector3::one = Vector3(1, 1, 1);
const Vector3 Vector3::right = Vector3(1, 0, 0);
const Vector3 Vector3::left = Vector3(-1, 0, 0);
const Vector3 Vector3::up = Vector3(0, 1, 0);
const Vector3 Vector3::down = Vector3(0, -1, 0);
const Vector3 Vector3::forward = Vector3(0, 0, 1);
const Vector3 Vector3::back = Vector3(0, 0, -1);

// inline float Vector3::Forward() { return z; }
// inline float Vector3::Up() { return y; }
// inline float Vector3::Right() { return x; }
// Vector3 Vector3::FromHorizontal(const Vector2 &v) {
//   return Vector3(v.x, 0, v.y);
// }

float Vector3::Magnitude(const Vector3& v) {
  return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
}
float Vector3::magnitude() const {
  return (float)sqrtf(x * x + y * y + z * z);
}

float Vector3::SqrMagnitude(const Vector3& v) {
  return v.x * v.x + v.y * v.y + v.z * v.z;
}
float Vector3::sqrMagnitude() const {
  return (x * x + y * y + z * z);
}

Vector3 Vector3::Normalize(const Vector3& v) {
  float num = Vector3::Magnitude(v);
  Vector3 result = Vector3::zero;
  if (num > epsilon) {
    result = v / num;
  }
  return result;
}
Vector3 Vector3::normalized() const {
  float num = this->magnitude();
  Vector3 result = Vector3::zero;
  if (num > epsilon) {
    result = ((Vector3) * this) / num;
  }
  return result;
}

Vector3 Vector3::operator-() const {
  return Vector3(-this->x, -this->y, -this->z);
}

Vector3 Vector3::operator-(const Vector3& v) const {
  return Vector3(this->x - v.x, this->y - v.y, this->z - v.z);
}
Vector3 Vector3::operator-=(const Vector3& v) {
  this->x -= v.x;
  this->y -= v.y;
  this->z -= v.z;
  return *this;
}
Vector3 Vector3::operator+(const Vector3& v) const {
  return Vector3(this->x + v.x, this->y + v.y, this->z + v.z);
}
Vector3 Vector3::operator+=(const Vector3& v) {
  this->x += v.x;
  this->y += v.y;
  this->z += v.z;
  return *this;
}

Vector3 Vector3::Scale(const Vector3& v1, const Vector3& v2) {
  return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
}
// Vector3 Passer::LinearAlgebra::operator*(const Vector3 &v, float f) {
//   return Vector3(v.x * f, v.y * f, v.z * f);
// }
// Vector3 Passer::LinearAlgebra::operator*(float f, const Vector3 &v) {
//   return Vector3(v.x * f, v.y * f, v.z * f);
// }
Vector3 Vector3::operator*=(float f) {
  this->x *= f;
  this->y *= f;
  this->z *= f;
  return *this;
}
// Vector3 Passer::LinearAlgebra::operator/(const Vector3 &v, float f) {
//   return Vector3(v.x / f, v.y / f, v.z / f);
// }
// Vector3 Passer::LinearAlgebra::operator/(float f, const Vector3 &v) {
//   return Vector3(v.x / f, v.y / f, v.z / f);
// }
Vector3 Vector3::operator/=(float f) {
  this->x /= f;
  this->y /= f;
  this->z /= f;
  return *this;
}

float Vector3::Dot(const Vector3& v1, const Vector3& v2) {
  return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

bool Vector3::operator==(const Vector3& v) const {
  return (this->x == v.x && this->y == v.y && this->z == v.z);
}

float Vector3::Distance(const Vector3& v1, const Vector3& v2) {
  return Magnitude(v1 - v2);
}

Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) {
  return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z,
                 v1.x * v2.y - v1.y * v2.x);
}

Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
  float sqrMagnitude = Dot(n, n);
  if (sqrMagnitude < epsilon)
    return Vector3::zero;
  else {
    float dot = Dot(v, n);
    Vector3 r = n * dot / sqrMagnitude;
    return r;
  }
}

Vector3 Vector3::ProjectOnPlane(const Vector3& v, const Vector3& n) {
  Vector3 r = v - Project(v, n);
  return r;
}

float clamp(float x, float lower, float upper) {
  float lowerClamp = fmaxf(x, lower);
  float upperClamp = fminf(upper, lowerClamp);
  return upperClamp;
}

AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
  float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
  if (denominator < epsilon)
    return AngleOf<float>();

  float dot = Vector3::Dot(v1, v2);
  float fraction = dot / denominator;
  if (isnan(fraction))
    return AngleOf<float>::Degrees(
        fraction);  // short cut to returning NaN universally

  float cdot = clamp(fraction, -1.0, 1.0);
  float r = ((float)acos(cdot));
  return AngleOf<float>::Radians(r);
}

AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
                                    const Vector3& v2,
                                    const Vector3& axis) {
  // angle in [0,180]
  AngleOf<float> angle = Vector3::Angle(v1, v2);

  Vector3 cross = Vector3::Cross(v1, v2);
  float b = Vector3::Dot(axis, cross);
  float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);

  // angle in [-179,180]
  AngleOf<float> signed_angle = angle * signd;

  return AngleOf<float>(signed_angle);
}

Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) {
  Vector3 v = v1 + (v2 - v1) * f;
  return v;
}