From 24cc2e79bcfda0ff27e6eeb918522e7d052f21d0 Mon Sep 17 00:00:00 2001 From: josh Date: Wed, 27 Dec 2023 21:42:38 -0600 Subject: [PATCH] Adding code x3 --- include/J3ML/LinearAlgebra.h | 5 +- include/J3ML/LinearAlgebra/AxisAngle.h | 10 ++- include/J3ML/LinearAlgebra/EulerAngle.h | 2 +- include/J3ML/LinearAlgebra/Quaternion.h | 48 ++++++++----- include/J3ML/LinearAlgebra/Vector4.h | 9 ++- src/J3ML/LinearAlgebra/Quaternion.cpp | 92 ++++++++++++++++++++++++- src/J3ML/LinearAlgebra/Transform2D.cpp | 8 ++- src/J3ML/LinearAlgebra/Vector4.cpp | 13 ++++ 8 files changed, 152 insertions(+), 35 deletions(-) diff --git a/include/J3ML/LinearAlgebra.h b/include/J3ML/LinearAlgebra.h index 6fd6231..d36c17c 100644 --- a/include/J3ML/LinearAlgebra.h +++ b/include/J3ML/LinearAlgebra.h @@ -21,10 +21,7 @@ namespace Math float NormalizeToRange(float input, float fromLower, float fromUpper, float toLower, float toUpper); - float NormalizeToRange(float input, const NumberRange& from, const NumberRange& to) - { - - } + float NormalizeToRange(float input, const NumberRange& from, const NumberRange& to); // auto rotation_normalized = NormalizeToRange(inp, {0, 360}, {-1, 1}); inline float Lerp(float a, float b, float t); diff --git a/include/J3ML/LinearAlgebra/AxisAngle.h b/include/J3ML/LinearAlgebra/AxisAngle.h index 240e88a..3a60d73 100644 --- a/include/J3ML/LinearAlgebra/AxisAngle.h +++ b/include/J3ML/LinearAlgebra/AxisAngle.h @@ -9,7 +9,13 @@ namespace LinearAlgebra /// Transitional datatype, not useful for internal representation of rotation /// But has uses for conversion and manipulation. class AxisAngle { - Vector3 Axis; - float Angle; + Vector3 axis; + float angle; + + public: + AxisAngle(); + AxisAngle(Vector3 axis, float angle); + + }; } \ No newline at end of file diff --git a/include/J3ML/LinearAlgebra/EulerAngle.h b/include/J3ML/LinearAlgebra/EulerAngle.h index b9b25a6..3116e6d 100644 --- a/include/J3ML/LinearAlgebra/EulerAngle.h +++ b/include/J3ML/LinearAlgebra/EulerAngle.h @@ -1,5 +1,5 @@ #pragma once - +#include #include namespace LinearAlgebra { diff --git a/include/J3ML/LinearAlgebra/Quaternion.h b/include/J3ML/LinearAlgebra/Quaternion.h index 5fabd3f..06ed631 100644 --- a/include/J3ML/LinearAlgebra/Quaternion.h +++ b/include/J3ML/LinearAlgebra/Quaternion.h @@ -2,9 +2,12 @@ #include +#include +#include + namespace LinearAlgebra { - class Quaternion + class Quaternion : public Vector4 { public: Quaternion(); @@ -12,15 +15,25 @@ namespace LinearAlgebra explicit Quaternion(const Matrix3x3& rotationMtrx); explicit Quaternion(const Matrix4x4& rotationMtrx); // @note The input data is not normalized after construction, this has to be done manually. - Quaternion(float x, float y, float z, float w); + Quaternion(float X, float Y, float Z, float W); + + + // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0. Quaternion(const Vector3& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); } Quaternion(const Vector4& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); } //void Inverse(); + + explicit Quaternion(Vector4 vector4); + + void SetFromAxisAngle(const Vector3 &vector3, float between); + void SetFromAxisAngle(const Vector4 &vector4, float between); + Quaternion Inverse() const; + Quaternion Conjugate() const; + //void Normalize(); - Quaternion Normalize() const; Vector3 GetWorldX() const; Vector3 GetWorldY() const; Vector3 GetWorldZ() const; @@ -35,22 +48,21 @@ namespace LinearAlgebra Quaternion GetInverse() const; Quaternion Lerp(const Quaternion& b, float t) const; - Quaternion Slerp(const Quaternion& target) const; + Quaternion Slerp(const Quaternion& q2, float t) const; - void SetFromAxisAngle(const Vector3& axis, float angle); - - void SetFromAxisAngle(const Vector4& axis, float angle) - { - - } + Quaternion Normalize() const; static Quaternion LookAt(const Vector3& position, const Vector3& direction, const Vector3& axisUp); + // TODO: Document (But do not override!) certain math functions that are the same for Vec4 + // TODO: Double Check which operations need to be overriden for correct behavior! + + // Multiplies two quaternions together. // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations. // The rotation q2 is applied first before q1. - Quaternion operator * (const Quaternion& rhs) const; + Quaternion operator * (const Quaternion& rhs) const; Quaternion operator * (float scalar) const; @@ -59,14 +71,16 @@ namespace LinearAlgebra Vector4 operator * (const Vector4& rhs) const; // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of - Quaternion operator / (const Quaternion& rhs) const; + //Quaternion operator / (const Quaternion& rhs) const; Quaternion operator +(const Quaternion& rhs) const; Quaternion operator +() const; Quaternion operator -() const; - public: - float x = 0; - float y = 0; - float z = 0; - float w = 0; + float Dot(const Quaternion &quaternion) const; + + float Angle() const; + + float AngleBetween(const Quaternion& target) const; + + AxisAngle ToAxisAngle() const; }; } \ No newline at end of file diff --git a/include/J3ML/LinearAlgebra/Vector4.h b/include/J3ML/LinearAlgebra/Vector4.h index 3f1372f..cef20f1 100644 --- a/include/J3ML/LinearAlgebra/Vector4.h +++ b/include/J3ML/LinearAlgebra/Vector4.h @@ -33,7 +33,6 @@ namespace LinearAlgebra { float operator[](std::size_t index) const; bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const; - bool IsNormalized(float epsilonSq = 1e-5f) const; bool IsZero(float epsilonSq = 1e-6f) const; bool IsFinite() const; @@ -85,10 +84,10 @@ namespace LinearAlgebra { Vector4 operator-() const; // Unary - Operator (Negation) public: #if MUTABLE - float x = 0; - float y = 0; - float z = 0; - float w = 0; + float x; + float y; + float z; + float w; #else float x = 0; float y = 0; diff --git a/src/J3ML/LinearAlgebra/Quaternion.cpp b/src/J3ML/LinearAlgebra/Quaternion.cpp index 55756bd..037f363 100644 --- a/src/J3ML/LinearAlgebra/Quaternion.cpp +++ b/src/J3ML/LinearAlgebra/Quaternion.cpp @@ -1,7 +1,7 @@ -#include - #include - +#include +#include +#include namespace LinearAlgebra { Quaternion Quaternion::operator-() const @@ -56,4 +56,90 @@ namespace LinearAlgebra { Quaternion Quaternion::operator+() const { return *this; } Quaternion::Quaternion() {} + + Quaternion::Quaternion(float X, float Y, float Z, float W) : Vector4(X,Y,Z,W) {} + + // TODO: implement + float Quaternion::Dot(const Quaternion &quaternion) const {} + + Quaternion::Quaternion(Vector4 vector4) { + + } + + float Quaternion::Angle() const { + return std::acos(w) * 2.f; + } + + Quaternion Quaternion::Normalize() const { + float length = Length(); + if (length < 1e-4f) + return {0,0,0,0}; + float reciprocal = 1.f / length; + return { + x * reciprocal, + y * reciprocal, + z * reciprocal, + w * reciprocal + }; + } + + Quaternion Quaternion::Conjugate() const { + return { -x, -y, -z, w }; + } + + Quaternion Quaternion::Inverse() const { + return Conjugate(); + } + + Quaternion Quaternion::Slerp(const Quaternion &q2, float t) const { + float angle = this->Dot(q2); + float sign = 1.f; + if (angle < 0.f) + { + angle = -angle; + sign = -1.f; + } + + float a; + float b; + if (angle < 0.999) + { + // angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1. + angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f; + + float ta = t*angle; + // Not using a lookup table, manually compute the two sines by using a very rough approximation. + float ta2 = ta*ta; + b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta; + a = angle - ta; + float a2 = a*a; + a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a; + } + else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization). + { + a = 1.f - t; + b = t; + } + // Lerp and renormalize. + return (*this * (a * sign) + q2 * b).Normalize(); + } + + AxisAngle Quaternion::ToAxisAngle() const { + float halfAngle = std::acos(w); + float angle = halfAngle * 2.f; + // TODO: Can Implement Fast Inverse Sqrt Here + float reciprocalSinAngle = 1.f / std::sqrt(1.f - w*w); + + Vector3 axis = { + x*reciprocalSinAngle, + y*reciprocalSinAngle, + z*reciprocalSinAngle + }; + return {axis, angle}; + } + + float Quaternion::AngleBetween(const Quaternion &target) const { + Quaternion delta = target / *this; + return delta.Normalize().Angle(); + } } \ No newline at end of file diff --git a/src/J3ML/LinearAlgebra/Transform2D.cpp b/src/J3ML/LinearAlgebra/Transform2D.cpp index a8cec1d..4f335ae 100644 --- a/src/J3ML/LinearAlgebra/Transform2D.cpp +++ b/src/J3ML/LinearAlgebra/Transform2D.cpp @@ -1,3 +1,5 @@ -// -// Created by josh on 12/26/2023. -// +#include + +namespace LinearAlgebra { + +} \ No newline at end of file diff --git a/src/J3ML/LinearAlgebra/Vector4.cpp b/src/J3ML/LinearAlgebra/Vector4.cpp index 288be3d..f5f2986 100644 --- a/src/J3ML/LinearAlgebra/Vector4.cpp +++ b/src/J3ML/LinearAlgebra/Vector4.cpp @@ -118,6 +118,19 @@ Vector4 Vector4::operator-(const Vector4& rhs) const return {x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w}; } + Vector4 Vector4::operator*(float rhs) const { + return { + this->x * rhs, + this->y * rhs, + this->z * rhs, + this->w * rhs + }; + } + + bool Vector4::IsWithinMarginOfError(const Vector4 &rhs, float margin) const { + return this->Distance(rhs) <= margin; + } + } #pragma endregion \ No newline at end of file