LinearAlgebra-cpp/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 "TypeTraits.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;
}

#pragma region Vector3Of

namespace LinearAlgebra {

template <typename T>
Vector3Of<T>::Vector3Of() {}

template <typename T>
Vector3Of<T>::Vector3Of(T horizontal, T vertical, T depth)
    : horizontal(horizontal), vertical(vertical), depth(depth) {}

template <typename T>
Vector3Of<T>::Vector3Of(Vector2Of<T> v) : horizontal(v.horizontal), vertical(v.vertical) {}

template <typename T>
Vector3Of<T>::Vector3Of(SphericalOf<T> v) {
  float cosVertical = Angle::Cos(v.direction.vertical);
  float sinVertical = Angle::Sin(v.direction.vertical);
  float cosHorizontal = Angle::Cos(v.direction.horizontal);
  float sinHorizontal = Angle::Sin(v.direction.horizontal);

  horizontal = v.distance * sinVertical * sinHorizontal;
  vertical = v.distance * cosVertical;
  depth = v.distance * sinVertical * cosHorizontal;
}

template <typename T>
Vector3Of<T>::Vector3Of(Vector3 v) : horizontal(v.x), vertical(v.y), depth(v.z) {}

template <typename T>
Vector3 Vector3Of<T>::ToVector3() {
  return Vector3(this->horizontal, this->vertical, this->depth);
}

template <typename T>
Vector3Of<T>::~Vector3Of() {}

template <typename T>
const Vector3Of<T> Vector3Of<T>::zero = Vector3Of(T{}, T{}, T{});

template <>
const Vector3Of<int> Vector3Of<int>::unit = Vector3Of<int>(1, 1, 1);
template <>
const Vector3Of<float> Vector3Of<float>::unit = Vector3Of<float>(1.0f, 1.0f, 1.0f);
template <>
const Vector3Of<double> Vector3Of<double>::unit = Vector3Of<double>(1.0f, 1.0f, 1.0f);

// template <typename T>
// const Vector3Of<T> Vector3Of<T>::unit =
//     Vector3Of(TypeTraits<T>::unit(), TypeTraits<T>::unit(), TypeTraits<T>::unit());
// const Vector3Of Vector3Of::right = Vector3Of(1, 0, 0);
// const Vector3Of Vector3Of::left = Vector3Of(-1, 0, 0);
// const Vector3Of Vector3Of::up = Vector3Of(0, 1, 0);
// const Vector3Of Vector3Of::down = Vector3Of(0, -1, 0);
// const Vector3Of Vector3Of::forward = Vector3Of(0, 0, 1);
// const Vector3Of Vector3Of::back = Vector3Of(0, 0, -1);

template <typename T>
T Vector3Of<T>::MagnitudeOf(const Vector3Of& v) {
  return sqrtf(v.horizontal * v.horizontal + v.vertical * v.vertical + v.depth * v.depth);
}
template <typename T>
T Vector3Of<T>::Magnitude() const {
  return (T)sqrtf(this->horizontal * this->horizontal + this->vertical * this->vertical +
                  this->depth * this->depth);
}

template <typename T>
float Vector3Of<T>::SqrMagnitudeOf(const Vector3Of& v) {
  return v.horizontal * v.horizontal + v.vertical * v.vertical + v.depth * v.depth;
}
template <typename T>
float Vector3Of<T>::SqrMagnitude() const {
  return (horizontal * horizontal + vertical * vertical + depth * depth);
}

template <typename T>
Vector3Of<T> Vector3Of<T>::Normalize(const Vector3Of& v) {
  float num = Vector3Of<T>::MagnitudeOf(v);
  Vector3Of<T> result = Vector3Of<T>::zero;
  if (num > epsilon) {
    result = v / num;
  }
  return result;
}

template <typename T>
Vector3Of<T> Vector3Of<T>::Normalized() const {
  float num = this->Magnitude();
  Vector3Of<T> result = Vector3Of<T>::zero;
  if (num > epsilon) {
    result = ((Vector3Of<T>) * this) / num;
  }
  return result;
}
template <typename T>
Vector3Of<T> Vector3Of<T>::operator-(const Vector3Of& v) const {
  return Vector3(this->horizontal - v.horizontal, this->vertical - v.vertical,
                 this->depth - v.depth);
}

template <typename T>
Vector3Of<T> Vector3Of<T>::operator-=(const Vector3Of& v) {
  this->horizontal -= v.horizontal;
  this->vertical -= v.vertical;
  this->depth -= v.depth;
  return *this;
}

template <typename T>
Vector3Of<T> Vector3Of<T>::operator+(const Vector3Of& v) const {
  return Vector3(this->horizontal + v.horizontal, this->vertical + v.vertical,
                 this->depth + v.depth);
}

template <typename T>
Vector3Of<T> Vector3Of<T>::operator+=(const Vector3Of& v) {
  this->horizontal += v.horizontal;
  this->vertical += v.vertical;
  this->depth += v.depth;
  return *this;
}

template <typename T>
Vector3Of<T> Vector3Of<T>::operator*=(float f) {
  this->horizontal *= f;
  this->vertical *= f;
  this->depth *= f;
  return *this;
}

template <typename T>
Vector3Of<T> Vector3Of<T>::operator/=(float f) {
  this->horizontal /= f;
  this->vertical /= f;
  this->depth /= f;
  return *this;
}

template <typename T>
float Vector3Of<T>::Dot(const Vector3Of& v1, const Vector3Of& v2) {
  return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + v1.depth * v2.depth;
}

template <typename T>
AngleOf<float> Vector3Of<T>::Angle(const Vector3Of& v1, const Vector3Of& v2) {
  float denominator = sqrtf(v1.SqrMagnitude() * v2.SqrMagnitude());
  if (denominator < epsilon)
    return AngleOf<float>();

  float dot = Vector3Of::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);
}

template class Vector3Of<float>;

}  // namespace LinearAlgebra

#pragma endregion Vector3Of