From 9c3503f3cb678622fcdc7935b791a28c6c48d052 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 22 Aug 2024 12:28:02 +0200 Subject: [PATCH] Added SphericalOf --- Angle.cpp | 12 +++- Angle.h | 1 + Spherical.cpp | 183 ++++++++++++++++++++++++++++++++++++++++++++++++ Spherical.h | 55 +++++++++++++++ Spherical16.cpp | 26 ++++++- Spherical16.h | 6 +- 6 files changed, 277 insertions(+), 6 deletions(-) diff --git a/Angle.cpp b/Angle.cpp index c808f88..4948086 100644 --- a/Angle.cpp +++ b/Angle.cpp @@ -98,7 +98,7 @@ float AngleOf::ToFloat() const { // AngleOf AngleOf::Deg2Rad = (pi * 2) / 360.0f; template <> -bool Passer::LinearAlgebra::AngleOf::operator==(AngleOf a) { +bool AngleOf::operator==(AngleOf a) { return this->ToFloat() == a.ToFloat(); } @@ -122,6 +122,12 @@ AngleOf AngleOf::operator+(const AngleOf& a) const { return angle; } +template +AngleOf AngleOf::operator+=(const AngleOf& a) { + this->value += a.value; + return *this; +} + template <> AngleOf AngleOf::Normalize(AngleOf angle) { float angleValue = angle.ToFloat(); @@ -200,4 +206,6 @@ AngleOf AngleOf::SineRuleAngle(float a, float deg2rad = Passer::LinearAlgebra::Deg2Rad; float alpha = asinf(a * sinf(beta.ToFloat() * deg2rad) / b); return alpha; -} \ No newline at end of file +} +template class AngleOf; +template class AngleOf; diff --git a/Angle.h b/Angle.h index 328354b..37c2758 100644 --- a/Angle.h +++ b/Angle.h @@ -35,6 +35,7 @@ class AngleOf { AngleOf operator-() const; AngleOf operator-(const AngleOf& a) const; AngleOf operator+(const AngleOf& a) const; + AngleOf operator+=(const AngleOf& a); friend AngleOf operator*(const AngleOf& a, float f) { return AngleOf((float)a.ToFloat() * f); diff --git a/Spherical.cpp b/Spherical.cpp index f1a80d1..dca766b 100644 --- a/Spherical.cpp +++ b/Spherical.cpp @@ -5,6 +5,189 @@ #include +template +SphericalOf::SphericalOf() { + this->distance = 0.0f; + this->horizontal = AngleOf(0); + this->vertical = AngleOf(0); +} + +// template <> +// SphericalOf::SphericalOf() { +// this->distance = 0.0f; +// this->horizontal = AngleOf(0); +// this->vertical = AngleOf(0); +// } + +template +SphericalOf::SphericalOf(float distance, + AngleOf horizontal, + AngleOf vertical) { + this->distance = distance; + this->horizontal = horizontal; + this->vertical = vertical; +} + +// template <> +// SphericalOf::SphericalOf(float distance, +// AngleOf horizontal, +// AngleOf vertical) { +// this->distance = distance; +// this->horizontal = horizontal; +// this->vertical = vertical; +// } + +// template <> +// SphericalOf::SphericalOf(float distance, +// AngleOf horizontal, +// AngleOf vertical) { +// this->distance = distance; +// this->horizontal = horizontal; +// this->vertical = vertical; +// } + +template +SphericalOf SphericalOf::FromVector3(Vector3 v) { + float distance = v.magnitude(); + if (distance == 0.0f) { + return SphericalOf(distance, 0, 0); + } else { + float verticalAngle = + (90.0f - acosf(v.Up() / distance) * Passer::LinearAlgebra::Rad2Deg); + float horizontalAngle = + atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg; + return SphericalOf(distance, horizontalAngle, verticalAngle); + } +} + +template +Vector3 SphericalOf::ToVector3() const { + float verticalRad = + (90.0f - this->vertical.ToFloat()) * Passer::LinearAlgebra::Deg2Rad; + float horizontalRad = + this->horizontal.ToFloat() * Passer::LinearAlgebra::Deg2Rad; + float cosVertical = cosf(verticalRad); + float sinVertical = sinf(verticalRad); + float cosHorizontal = cosf(horizontalRad); + float sinHorizontal = sinf(horizontalRad); + + float x = this->distance * sinVertical * sinHorizontal; + float y = this->distance * cosVertical; + float z = this->distance * sinVertical * cosHorizontal; + + Vector3 v = Vector3(x, y, z); + return Vector3(); +} + +template +const SphericalOf SphericalOf::zero = + SphericalOf(0.0f, AngleOf(0), AngleOf(0)); +template +const SphericalOf SphericalOf::forward = SphericalOf(1.0f, 0.0f, 0.0f); +template +const SphericalOf SphericalOf::back = SphericalOf(1.0f, 180.0f, 0.0f); +template +const SphericalOf SphericalOf::right = SphericalOf(1.0f, 90.0f, 0.0f); +template +const SphericalOf SphericalOf::left = SphericalOf(1.0f, -90.0f, 0.0f); +template +const SphericalOf SphericalOf::up = SphericalOf(1.0f, 0.0f, 90.0f); +template +const SphericalOf SphericalOf::down = SphericalOf(1.0f, 0.0f, -90.0f); + +template <> +const SphericalOf SphericalOf::zero = + SphericalOf(0.0f, AngleOf(0), AngleOf(0)); + +template +SphericalOf SphericalOf::operator-() const { + SphericalOf v = + SphericalOf(this->distance, this->horizontal.ToFloat() + 180.0f, + this->vertical.ToFloat() + 180.0f); + return v; +} + +template +SphericalOf SphericalOf::operator-(const SphericalOf& s2) const { + // let's do it the easy way... + Vector3 v1 = this->ToVector3(); + Vector3 v2 = s2.ToVector3(); + Vector3 v = v1 - v2; + SphericalOf r = SphericalOf::FromVector3(v); + return r; +} +template +SphericalOf SphericalOf::operator-=(const SphericalOf& v) { + *this = *this - v; + return *this; +} + +template +SphericalOf SphericalOf::operator+(const SphericalOf& s2) const { + // let's do it the easy way... + Vector3 v1 = this->ToVector3(); + Vector3 v2 = s2.ToVector3(); + Vector3 v = v1 + v2; + SphericalOf r = SphericalOf::FromVector3(v); + return r; + /* + // This is the hard way... + if (v2.distance <= 0) + return Spherical(this->distance, this->horizontalAngle, + this->verticalAngle); + if (this->distance <= 0) + return v2; + + float deltaHorizontalAngle = + (float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle); + float horizontalRotation = deltaHorizontalAngle < 0 + ? 180 + deltaHorizontalAngle + : 180 - deltaHorizontalAngle; + float deltaVerticalAngle = + Angle::Normalize(v2.verticalAngle - this->verticalAngle); + float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle + : 180 - deltaVerticalAngle; + + if (horizontalRotation == 180 && verticalRotation == 180) + // angle is too small, take this angle and add the distances + return Spherical(this->distance + v2.distance, this->horizontalAngle, + this->verticalAngle); + + Angle rotation = AngleBetween(*this, v2); + float newDistance = + Angle::CosineRuleSide(v2.distance, this->distance, rotation); + float angle = + Angle::CosineRuleAngle(newDistance, this->distance, v2.distance); + + // Now we have to project the angle to the horizontal and vertical planes... + // The axis for the angle is the cross product of the two spherical vectors + // (which function we do not have either...) + float horizontalAngle = 0; + float verticalAngle = 0; + + float newHorizontalAngle = + deltaHorizontalAngle < 0 + ? Angle::Normalize(this->horizontalAngle - horizontalAngle) + : Angle::Normalize(this->horizontalAngle + horizontalAngle); + float newVerticalAngle = + deltaVerticalAngle < 0 + ? Angle::Normalize(this->verticalAngle - verticalAngle) + : Angle::Normalize(this->verticalAngle + verticalAngle); + + Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle); + */ +} +template +SphericalOf SphericalOf::operator+=(const SphericalOf& v) { + *this = *this + v; + return *this; +} + +template class SphericalOf; +template class SphericalOf; + +//--------------------------------------- + Spherical::Spherical() { this->distance = 0.0f; this->horizontalAngle = 0.0f; diff --git a/Spherical.h b/Spherical.h index 112f5e0..37799df 100644 --- a/Spherical.h +++ b/Spherical.h @@ -13,6 +13,61 @@ namespace LinearAlgebra { struct Vector3; +template +class SphericalOf { + public: + /// @brief The distance in meters + /// @remark The distance should never be negative + float distance; + /// @brief The angle in the horizontal plane in degrees, clockwise rotation + /// @details The angle is automatically normalized to -180 .. 180 + AngleOf horizontal; + /// @brief The angle in the vertical plane in degrees. Positive is upward. + /// @details The angle is automatically normalized to -180 .. 180 + AngleOf vertical; + + SphericalOf(); + SphericalOf(float distance, AngleOf horizontal, AngleOf vertical); + + static SphericalOf FromVector3(Vector3 v); + Vector3 ToVector3() const; + + /// @brief A spherical vector with zero degree angles and distance + const static SphericalOf zero; + /// @brief A normalized forward-oriented vector + const static SphericalOf forward; + /// @brief A normalized back-oriented vector + const static SphericalOf back; + /// @brief A normalized right-oriented vector + const static SphericalOf right; + /// @brief A normalized left-oriented vector + const static SphericalOf left; + /// @brief A normalized up-oriented vector + const static SphericalOf up; + /// @brief A normalized down-oriented vector + const static SphericalOf down; + + /// @brief Negate the vector + /// @return The negated vector + /// This will rotate the vector by 180 degrees horizontally and + /// vertically. Distance will stay the same. + SphericalOf operator-() const; + + /// @brief Subtract a spherical vector from this vector + /// @param v The vector to subtract + /// @return The result of the subtraction + SphericalOf operator-(const SphericalOf& v) const; + SphericalOf operator-=(const SphericalOf& v); + /// @brief Add a spherical vector to this vector + /// @param v The vector to add + /// @return The result of the addition + SphericalOf operator+(const SphericalOf& v) const; + SphericalOf operator+=(const SphericalOf& v); +}; + +using SphericalSingle = SphericalOf; +using Spherical16 = SphericalOf; + /// @brief A spherical vector /// @details This is a vector in 3D space using a spherical coordinate system. /// It consists of a distance and the polar and elevation angles from a diff --git a/Spherical16.cpp b/Spherical16.cpp index 5000841..d9f0c8c 100644 --- a/Spherical16.cpp +++ b/Spherical16.cpp @@ -1,3 +1,4 @@ +/* #include "Spherical16.h" #include "Quaternion.h" @@ -44,6 +45,24 @@ Spherical16::Spherical16(Vector3 v) { } } +Vector3 Passer::LinearAlgebra::Spherical16::ToVector3() { + float verticalRad = + (90.0f - this->verticalAngle.ToFloat()) * Passer::LinearAlgebra::Deg2Rad; + float horizontalRad = + this->horizontalAngle.ToFloat() * Passer::LinearAlgebra::Deg2Rad; + float cosVertical = cosf(verticalRad); + float sinVertical = sinf(verticalRad); + float cosHorizontal = cosf(horizontalRad); + float sinHorizontal = sinf(horizontalRad); + + float x = this->distance * sinVertical * sinHorizontal; + float y = this->distance * cosVertical; + float z = this->distance * sinVertical * cosHorizontal; + + Vector3 v = Vector3(x, y, z); + return Vector3(); +} + const Spherical16 Spherical16::zero = Spherical16(0.0f, 0.0f, 0.0f); const Spherical16 Spherical16::forward = Spherical16(1.0f, 0.0f, 0.0f); const Spherical16 Spherical16::back = Spherical16(1.0f, 180.0f, 0.0f); @@ -103,7 +122,7 @@ Spherical16 Spherical16::operator+(const Spherical16& s2) const { Vector3 v = v1 + v2; Spherical16 r = Spherical16(v); return r; - /* + // This is the hard way... if (v2.distance <= 0) return Spherical(this->distance, this->horizontalAngle, @@ -148,7 +167,7 @@ Spherical16 Spherical16::operator+(const Spherical16& s2) const { : Angle::Normalize(this->verticalAngle + verticalAngle); Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle); - */ + } Spherical16 Spherical16::operator+=(const Spherical16& v) { *this = *this + v; @@ -234,4 +253,5 @@ Spherical16 Spherical16::RotateVertical(const Spherical16& v, Angle a) { Spherical16 r = Spherical16(v.distance, v.horizontalAngle, v.verticalAngle.ToFloat() + a.ToFloat()); return r; -} \ No newline at end of file +} +*/ \ No newline at end of file diff --git a/Spherical16.h b/Spherical16.h index 1f9f448..803d179 100644 --- a/Spherical16.h +++ b/Spherical16.h @@ -1,3 +1,4 @@ +/* // 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/. @@ -48,6 +49,8 @@ struct Spherical16 { /// @param v Vector in 3D carthesian coordinates; Spherical16(Vector3 v); + Vector3 ToVector3(); + /// @brief A spherical vector with zero degree angles and distance const static Spherical16 zero; /// @brief A normalized forward-oriented vector @@ -155,4 +158,5 @@ using namespace Passer::LinearAlgebra; #include "Vector3.h" -#endif \ No newline at end of file +#endif +*/ \ No newline at end of file