RoboidControl-cpp/Vector3.cpp

#include "pch.h"
#include <math.h>
#include "Vector3.h"
#include "Quaternion.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()
{
}

const Vector3 Vector3::zero = Vector3(0, 0, 0);
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);

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(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 operator *(const Quaternion& o, const Vector3& p) {
	float num = o.x * 2;
	float num2 = o.y * 2;
	float num3 = o.z * 2;
	float num4 = o.x * num;
	float num5 = o.y * num2;
	float num6 = o.z * num3;
	float num7 = o.x * num2;
	float num8 = o.x * num3;
	float num9 = o.y * num3;
	float num10 = o.w * num;
	float num11 = o.w * num2;
	float num12 = o.w * num3;
	Vector3 result = Vector3::zero;
	result.x = (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z;
	result.y = (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z;
	result.z = (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z;
	return result;
}

Vector3 Vector3::operator/(const float& d) {
	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);
}

// Projects a vector onto another vector.
Vector3 Vector3::Project(Vector3 vector, 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;
	}
}

// Projects a vector onto a plane defined by a normal orthogonal to the plane.
Vector3 Vector3::ProjectOnPlane(Vector3 vector, Vector3 planeNormal) {
	Vector3 r = vector - Project(vector, planeNormal);
	return r;
}

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

// This function is buggy (0-1-0, 0-1-0) gives 180 degrees in return!!!
float Vector3::Angle(Vector3 from, Vector3 to) {
	float denominator = sqrtf(from.sqrMagnitude() * to.sqrMagnitude());
	if (denominator < epsilon)
		return 0;

	float dot = Vector3::Dot(from, to);
	float fraction = dot / denominator;
	float cdot = clamp(fraction, -1.0, 1.0);
	float r = ((float)acos(cdot))* Rad2Deg;
	return r;
}

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

	float cross_x = from.y * to.z - from.z * to.y;
	float cross_y = from.z * to.x - from.x * to.z;
	float cross_z = from.x * to.y - from.y * to.x;
	float b = axis.x * cross_x + axis.y * cross_y + axis.z * cross_z;
	float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
	 
	// angle in [-179,180]
	float signed_angle = angle * signd;

	return signed_angle;
}

void CopyVec3(const Vec3& v1, Vec3& v2) {
	v2.x = v1.x;
	v2.y = v1.y;
	v2.z = v1.z;
}

void Vec3_Constructor(Vec3* v, float _x, float _y, float _z) {
	v->x = _x;
	v->y = _y;
	v->z = _z;
}