diff --git a/Spherical.cpp b/Spherical.cpp index 92efeb9..5dabd01 100644 --- a/Spherical.cpp +++ b/Spherical.cpp @@ -203,6 +203,51 @@ SphericalOf SphericalOf::operator/=(float f) { return *this; } +#include "FloatSingle.h" +#include "Vector3.h" + +const float epsilon = 1E-05f; + +template +AngleOf SphericalOf::AngleBetween(const SphericalOf& v1, + const SphericalOf& v2) { + float denominator = v1.distance * v2.distance; + if (denominator < epsilon) + return 0.0f; + + Vector3 v1_3 = v1.ToVector3(); + Vector3 v2_3 = v2.ToVector3(); + float dot = Vector3::Dot(v1_3, v2_3); + float fraction = dot / denominator; + if (isnan(fraction)) + return fraction; // short cut to returning NaN universally + + float cdot = Float::Clamp(fraction, -1.0, 1.0); + float r = ((float)acos(cdot)) * Rad2Deg; + return r; +} + +template +SphericalOf SphericalOf::Rotate(const SphericalOf& v, + AngleOf horizontalAngle, + AngleOf verticalAngle) { + SphericalOf r = SphericalOf(v.distance, v.horizontal + horizontalAngle, + v.vertical + verticalAngle); + return r; +} +template +SphericalOf SphericalOf::RotateHorizontal(const SphericalOf& v, + AngleOf a) { + SphericalOf r = SphericalOf(v.distance, v.horizontal + a, v.vertical); + return r; +} +template +SphericalOf SphericalOf::RotateVertical(const SphericalOf& v, + AngleOf a) { + SphericalOf r = SphericalOf(v.distance, v.horizontal, v.vertical + a); + return r; +} + template class SphericalOf; template class SphericalOf; diff --git a/Spherical.h b/Spherical.h index 06956ec..fa4b79f 100644 --- a/Spherical.h +++ b/Spherical.h @@ -94,6 +94,25 @@ class SphericalOf { v.vertical); // not correct, should be f / v.distance } SphericalOf operator/=(float f); + + /// + /// The distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + // static float Distance(const Spherical &s1, const Spherical &s2); + + static AngleOf AngleBetween(const SphericalOf& v1, + const SphericalOf& v2); + + static SphericalOf Rotate(const SphericalOf& v, + AngleOf horizontalAngle, + AngleOf verticalAngle); + static SphericalOf RotateHorizontal(const SphericalOf& v, + AngleOf angle); + static SphericalOf RotateVertical(const SphericalOf& v, + AngleOf angle); }; using SphericalSingle = SphericalOf;