RoboidControl-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 <math.h>

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

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

Vector3::Vector3(float _x, float _y, float _z) {
  x = _x;
  y = _y;
  z = _z;
}

Vector3::Vector3(Vec3 v) {
  x = v.x;
  y = v.y;
  z = v.z;
}

Vector3::Vector3(Spherical s) {
  float verticalRad = (90 - s.verticalAngle) * Angle::Deg2Rad;
  float horizontalRad = s.horizontalAngle * Angle::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 &a) {
  return sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
}
float Vector3::magnitude() const { return (float)sqrtf(x * x + y * y + z * z); }

float Vector3::SqrMagnitude(const Vector3 &a) {
  return a.x * a.x + a.y * a.y + a.z * a.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 Vector3 &v2) const {
  return Vector3(this->x - v2.x, this->y - v2.y, this->z - v2.z);
}

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

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

Vector3 Vector3::Scale(const Vector3 &p1, const Vector3 &p2) {
  return Vector3(p1.x * p2.x, p1.y * p2.y, p1.z * p2.z);
}

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

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

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) {
  return (this->x == v.x && this->y == v.y && this->z == v.z);
}

float Vector3::Distance(const Vector3 &p1, const Vector3 &p2) {
  return Magnitude(p1 - p2);
}

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 &vector, const Vector3 &onNormal) {
  float sqrMagnitude = Dot(onNormal, onNormal);
  if (sqrMagnitude < epsilon)
    return Vector3::zero;
  else {
    float dot = Dot(vector, onNormal);
    Vector3 r = onNormal * dot / sqrMagnitude;
    return r;
  }
}

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

Vector2 Vector3::ProjectHorizontalPlane(const Vector3 &vector) {
  Vector2 r = Vector2(vector.x, vector.z);
  return r;
}

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

float Vector3::Angle(const Vector3 &from, const Vector3 &to) {
  float denominator = sqrtf(from.sqrMagnitude() * to.sqrMagnitude());
  if (denominator < epsilon)
    return 0;

  float dot = Vector3::Dot(from, to);
  float fraction = dot / denominator;
  if (isnan(fraction))
    return fraction; // short cut to returning NaN universally

  float cdot = clamp(fraction, -1.0, 1.0);
  float r = ((float)acos(cdot)) * Rad2Deg;
  return r;
}

float Vector3::SignedAngle(const Vector3 &from, const Vector3 &to,
                           const Vector3 &axis) {
  // angle in [0,180]
  float angle = Vector3::Angle(from, to);

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

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

  return signed_angle;
}

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