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/trackFindingCDC/geometry/Vector2D.h>
#include <tracking/trackFindingCDC/numerics/Quadratic.h>
 
#include <framework/geometry/B2Vector3.h>
 
#include <string>
#include <iosfwd>
#include <cmath>
 
class TVector3;
 
namespace Belle2 {
  namespace TrackFindingCDC {
 
 
    class Vector3D {
 
    public:
      Vector3D()
        : m_xy(0.0, 0.0)
        , m_z(0.0)
      {
      }
 
      explicit Vector3D(const TVector3& tVector3);
 
      explicit Vector3D(const B2Vector3D& b2Vector3);
 
      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);
 
 
      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;
 
      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 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 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();
      }
 
      Vector3D parallelVector(const Vector3D& relativTo) const
      {
        return relativTo.scaled(relativTo.dot(*this) / relativTo.normSquared());
      }
 
 
      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 - parallelVector(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;
 
      double m_z;
    };
 
    std::ostream& operator<<(std::ostream& output, const Vector3D& vector3D);
  }
}