C++ function to compute angle between two vectors

angle_between_vectors.cpp

  bool angleBetweenVectors(Eigen::Vector3f u, Eigen::Vector3f v, double& angle_rad)
  {
    float dot_product = u.dot(v);
    float norms = u.norm() * v.norm();

    // Avoid division by zero or invalid acos input
    if (norms == 0) {
      RCLCPP_ERROR(this->get_logger(), "One or both vectors have zero magnitude.");
      return false;
    }

    float theta_rad = dot_product / norms;

    // Clamp the value to the range [-1, 1] to avoid precision issues with acos
    angle_rad = std::max(-1.0f, std::min(1.0f, theta_rad));
    angle_rad = std::acos(angle_rad);

    return true;
  }