From c7aef869a0c42495dfebbf49e4a2e71acd886927 Mon Sep 17 00:00:00 2001 From: josh Date: Mon, 27 May 2024 13:35:35 -0400 Subject: [PATCH] Implement Vector3::Orthonormalize() AreOrthonormal() --- include/J3ML/LinearAlgebra/Vector3.h | 38 ++++++++++++++++++++++---- src/J3ML/LinearAlgebra/Vector3.cpp | 41 ++++++++++++++++++++++------ 2 files changed, 65 insertions(+), 14 deletions(-) diff --git a/include/J3ML/LinearAlgebra/Vector3.h b/include/J3ML/LinearAlgebra/Vector3.h index 5db6207..6123547 100644 --- a/include/J3ML/LinearAlgebra/Vector3.h +++ b/include/J3ML/LinearAlgebra/Vector3.h @@ -87,7 +87,7 @@ public: // Constructs a new Vector3 with the value (X, Y, Z) Vector3(float X, float Y, float Z); Vector3(const Vector2& XY, float Z); - Vector3(const Vector3& rhs); // Copy Constructor + Vector3(const Vector3& rhs) = default; // Copy Constructor Vector3(Vector3&&) = default; // Move Constructor /// Constructs this float3 from a C array, to the value (data[0], data[1], data[2]). /** @param data An array containing three elements for x, y and z. This pointer may not be null. */ @@ -126,7 +126,19 @@ public: float At(int index) const; float &At(int index); + /// Makes the given vectors linearly independent and normalized in length. + /** This function directly follows the Gram-Schmidt procedure on the input vectors. + The vector a is first normalized, and vector b is modified to be perpendicular to a, and also normalized. + Finally, if specified, the vector c is adjusted to be perpendicular to a and b, and normalized. + @note If any of the input vectors is zero, then the resulting set of vectors cannot be made orthonormal. + @see Orthogonalize(), AreOrthogonal(), AreOrthonormal(). */ static void Orthonormalize(Vector3& a, Vector3& b); + static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c); + + /// Returns true if the given vectors are orthogonal to each other and all of length 1. + /** @see Orthogonalize(), AreOrthogonal(), Orthonormalize(), AreCollinear(). */ + static bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon = 1e-3f); + static bool AreOrthonormal(const Vector3& a, const Vector3& b, const Vector3& c, float epsilon = 1e-3f); Vector3 Abs() const; static Vector3 Abs(const Vector3& rhs); @@ -134,9 +146,9 @@ public: /// Returns the DirectionVector for a given angle. static Vector3 Direction(const Vector3 &rhs) ; - static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c); - bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon); + + Vector3 ProjectToNorm(const Vector3& direction) const; @@ -226,8 +238,23 @@ public: static Vector3 Cross(const Vector3& lhs, const Vector3& rhs); /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction" - Vector3 Normalize() const; - static Vector3 Normalize(const Vector3& targ); + /// @note If the vector is zero and cannot be normalized, the vector (1, 0, 0) is returned, and an error message is printed. + /// If you do not want to generate an error message on failure, but want to handle the failure yourself, use the Normalize() function instead. + /// @see Normalize() + Vector3 Normalized() const; + static Vector3 Normalized(const Vector3& targ); + + /// Normalizes this Vector3. + /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an + un-normalized vector. + @note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and + an error code 0 is returned. This is different than the behavior of the Normalized() function, which prints an + error if normalization fails. + @note This function operates in-place. + @return The old length of this vector, or 0 if normalization fails. + @see Normalized(). */ + float Normalize(); + /// Linearly interpolates between two points. /// Interpolates between the points and b by the interpolant t. @@ -290,6 +317,7 @@ public: Vector3& operator*=(float scalar); Vector3& operator/=(float scalar); + void Set(float d, float d1, float d2); }; diff --git a/src/J3ML/LinearAlgebra/Vector3.cpp b/src/J3ML/LinearAlgebra/Vector3.cpp index a4b35d4..108eca9 100644 --- a/src/J3ML/LinearAlgebra/Vector3.cpp +++ b/src/J3ML/LinearAlgebra/Vector3.cpp @@ -64,7 +64,7 @@ namespace J3ML::LinearAlgebra { Vector3::Vector3(float X, float Y, float Z): x(X), y(Y), z(Z) {} - Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {} + //Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {} Vector3& Vector3::operator=(const Vector3& rhs) { @@ -178,7 +178,21 @@ namespace J3ML::LinearAlgebra { }; } - Vector3 Vector3::Normalize() const + float Vector3::Normalize() + { + assert(IsFinite()); + float length = Length(); + if (length > 1e-6f) + { + *this *= 1.f / length; + return length; + } else { + Set(1.f, 0.f, 0.f); // We will always produce a normalized vector. + return 0; // But signal failure, so user knows we have generated an arbitrary normalization. + } + } + + Vector3 Vector3::Normalized() const { if (Length() > 0) return { @@ -248,8 +262,8 @@ namespace J3ML::LinearAlgebra { return lhs.Cross(rhs); } - Vector3 Vector3::Normalize(const Vector3 &targ) { - return targ.Normalize(); + Vector3 Vector3::Normalized(const Vector3 &targ) { + return targ.Normalized(); } Vector3 Vector3::Project(const Vector3 &lhs, const Vector3 &rhs) { @@ -329,18 +343,18 @@ namespace J3ML::LinearAlgebra { } void Vector3::Orthonormalize(Vector3 &a, Vector3 &b) { - a = a.Normalize(); + a = a.Normalized(); b = b - b.ProjectToNorm(a); - b = b.Normalize(); + b = b.Normalized(); } void Vector3::Orthonormalize(Vector3 &a, Vector3 &b, Vector3 &c) { - a = a.Normalize(); + a = a.Normalized(); b = b - b.ProjectToNorm(a); - b = b.Normalize(); + b = b.Normalized(); c = c - c.ProjectToNorm(a); c = c - c.ProjectToNorm(b); - c = c.Normalize(); + c = c.Normalized(); } Vector3 Vector3::ProjectToNorm(const Vector3 &direction) const { @@ -506,5 +520,14 @@ namespace J3ML::LinearAlgebra { return from.DistanceSquared(to); } + bool Vector3::AreOrthonormal(const Vector3 &a, const Vector3 &b, const Vector3 &c, float epsilon) { + return a.IsPerpendicular(b, epsilon) && + a.IsPerpendicular(c, epsilon) && + b.IsPerpendicular(c, epsilon) && + a.IsNormalized(epsilon*epsilon) && + b.IsNormalized(epsilon*epsilon) && + c.IsNormalized(epsilon*epsilon); + } + } \ No newline at end of file