diff --git a/src/unitvector.hh b/src/unitvector.hh index 9826c247e8c4bd8efd7cde571f467ce163a94bc5..ba3321fc7eba1f640fc3ca027738154dbda997e2 100644 --- a/src/unitvector.hh +++ b/src/unitvector.hh @@ -32,6 +32,16 @@ public: return std::acos(a.data_ * b.data_); } + /** \brief Compute the gradient of the squared distance function keeping the first argument fixed + + Unlike the distance itself the squared distance is differentiable at zero + */ + static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const UnitVector& a, const UnitVector& b) { + EmbeddedTangentVector result = a.data_; + result *= -2*std::acos(a.data_ * b.data_) / std::sqrt(1-(a.data_*b.data_)*(a.data_*b.data_)); + return result; + } + /** \brief Write LocalKey object to output stream */ friend std::ostream& operator<< (std::ostream& s, const UnitVector& unitVector) {