Vector3D.h

/**************************************************************************
 * basf2 (Belle II Analysis Software Framework)                           *
 * Author: The Belle II Collaboration                                     *
 *                                                                        *
 * See git log for contributors and copyright holders.                    *
 * This file is licensed under LGPL-3.0, see LICENSE.md.                  *
 **************************************************************************/
#pragma once
 
#include <tracking/trackingUtilities/geometry/Vector2D.h>
#include <tracking/trackingUtilities/numerics/Quadratic.h>
 
#include <framework/geometry/B2Vector3.h>
#include <framework/geometry/VectorUtil.h>
#include <Math/Vector3D.h>
 
#include <string>
#include <iosfwd>
#include <cmath>
 
class TVector3;
 
namespace Belle2 {
  namespace TrackingUtilities {


    class Vector3D {
 
    public:
      Vector3D() = default;

      explicit Vector3D(const TVector3& tVector3);

      explicit Vector3D(const B2Vector3D& b2Vector3);

      Vector3D(const ROOT::Math::XYZVector& xyzVector);

      Vector3D(double x, double y, double z)
        : m_xy(x, y)
        , m_z(z)
      {
      }

      explicit Vector3D(const Vector2D& xy)
        : m_xy(xy)
        , m_z(0.0)
      {
      }

      Vector3D(const Vector2D& xy, double z)
        : m_xy(xy)
        , m_z(z)
      {
      }

      Vector3D& operator=(const TVector3& tVector3);

      Vector3D& operator=(const B2Vector3D& b2Vector3);

      Vector3D& operator=(const ROOT::Math::XYZVector& xyzVector);


      static Vector3D average(const Vector3D& one, const Vector3D& two)
      {
        if (one.hasNAN()) {
          return two;
        } else if (two.hasNAN()) {
          return one;
        } else {
          return Vector3D((one.x() + two.x()) / 2.0,
                          (one.y() + two.y()) / 2.0,
                          (one.z() + two.z()) / 2.0);
        }
      }


      static Vector3D average(const Vector3D& one, const Vector3D& two, const Vector3D& three)
      {
 
        if (one.hasNAN()) {
          return average(two, three);
        } else if (two.hasNAN()) {
          return average(one, three);
        } else if (three.hasNAN()) {
          return average(one, two);
        } else {
          return Vector3D((one.x() + two.x() + three.x()) / 3.0,
                          (one.y() + two.y() + three.y()) / 3.0,
                          (one.z() + two.z() + three.z()) / 3.0);
        }
      }

      operator const TVector3() const;

      operator const B2Vector3D() const;

      operator const ROOT::Math::XYZVector() const;

      bool operator==(const Vector3D& rhs) const
      {
        return x() == rhs.x() and y() == rhs.y() and z() == rhs.z();
      }


      bool operator<(const Vector3D& rhs) const
      {
        return norm() < rhs.norm() or (norm() == rhs.norm() and
                                       (z() < rhs.z() or (z() == rhs.z() and (phi() < rhs.phi()))));
      }


      static Vector3D getLowest()
      {
        return Vector3D(0.0, 0.0, 0.0);
      }

      bool isNull() const
      {
        return x() == 0.0 and y() == 0.0 and z() == 0.0;
      }

      bool hasNAN() const
      {
        return std::isnan(x()) or std::isnan(y()) or std::isnan(z());
      }

      std::string __str__() const;

      double dot(const Vector3D& rhs) const
      {
        return x() * rhs.x() + y() * rhs.y() + z() * rhs.z();
      }

      double Dot(const Vector3D& rhs) const
      {
        return dot(rhs);
      }

      double dotXY(const Vector3D& rhs) const
      {
        return x() * rhs.x() + y() * rhs.y();
      }

      Vector3D cross(const Vector3D& rhs) const
      {
        return Vector3D(y() * rhs.z() - z() * rhs.y(),
                        z() * rhs.x() - x() * rhs.z(),
                        x() * rhs.y() - y() * rhs.x());
      }

      double crossXY(const Vector3D& rhs) const
      {
        return xy().cross(rhs.xy());
      }

      double crossXY(const Vector2D& rhs) const
      {
        return xy().cross(rhs);
      }

      double normSquared() const
      {
        return x() * x() + y() * y() + z() * z();
      }

      double Mag2() const
      {
        return normSquared();
      }

      double norm() const
      {
        return hypot3(x(), y(), z());
      }

      double cosWith(const Vector3D& rhs) const
      {
        return dot(rhs) / (norm() * rhs.norm());
      }

      double sinWith(const Vector3D& rhs) const
      {
        return cross(rhs).norm() / (norm() * rhs.norm());
      }

      double angleWith(const Vector3D& rhs) const
      {
        return atan2(sinWith(rhs), cosWith(rhs));
      }


      double distance(const Vector3D& rhs = Vector3D(0.0, 0.0, 0.0)) const
      {
        double deltaX = x() - rhs.x();
        double deltaY = y() - rhs.y();
        double deltaZ = z() - rhs.z();
        return hypot3(deltaX, deltaY, deltaZ);
      }

      Vector3D& scale(const double factor)
      {
        m_xy.scale(factor);
        m_z *= factor;
        return *this;
      }

      Vector3D& operator*=(const double factor)
      {
        return scale(factor);
      }

      Vector3D scaled(const double factor) const
      {
        return Vector3D(xy().scaled(factor), z() * factor);
      }

      Vector3D operator*(const double factor) const
      {
        return scaled(factor);
      }

      Vector3D& divide(const double denominator)
      {
        m_xy.divide(denominator);
        m_z /= denominator;
        return *this;
      }

      Vector3D& operator/=(const double denominator)
      {
        return divide(denominator);
      }

      Vector3D divided(const double denominator) const
      {
        return Vector3D(xy().divided(denominator), z() / denominator);
      }

      Vector3D operator/(const double denominator) const
      {
        return divided(denominator);
      }

      Vector3D& add(const Vector3D& rhs)
      {
        m_xy.add(rhs.xy());
        m_z += rhs.z();
        return *this;
      }

      Vector3D& add(const Vector2D& rhs)
      {
        m_xy.add(rhs);
        return *this;
      }

      Vector3D& operator+=(const Vector3D& rhs)
      {
        return add(rhs);
      }

      Vector3D& operator+=(const Vector2D& rhs)
      {
        return add(rhs);
      }

      Vector3D& subtract(const Vector3D& rhs)
      {
        m_xy.subtract(rhs.xy());
        m_z -= rhs.z();
        return *this;
      }

      Vector3D& subtract(const Vector2D& rhs)
      {
        m_xy.subtract(rhs);
        return *this;
      }

      Vector3D& operator-=(const Vector3D& rhs)
      {
        return subtract(rhs);
      }

      Vector3D& operator-=(const Vector2D& rhs)
      {
        return subtract(rhs);
      }

      Vector3D unit() const
      {
        return isNull() ? Vector3D(0.0, 0.0, 0.0) : divided(norm());
      }


      double normalize()
      {
        double result = norm();
        if (result != 0.0) divide(result);
        return result;
      }


      double normalizeTo(const double toLength)
      {
        double result = norm();
        if (result != 0.0) scale(toLength / result);
        return result;
      }

      Vector3D& reverse()
      {
        scale(-1.0);
        return *this;
      }

      Vector3D reversed() const
      {
        return scaled(-1.0);
      }

      Vector3D operator-() const
      {
        return reversed();
      }

      Vector3D operator+(const Vector3D& rhs) const
      {
        return Vector3D(xy() + rhs.xy(), z() + rhs.z());
      }

      Vector3D operator-(const Vector3D& rhs) const
      {
        return Vector3D(xy() - rhs.xy(), z() - rhs.z());
      }

      double parallelComp(const Vector3D& relativTo) const
      {
        return relativTo.dot(*this) / relativTo.norm();
      }


      double unnormalizedParallelComp(const Vector3D& relativTo) const
      {
        return relativTo.dot(*this);
      }


      double orthogonalComp(const Vector3D& relativTo) const
      {
        return relativTo.cross(*this).norm() / relativTo.norm();
      }

      Vector3D orthogonalVector(const Vector3D& relativTo) const
      {
        return *this - Belle2::VectorUtil::parallelVector(*this, relativTo);
      }


      double unnormalizedOrthogonalComp(const Vector3D& relativTo) const
      {
        return relativTo.cross(*this).norm();
      }

      void passiveMoveBy(const Vector3D& by)
      {
        subtract(by);
      }

      Vector3D passiveMovedBy(const Vector3D& by)
      {
        return *this - by;
      }

      double x() const
      {
        return m_xy.x();
      }

      void setX(const double x)
      {
        m_xy.setX(x);
      }

      double y() const
      {
        return m_xy.y();
      }

      void setY(const double y)
      {
        m_xy.setY(y);
      }

      double z() const
      {
        return m_z;
      }

      void setZ(const double z)
      {
        m_z = z;
      }

      const Vector2D& xy() const
      {
        return m_xy;
      }

      void setXY(const Vector2D& xy)
      {
        m_xy = xy;
      }

      void set(const double first, const double second, const double third)
      {
        setX(first);
        setY(second);
        setZ(third);
      }

      double cylindricalRSquared() const
      {
        return xy().normSquared();
      }

      double cylindricalR() const
      {
        return xy().norm();
      }

      double phi() const
      {
        return xy().phi();
      }

      double theta() const
      {
        return atan2(cylindricalR(), z());
      }

      double lambda() const
      {
        return atan2(z(), cylindricalR());
      }

      double cotTheta() const
      {
        return z() / cylindricalR();
      }

      double tanLambda() const
      {
        return z() / cylindricalR();
      }
 
    private:
      Vector2D m_xy{0.0, 0.0};

      double m_z = 0.0;
    };

    template<class Vector>
    Vector3D operator- (const Vector& a, const Vector3D& b)
    {
      return Vector3D(a.X() - b.x(), a.Y() - b.y(), a.Z() - b.z());
    }

    std::ostream& operator<<(std::ostream& output, const Vector3D& vector3D);
  }
}