Initial Commit
This commit is contained in:
613
src/J3ML/LinearAlgebra.cpp
Normal file
613
src/J3ML/LinearAlgebra.cpp
Normal file
@@ -0,0 +1,613 @@
|
||||
#include <types/vector.h>
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
|
||||
|
||||
#pragma region vector2
|
||||
|
||||
Vector2::Vector2(): x(0), y(0)
|
||||
{}
|
||||
|
||||
Vector2::Vector2(float X, float Y): x(X), y(Y)
|
||||
{}
|
||||
|
||||
Vector2::Vector2(const Vector2& rhs): x(rhs.x), y(rhs.y)
|
||||
{}
|
||||
|
||||
float Vector2::operator[](std::size_t index)
|
||||
{
|
||||
assert(index < 2);
|
||||
if (index == 0) return x;
|
||||
if (index == 1) return y;
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool Vector2::IsWithinMarginOfError(const Vector2& rhs, float margin) const
|
||||
{
|
||||
return this->Distance(rhs) <= margin;
|
||||
}
|
||||
|
||||
bool Vector2::operator==(const Vector2& rhs) const
|
||||
{
|
||||
return this->IsWithinMarginOfError(rhs);
|
||||
}
|
||||
|
||||
bool Vector2::operator!=(const Vector2& rhs) const
|
||||
{
|
||||
return this->IsWithinMarginOfError(rhs) == false;
|
||||
}
|
||||
|
||||
Vector2 Vector2::Min(const Vector2& min) const
|
||||
{
|
||||
return {
|
||||
std::min(this->x, min.x),
|
||||
std::min(this->y, min.y)
|
||||
};
|
||||
}
|
||||
|
||||
Vector2 Vector2::Max(const Vector2& max) const
|
||||
{
|
||||
return {
|
||||
std::max(this->x, max.x),
|
||||
std::max(this->y, max.y)
|
||||
};
|
||||
}
|
||||
|
||||
Vector2 Vector2::Clamp(const Vector2& min, const Vector2& max) const
|
||||
{
|
||||
return {
|
||||
std::clamp(this->x, min.x, max.x),
|
||||
std::clamp(this->y, min.y, max.y)
|
||||
};
|
||||
}
|
||||
|
||||
float Vector2::Distance(const Vector2& to) const
|
||||
{
|
||||
return ((*this)-to).Magnitude();
|
||||
}
|
||||
|
||||
float Vector2::Length() const
|
||||
{
|
||||
return std::sqrt(LengthSquared());
|
||||
}
|
||||
|
||||
float Vector2::LengthSquared() const
|
||||
{
|
||||
return (x*x + y*y);
|
||||
}
|
||||
|
||||
float Vector2::Magnitude() const
|
||||
{
|
||||
return std::sqrt(LengthSquared());
|
||||
}
|
||||
|
||||
float Vector2::Dot(const Vector2& rhs) const
|
||||
{
|
||||
auto a = this->Normalize();
|
||||
auto b = rhs.Normalize();
|
||||
|
||||
return a.x * b.x + a.y * b.y;
|
||||
}
|
||||
|
||||
Vector2 Vector2::Project(const Vector2& rhs) const
|
||||
{
|
||||
float scalar = this->Dot(rhs) / (rhs.Magnitude()*rhs.Magnitude());
|
||||
return rhs * scalar;
|
||||
}
|
||||
|
||||
Vector2 Vector2::Normalize() const
|
||||
{
|
||||
if (Length() > 0)
|
||||
return {
|
||||
x / Length(),
|
||||
y / Length()
|
||||
};
|
||||
else
|
||||
return {0,0};
|
||||
}
|
||||
|
||||
Vector2 Vector2::Lerp(const Vector2& rhs, float alpha) const
|
||||
{
|
||||
return this->operator*(1.0f - alpha) + (rhs * alpha);
|
||||
}
|
||||
|
||||
float Vector2::AngleBetween(const Vector2& rhs) const
|
||||
{
|
||||
auto numer = this->Dot(rhs);
|
||||
auto denom = this->Magnitude() * rhs.Magnitude();
|
||||
return std::acos(numer / denom);
|
||||
}
|
||||
|
||||
float Vector2::AngleBetween(const Vector2& lhs, const Vector2& rhs)
|
||||
{ return lhs.AngleBetween(rhs); }
|
||||
|
||||
Vector2 Vector2::operator+(const Vector2& rhs) const
|
||||
{
|
||||
return {this->x + rhs.x, this->y + rhs.y};
|
||||
}
|
||||
|
||||
Vector2 Vector2::operator-(const Vector2& rhs) const
|
||||
{
|
||||
return {this->x - rhs.x, this->y - rhs.y};
|
||||
}
|
||||
|
||||
Vector2 Vector2::operator*(float rhs) const
|
||||
{
|
||||
return {
|
||||
this->x * rhs,
|
||||
this->y * rhs
|
||||
};
|
||||
}
|
||||
|
||||
Vector2 Vector2::operator/(float rhs) const
|
||||
{
|
||||
return {
|
||||
this->x / rhs,
|
||||
this->y / rhs
|
||||
};
|
||||
}
|
||||
|
||||
Vector2 Vector2::operator-() const
|
||||
{
|
||||
return {-x, -y};
|
||||
}
|
||||
|
||||
const Vector2 Vector2::Zero = {0, 0};
|
||||
const Vector2 Vector2::Up = {0, -1};
|
||||
const Vector2 Vector2::Down = {0, 1};
|
||||
const Vector2 Vector2::Left = {-1, 0};
|
||||
const Vector2 Vector2::Right = {1, 0};
|
||||
#pragma endregion
|
||||
|
||||
#pragma region vector3
|
||||
|
||||
const Vector3 Vector3::Zero = {0,0,0};
|
||||
const Vector3 Vector3::Up = {0, -1, 0};
|
||||
const Vector3 Vector3::Down = {0, 1, 0};
|
||||
const Vector3 Vector3::Left = {-1, 0, 0};
|
||||
const Vector3 Vector3::Right = {1, 0, 0};
|
||||
const Vector3 Vector3::Forward = {0, 0, -1};
|
||||
const Vector3 Vector3::Backward = {0, 0, 1};
|
||||
|
||||
Vector3 Vector3::operator+(const Vector3& rhs) const
|
||||
{
|
||||
return {this->x + rhs.x, this->y + rhs.y, this->z + rhs.z};
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator-(const Vector3& rhs) const
|
||||
{
|
||||
return {
|
||||
this->x- rhs.x,
|
||||
this->y-rhs.y,
|
||||
this->z-rhs.z
|
||||
};
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator*(float rhs) const
|
||||
{
|
||||
return {
|
||||
this->x * rhs,
|
||||
this->y * rhs,
|
||||
this->z * rhs
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
Vector3 Vector3::operator/(float rhs) const
|
||||
{
|
||||
return {
|
||||
this->x / rhs,
|
||||
this->y / rhs,
|
||||
this->z / rhs
|
||||
};
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator-() const
|
||||
{
|
||||
return {-x, -y, -z};
|
||||
}
|
||||
|
||||
Vector4::Vector4(): x(0), y(0), z(0), w(0)
|
||||
{}
|
||||
|
||||
Vector4::Vector4(float X, float Y, float Z, float W): x(X),y(Y),z(Z),w(W)
|
||||
{ }
|
||||
|
||||
|
||||
Vector3::Vector3(): x(0), y(0), z(0)
|
||||
{}
|
||||
|
||||
Vector3::Vector3(float X, float Y, float Z): x(X), y(Y), z(Z)
|
||||
{}
|
||||
|
||||
Vector3::Vector3(const Vector3& rhs)
|
||||
{
|
||||
this->x = rhs.x;
|
||||
this->y = rhs.y;
|
||||
this->z = rhs.z;
|
||||
}
|
||||
|
||||
Vector3& Vector3::operator=(const Vector3& rhs)
|
||||
{
|
||||
this->x = rhs.x;
|
||||
this->y = rhs.y;
|
||||
this->z = rhs.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
float Vector3::operator[](std::size_t index) const
|
||||
{
|
||||
assert(index < 3);
|
||||
if (index==0) return x;
|
||||
if (index==1) return y;
|
||||
if (index==2) return z;
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool Vector3::IsWithinMarginOfError(const Vector3& rhs, float margin) const
|
||||
{
|
||||
return this->Distance(rhs) <= margin;
|
||||
}
|
||||
|
||||
bool Vector3::operator==(const Vector3& rhs) const
|
||||
{
|
||||
return this->IsWithinMarginOfError(rhs);
|
||||
}
|
||||
|
||||
bool Vector3::operator!=(const Vector3& rhs) const
|
||||
{
|
||||
return this->IsWithinMarginOfError(rhs) == false;
|
||||
}
|
||||
|
||||
Vector3 Vector3::Min(const Vector3& min) const
|
||||
{
|
||||
return {
|
||||
std::min(this->x, min.x),
|
||||
std::min(this->y, min.y),
|
||||
std::min(this->z, min.z)
|
||||
};
|
||||
}
|
||||
|
||||
Vector3 Vector3::Max(const Vector3& max) const
|
||||
{
|
||||
return {
|
||||
std::max(this->x, max.x),
|
||||
std::max(this->y, max.y),
|
||||
std::max(this->z, max.z)
|
||||
};
|
||||
}
|
||||
|
||||
Vector3 Vector3::Clamp(const Vector3& min, const Vector3& max) const
|
||||
{
|
||||
return {
|
||||
std::clamp(this->x, min.x, max.x),
|
||||
std::clamp(this->y, min.y, max.y),
|
||||
std::clamp(this->z, min.z, max.z)
|
||||
};
|
||||
}
|
||||
|
||||
float Vector3::Distance(const Vector3& to) const
|
||||
{
|
||||
return ((*this)-to).Magnitude();
|
||||
}
|
||||
|
||||
float Vector3::Length() const
|
||||
{
|
||||
return std::sqrt(LengthSquared());
|
||||
}
|
||||
|
||||
float Vector3::LengthSquared() const
|
||||
{
|
||||
return (x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
float Vector3::Magnitude() const
|
||||
{
|
||||
return std::sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
float Vector3::Dot(const Vector3& rhs) const
|
||||
{
|
||||
auto a = this->Normalize();
|
||||
auto b = rhs.Normalize();
|
||||
return a.x * b.x +
|
||||
a.y * b.y +
|
||||
a.z * b.z;
|
||||
}
|
||||
|
||||
Vector3 Vector3::Project(const Vector3& rhs) const
|
||||
{
|
||||
float scalar = this->Dot(rhs) / (rhs.Magnitude()*rhs.Magnitude());
|
||||
return rhs * scalar;
|
||||
}
|
||||
|
||||
Vector3 Vector3::Cross(const Vector3& rhs) const
|
||||
{
|
||||
return {
|
||||
this->y * rhs.z - this->z * rhs.y,
|
||||
this->z * rhs.x - this->x * rhs.z,
|
||||
this->x * rhs.y - this->y * rhs.x
|
||||
};
|
||||
}
|
||||
|
||||
Vector3 Vector3::Normalize() const
|
||||
{
|
||||
if (Length() > 0)
|
||||
return {
|
||||
x / Length(),
|
||||
y / Length(),
|
||||
z / Length()
|
||||
};
|
||||
else
|
||||
return {0,0,0};
|
||||
}
|
||||
|
||||
Vector3 Vector3::Lerp(const Vector3& goal, float alpha) const
|
||||
{
|
||||
return this->operator*(1.0f - alpha) + (goal * alpha);
|
||||
}
|
||||
|
||||
#pragma endregion
|
||||
|
||||
#pragma region vector4
|
||||
|
||||
|
||||
bool Vector4::operator==(const Vector4& rhs) const
|
||||
{
|
||||
return this->IsWithinMarginOfError(rhs);
|
||||
}
|
||||
|
||||
bool Vector4::operator!=(const Vector4& rhs) const
|
||||
{
|
||||
return this->IsWithinMarginOfError(rhs) == false;
|
||||
}
|
||||
|
||||
Vector4 Vector4::min(const Vector4& min) const
|
||||
{
|
||||
return {
|
||||
std::min(this->x, min.x),
|
||||
std::min(this->y, min.y),
|
||||
std::min(this->z, min.z),
|
||||
std::min(this->w, min.w)
|
||||
};
|
||||
}
|
||||
|
||||
Vector4 Vector4::max(const Vector4& max) const
|
||||
{
|
||||
return {
|
||||
std::max(this->x, max.x),
|
||||
std::max(this->y, max.y),
|
||||
std::max(this->z, max.z),
|
||||
std::max(this->w, max.w)
|
||||
};
|
||||
}
|
||||
|
||||
Vector4 Vector4::clamp(const Vector4& min, const Vector4& max) const
|
||||
{
|
||||
return {
|
||||
std::clamp(this->x, min.x, max.x),
|
||||
std::clamp(this->y, min.y, max.y),
|
||||
std::clamp(this->z, min.z, max.z),
|
||||
std::clamp(this->w, min.w, max.w)
|
||||
};
|
||||
}
|
||||
|
||||
float Vector4::distance(const Vector4& to) const
|
||||
{
|
||||
return ( (*this) - to ).magnitude();
|
||||
}
|
||||
|
||||
float Vector4::length() const
|
||||
{
|
||||
return std::sqrt(lengthSquared());
|
||||
}
|
||||
|
||||
float Vector4::lengthSquared() const
|
||||
{
|
||||
return (x*x + y*y + z*z + w*w);
|
||||
}
|
||||
|
||||
float Vector4::magnitude() const
|
||||
{
|
||||
return std::sqrt(x*x + y*y + z*z + w*w);
|
||||
}
|
||||
|
||||
float Vector4::dot(const Vector4& rhs) const
|
||||
{
|
||||
auto a = this->normalize();
|
||||
auto b = rhs.normalize();
|
||||
|
||||
return a.x * b.x +
|
||||
a.y * b.y +
|
||||
a.z * b.z +
|
||||
a.w * b.w;
|
||||
}
|
||||
|
||||
Vector4 Vector4::project(const Vector4& rhs) const
|
||||
{
|
||||
float scalar = this->dot(rhs) / (rhs.magnitude()* rhs.magnitude());
|
||||
return rhs * scalar;
|
||||
}
|
||||
|
||||
Vector4 Vector4::normalize() const
|
||||
{
|
||||
if (length() > 0)
|
||||
return {
|
||||
x / length(),
|
||||
y / length(),
|
||||
z / length(),
|
||||
w / length()
|
||||
};
|
||||
else
|
||||
return {0,0,0,0};
|
||||
}
|
||||
|
||||
Vector4 Vector4::lerp(const Vector4& goal, float alpha) const
|
||||
{
|
||||
return this->operator*(1.0f - alpha) + (goal * alpha);
|
||||
}
|
||||
|
||||
Vector4 Vector4::operator+(const Vector4& rhs) const
|
||||
{
|
||||
return {x+rhs.x, y+rhs.y, z+rhs.z, w+rhs.w};
|
||||
}
|
||||
|
||||
Vector4 Vector4::operator-(const Vector4& rhs) const
|
||||
{
|
||||
return {x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w};
|
||||
}
|
||||
|
||||
|
||||
|
||||
#pragma endregion
|
||||
|
||||
|
||||
|
||||
#pragma region EulerAngle
|
||||
|
||||
EulerAngle::EulerAngle(float pitch, float yaw, float roll): pitch(pitch), yaw(yaw), roll(roll)
|
||||
{}
|
||||
|
||||
float EulerAngle::GetPitch(float pitch_limit) const
|
||||
{ return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }
|
||||
|
||||
float EulerAngle::GetYaw(float yaw_limit) const
|
||||
{ return std::clamp(std::remainderf(yaw, 360.f), -yaw_limit, yaw_limit); }
|
||||
|
||||
float EulerAngle::GetRoll(float pitch_limit) const
|
||||
{ return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }
|
||||
|
||||
bool EulerAngle::operator==(const EulerAngle& a) const
|
||||
{
|
||||
return (pitch == a.pitch) && (yaw == a.yaw) && (roll == a.roll);
|
||||
}
|
||||
|
||||
void EulerAngle::clamp()
|
||||
{
|
||||
if (this->pitch > 89.0f)
|
||||
this->pitch = 89.0f;
|
||||
if (this->pitch <= -89.0f)
|
||||
this->pitch = -89.0f;
|
||||
//TODO: Make this entirely seamless by getting the amount they rotated passed -180 and +180 by.
|
||||
if (this->yaw <= -180.0f)
|
||||
this->yaw = 180.0f;
|
||||
if (this->yaw >= 180.01f)
|
||||
this->yaw = -179.9f;
|
||||
if (this->roll >= 360.0f)
|
||||
this->roll = 0.0;
|
||||
if (this->roll <= -360.0f)
|
||||
this->roll = 0.0;
|
||||
}
|
||||
|
||||
EulerAngle EulerAngle::movementAngle() const
|
||||
{
|
||||
EulerAngle a;
|
||||
a.pitch = (cos(glm::radians(yaw)) * cos(glm::radians(pitch)));
|
||||
a.yaw = -sin(glm::radians(pitch));
|
||||
a.roll = (sin(glm::radians(yaw)) * cos(glm::radians(pitch)));
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
const Matrix3x3 Matrix3x3::Zero = Matrix3x3(0,0,0, 0,0,0, 0,0,0);
|
||||
const Matrix3x3 Matrix3x3::Identity = Matrix3x3(1,0,0, 0,1,0, 0,0,1);
|
||||
const Matrix3x3 Matrix3x3::NaN = Matrix3x3(NAN);
|
||||
|
||||
Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21,
|
||||
float m22)
|
||||
{
|
||||
this->elems[0][0] = m00;
|
||||
this->elems[0][1] = m01;
|
||||
this->elems[0][2] = m02;
|
||||
|
||||
this->elems[1][0] = m10;
|
||||
this->elems[1][1] = m11;
|
||||
this->elems[1][2] = m12;
|
||||
|
||||
this->elems[2][0] = m20;
|
||||
this->elems[2][1] = m21;
|
||||
this->elems[2][2] = m22;
|
||||
}
|
||||
|
||||
Vector3 Matrix3x3::GetRow(int index) const
|
||||
{
|
||||
float x = this->elems[index][0];
|
||||
float y = this->elems[index][1];
|
||||
float z = this->elems[index][2];
|
||||
return {x,y,z};
|
||||
}
|
||||
|
||||
Vector3 Matrix3x3::GetColumn(int index) const
|
||||
{
|
||||
float x = this->elems[0][index];
|
||||
float y = this->elems[1][index];
|
||||
float z = this->elems[2][index];
|
||||
|
||||
return {x,y,z};
|
||||
}
|
||||
|
||||
float Matrix3x3::At(int x, int y) const
|
||||
{
|
||||
return this->elems[x][y];
|
||||
|
||||
}
|
||||
|
||||
Vector3 Matrix3x3::operator*(const Vector3& rhs) const
|
||||
{
|
||||
return {
|
||||
At(0,0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
|
||||
At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
|
||||
At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2,2) * rhs.z
|
||||
};
|
||||
}
|
||||
|
||||
Matrix3x3 Matrix3x3::operator*(const Matrix3x3& rhs) const
|
||||
{
|
||||
//Matrix3x3 r;
|
||||
auto m00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
|
||||
auto m01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
|
||||
auto m02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
|
||||
|
||||
auto m10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
|
||||
auto m11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
|
||||
auto m12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2,2);
|
||||
|
||||
auto m20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2,2) * rhs.At(2, 0);
|
||||
auto m21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2,2) * rhs.At(2, 1);
|
||||
auto m22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2,2) * rhs.At(2,2);
|
||||
|
||||
return Matrix3x3({m00, m01, m02}, {m10, m11, m12}, {m20, m21, m22});
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator-() const
|
||||
{
|
||||
return {-x, -y, -z, -w};
|
||||
}
|
||||
|
||||
|
||||
#pragma endregion
|
||||
|
||||
|
||||
|
||||
#pragma region Matrix2x2
|
||||
|
||||
#pragma endregion
|
||||
|
||||
|
||||
|
||||
#pragma region Matrix3x3
|
||||
|
||||
#pragma endregion
|
||||
|
||||
|
||||
#pragma region Matrix4x4
|
||||
|
||||
|
||||
#pragma endregion
|
||||
|
||||
|
||||
#pragma region Quaternion
|
||||
|
||||
#pragma endregion
|
||||
}
|
||||
Reference in New Issue
Block a user