diff --git a/Source/SIMPLib/Math/GeometryMath.cpp b/Source/SIMPLib/Math/GeometryMath.cpp index e8863f949..fa4bb8866 100644 --- a/Source/SIMPLib/Math/GeometryMath.cpp +++ b/Source/SIMPLib/Math/GeometryMath.cpp @@ -66,6 +66,19 @@ double GeometryMath::CosThetaBetweenVectors(const double a[3], const double b[3] } return (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) / (norm1 * norm2); } +float GeometryMath::CosThetaBetweenVectors(const Eigen::Vector3f& vectorA, const Eigen::Vector3f& vectorB) +{ + const float normA = vectorA.norm(); + const float normB = vectorB.norm(); + + if(normA == 0.0f || normB == 0.0f) + { + return 1.0f; + } + + return vectorA.dot(vectorB) / (normA * normB); +} + // ----------------------------------------------------------------------------- // // ----------------------------------------------------------------------------- diff --git a/Source/SIMPLib/Math/GeometryMath.h b/Source/SIMPLib/Math/GeometryMath.h index 6a755761b..92e28d932 100644 --- a/Source/SIMPLib/Math/GeometryMath.h +++ b/Source/SIMPLib/Math/GeometryMath.h @@ -37,6 +37,8 @@ #include +#include + #include "SIMPLib/SIMPLib.h" #include "SIMPLib/DataArrays/DynamicListArray.hpp" @@ -62,6 +64,7 @@ namespace GeometryMath */ SIMPLib_EXPORT float CosThetaBetweenVectors(const float a[3], const float b[3]); SIMPLib_EXPORT double CosThetaBetweenVectors(const double a[3], const double b[3]); +SIMPLib_EXPORT float CosThetaBetweenVectors(const Eigen::Vector3f& vectorA, const Eigen::Vector3f& vectorB); /** * @brief Computes the angle in RADIANS between 2 vectors.