text: /*=========================================================================
   *
   *  Copyright NumFOCUS
   *
   *  Licensed under the Apache License, Version 2.0 (the "License");
   *  you may not use this file except in compliance with the License.
   *  You may obtain a copy of the License at
   *
   *         https://www.apache.org/licenses/LICENSE-2.0.txt
   *
   *  Unless required by applicable law or agreed to in writing, software
   *  distributed under the License is distributed on an "AS IS" BASIS,
   *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   *  See the License for the specific language governing permissions and
   *  limitations under the License.
   *
   *=========================================================================*/
  #ifndef itkVersor_hxx
  #define itkVersor_hxx
  
  #include "itkNumericTraits.h"
  #include "itkMath.h"
  #include <vnl/vnl_det.h>
  
  namespace itk
  {
  /** Constructor to initialize entire vector to one value. */
  template <typename T>
  Versor<T>::Versor()
    : m_X(NumericTraits<T>::ZeroValue())
    , m_Y(NumericTraits<T>::ZeroValue())
    , m_Z(NumericTraits<T>::ZeroValue())
    , m_W(NumericTraits<T>::OneValue())
  {}
  
  /** Copy Constructor */
  template <typename T>
  Versor<T>::Versor(const Self & v)
  {
    m_X = v.m_X;
    m_Y = v.m_Y;
    m_Z = v.m_Z;
    m_W = v.m_W;
  }
  
  /** Assignment Operator */
  template <typename T>
  Versor<T> &
  Versor<T>::operator=(const Self & v)
  {
    m_X = v.m_X;
    m_Y = v.m_Y;
    m_Z = v.m_Z;
    m_W = v.m_W;
    return *this;
  }
  
  /** Set to an identity transform */
  template <typename T>
  void
  Versor<T>::SetIdentity()
  {
    m_X = NumericTraits<T>::ZeroValue();
    m_Y = NumericTraits<T>::ZeroValue();
    m_Z = NumericTraits<T>::ZeroValue();
    m_W = NumericTraits<T>::OneValue();
  }
  
  /** Return a vnl_quaternion */
  template <typename T>
  vnl_quaternion<T>
  Versor<T>::GetVnlQuaternion() const
  {
    return vnl_quaternion<T>(m_X, m_Y, m_Z, m_W);
  }
  
  /** Assignment and Composition Operator */
  template <typename T>
  const Versor<T> &
  Versor<T>::operator*=(const Self & v)
  {
    const double mx = m_W * v.m_X - m_Z * v.m_Y + m_Y * v.m_Z + m_X * v.m_W;
    const double my = m_Z * v.m_X + m_W * v.m_Y - m_X * v.m_Z + m_Y * v.m_W;
    const double mz = -m_Y * v.m_X + m_X * v.m_Y + m_W * v.m_Z + m_Z * v.m_W;
    const double mw = -m_X * v.m_X - m_Y * v.m_Y - m_Z * v.m_Z + m_W * v.m_W;
  
    m_X = mx;
    m_Y = my;
    m_Z = mz;
    m_W = mw;
  
    return *this;
  }
  
  /** Composition Operator */
  template <typename T>
  Versor<T> Versor<T>::operator*(const Self & v) const
  {
    Self result;
  
    result.m_X = m_W * v.m_X - m_Z * v.m_Y + m_Y * v.m_Z + m_X * v.m_W;
    result.m_Y = m_Z * v.m_X + m_W * v.m_Y - m_X * v.m_Z + m_Y * v.m_W;
    result.m_Z = -m_Y * v.m_X + m_X * v.m_Y + m_W * v.m_Z + m_Z * v.m_W;
    result.m_W = -m_X * v.m_X - m_Y * v.m_Y - m_Z * v.m_Z + m_W * v.m_W;
  
    return result;
  }
  
  /** Division and Assignment Operator */
  template <typename T>
  const Versor<T> &
  Versor<T>::operator/=(const Self & v)
  {
    const double mx = -m_W * v.m_X + m_Z * v.m_Y - m_Y * v.m_Z + m_X * v.m_W;
    const double my = -m_Z * v.m_X - m_W * v.m_Y + m_X * v.m_Z + m_Y * v.m_W;
    const double mz = m_Y * v.m_X - m_X * v.m_Y - m_W * v.m_Z + m_Z * v.m_W;
    const double mw = m_X * v.m_X + m_Y * v.m_Y + m_Z * v.m_Z + m_W * v.m_W;
  
    m_X = mx;
    m_Y = my;
    m_Z = mz;
    m_W = mw;
  
    return *this;
  }
  
  /** Division Operator  */
  template <typename T>
  Versor<T>
  Versor<T>::operator/(const Self & v) const
  {
    Self result;
  
    result.m_X = -m_W * v.m_X + m_Z * v.m_Y - m_Y * v.m_Z + m_X * v.m_W;
    result.m_Y = -m_Z * v.m_X - m_W * v.m_Y + m_X * v.m_Z + m_Y * v.m_W;
    result.m_Z = m_Y * v.m_X - m_X * v.m_Y - m_W * v.m_Z + m_Z * v.m_W;
    result.m_W = m_X * v.m_X + m_Y * v.m_Y + m_Z * v.m_Z + m_W * v.m_W;
  
    return result;
  }
  
  /** Comparison operator */
  template <typename T>
  bool
  Versor<T>::operator==(const Self & v) const
  {
    // Evaluate the quaternion ratio between them
    Self ratio = *this * v.GetReciprocal();
  
    const typename itk::NumericTraits<T>::AccumulateType square = ratio.m_W * ratio.m_W;
  
    const double epsilon = 1e-300;
  
    if (itk::Math::abs(1.0f - square) < epsilon)
    {
      return true;
    }
  
    return false;
  }
  
  /** Get Conjugate */
  template <typename T>
  Versor<T>
  Versor<T>::GetConjugate() const
  {
    Self result;
  
    result.m_X = -m_X;
    result.m_Y = -m_Y;
    result.m_Z = -m_Z;
    result.m_W = m_W;
  
    return result;
  }
  
  /** Get Reciprocal */
  template <typename T>
  Versor<T>
  Versor<T>::GetReciprocal() const
  {
    Self result;
  
    result.m_X = -m_X;
    result.m_Y = -m_Y;
    result.m_Z = -m_Z;
    result.m_W = m_W;
  
    return result;
  }
  
  /** Get Tensor part */
  template <typename T>
  auto
  Versor<T>::GetTensor() const -> ValueType
  {
    const auto tensor = static_cast<ValueType>(std::sqrt(m_X * m_X + m_Y * m_Y + m_Z * m_Z + m_W * m_W));
  
    return tensor;
  }
  
  /** Normalize */
  template <typename T>
  void
  Versor<T>::Normalize()
  {
    const ValueType tensor = this->GetTensor();
  
    if (itk::Math::abs(tensor) < 1e-20)
    {
      ExceptionObject except;
      except.SetDescription("Attempt to normalize a \
                             itk::Versor with zero tensor");
      except.SetLocation(__FILE__);
      throw except;
    }
    m_X /= tensor;
    m_Y /= tensor;
    m_Z /= tensor;
    m_W /= tensor;
  }
  
  /** Get Axis */
  template <typename T>
  auto
  Versor<T>::GetAxis() const -> VectorType
  {
    VectorType axis;
  
    const auto ax = static_cast<RealType>(m_X);
    const auto ay = static_cast<RealType>(m_Y);
    const auto az = static_cast<RealType>(m_Z);
  
    const RealType vectorNorm = std::sqrt(ax * ax + ay * ay + az * az);
  
    if (vectorNorm == NumericTraits<RealType>::ZeroValue())
    {
      axis[0] = NumericTraits<T>::ZeroValue();
      axis[1] = NumericTraits<T>::ZeroValue();
      axis[2] = NumericTraits<T>::ZeroValue();
    }
    else
    {
      axis[0] = m_X / vectorNorm;
      axis[1] = m_Y / vectorNorm;
      axis[2] = m_Z / vectorNorm;
    }
  
    return axis;
  }
  
  /** Get Right part */
  template <typename T>
  auto
  Versor<T>::GetRight() const -> VectorType
  {
    VectorType axis;
  
    axis[0] = m_X;
    axis[1] = m_Y;
    axis[2] = m_Z;
  
    return axis;
  }
  
  /** Get Scalar part */
  template <typename T>
  auto
  Versor<T>::GetScalar() const -> ValueType
  {
    return m_W;
  }
  
  /** Get Angle (in radians) */
  template <typename T>
  auto
  Versor<T>::GetAngle() const -> ValueType
  {
    const auto ax = static_cast<RealType>(m_X);
    const auto ay = static_cast<RealType>(m_Y);
    const auto az = static_cast<RealType>(m_Z);
  
    const RealType vectorNorm = std::sqrt(ax * ax + ay * ay + az * az);
  
    const ValueType angle = 2.0 * std::atan2(vectorNorm, static_cast<RealType>(m_W));
  
    return angle;
  }
  
  /** Get the Square root of the unit quaternion */
  template <typename T>
  Versor<T>
  Versor<T>::SquareRoot() const
  {
    const ValueType newScalar = std::sqrt(static_cast<double>(1.0 + m_W));
    const double    sqrtOfTwo = std::sqrt(2.0f);
  
    const double factor = 1.0f / (newScalar * sqrtOfTwo);
  
    Self result;
  
    result.m_X = m_X * factor;
    result.m_Y = m_Y * factor;
    result.m_Z = m_Z * factor;
    result.m_W = newScalar / sqrtOfTwo;
  
    return result;
  }
  
  /** Compute the Exponential of the quaternion */
  template <typename T>
  Versor<T>
  Versor<T>::Exponential(ValueType exponent) const
  {
    Self result;
  
    result.Set(this->GetAxis(), this->GetAngle() * exponent);
  
    return result;
  }
  
  /** Set Axis and Angle (in radians) */
  template <typename T>
  void
  Versor<T>::Set(const VectorType & axis, ValueType angle)
  {
    const RealType vectorNorm = axis.GetNorm();
    if (Math::FloatAlmostEqual<T>(vectorNorm, 0.0))
    {
      ExceptionObject except;
      except.SetDescription("Attempt to set rotation axis with zero norm");
      except.SetLocation(__FILE__);
      throw except;
    }
  
    const RealType cosangle2 = std::cos(angle / 2.0);
    const RealType sinangle2 = std::sin(angle / 2.0);
  
    const RealType factor = sinangle2 / vectorNorm;
  
    m_X = axis[0] * factor;
    m_Y = axis[1] * factor;
    m_Z = axis[2] * factor;
  
    m_W = cosangle2;
  }
  
  /**  Set using an orthogonal matrix. */
  template <typename T>
  void
  Versor<T>::Set(const MatrixType & mat)
  {
    // const double epsilon = 1e-30;
    // Keep the epsilon value large enough so that the alternate routes of
    // computing the quaternion are used to within floating point precision of the
    // math to be used.  Using 1e-30 results in degenerate matries for rotations
    // near itk::Math::pi due to imprecision of the math.  0.5/std::sqrt(trace) is
    // not accurate to 1e-30, so the resulting matrices would have very large
    // errors.  By decreasing this epsilon value to a higher tolerance, the
    // alternate stable methods for conversion are used.
    //
    // The use of std::numeric_limits< T >::epsilon() was not consistent with
    // the rest of the ITK toolkit with respect to epsilon values for
    // determining rotational orthogonality, and it occasionally
    // prevented the conversion between different rigid transform types.
  
    const T epsilon = Self::Epsilon(); // vnl_sqrt( std::numeric_limits< T >::epsilon() );
    // Use a slightly less epsilon for detecting difference
    const T epsilonDiff = Self::Epsilon(); // std::numeric_limits< T >::epsilon() * 10.0;
  
    const vnl_matrix<T> m(mat.GetVnlMatrix());
  
    // check for orthonormality and that it isn't a reflection
    const vnl_matrix_fixed<T, 3, 3> & I = m * m.transpose();
    if (itk::Math::abs(I[0][1]) > epsilon || itk::Math::abs(I[0][2]) > epsilon || itk::Math::abs(I[1][0]) > epsilon ||
        itk::Math::abs(I[1][2]) > epsilon || itk::Math::abs(I[2][0]) > epsilon || itk::Math::abs(I[2][1]) > epsilon ||
        itk::Math::abs(I[0][0] - itk::NumericTraits<T>::OneValue()) > epsilonDiff ||
        itk::Math::abs(I[1][1] - itk::NumericTraits<T>::OneValue()) > epsilonDiff ||
        itk::Math::abs(I[2][2] - itk::NumericTraits<T>::OneValue()) > epsilonDiff || vnl_det(I) < 0)
    {
      itkGenericExceptionMacro(<< "The following matrix does not represent rotation to within an epsion of " << epsilon
                               << "." << std::endl
                               << m << std::endl
                               << "det(m * m transpose) is: " << vnl_det(I) << std::endl
                               << "m * m transpose is:" << std::endl
                               << I << std::endl);
    }
  
    const double trace = m(0, 0) + m(1, 1) + m(2, 2) + 1.0;
  
    if (trace > epsilon)
    {
      const double s = 0.5 / std::sqrt(trace);
      m_W = 0.25 / s;
      m_X = (m(2, 1) - m(1, 2)) * s;
      m_Y = (m(0, 2) - m(2, 0)) * s;
      m_Z = (m(1, 0) - m(0, 1)) * s;
    }
    else
    {
      if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
      {
        const double s = 2.0 * std::sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2));
        m_X = 0.25 * s;
        m_Y = (m(0, 1) + m(1, 0)) / s;
        m_Z = (m(0, 2) + m(2, 0)) / s;
        m_W = (m(1, 2) - m(2, 1)) / s;
      }
      else
      {
        if (m(1, 1) > m(2, 2))
        {
          const double s = 2.0 * std::sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2));
          m_X = (m(0, 1) + m(1, 0)) / s;
          m_Y = 0.25 * s;
          m_Z = (m(1, 2) + m(2, 1)) / s;
          m_W = (m(0, 2) - m(2, 0)) / s;
        }
        else
        {
          const double s = 2.0 * std::sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1));
          m_X = (m(0, 2) + m(2, 0)) / s;
          m_Y = (m(1, 2) + m(2, 1)) / s;
          m_Z = 0.25 * s;
          m_W = (m(0, 1) - m(1, 0)) / s;
        }
      }
    }
    this->Normalize();
  }
  
  /** Set right Part (in radians) */
  template <typename T>
  void
  Versor<T>::Set(const VectorType & axis)
  {
    const ValueType sinangle2 = axis.GetNorm();
    if (sinangle2 > NumericTraits<ValueType>::OneValue())
    {
      ExceptionObject exception;
      exception.SetDescription("Trying to initialize a Versor with "
                               "a vector whose magnitude is greater than 1");
      exception.SetLocation("itk::Versor::Set( const VectorType )");
      throw exception;
    }
  
    const ValueType cosangle2 = std::sqrt(1.0 - sinangle2 * sinangle2);
  
    m_X = axis[0];
    m_Y = axis[1];
    m_Z = axis[2];
  
    m_W = cosangle2;
  }
  
  /** Set the Versor from four components.
   *  After assignment, the quaternion is normalized
   *  in order to get a consistent Versor (unit quaternion). */
  template <typename T>
  void
  Versor<T>::Set(T x, T y, T z, T w)
  {
    //
    // We assume in this class that the W component is always non-negative.
    // The rotation represented by a Versor remains unchanged if all its
    // four components are negated simultaneously. Therefore, if we are
    // requested to initialize a Versor with a negative W, we negate the
    // signs of all the components.
    //
    if (w < 0.0)
    {
      m_X = -x;
      m_Y = -y;
      m_Z = -z;
      m_W = -w;
    }
    else
    {
      m_X = x;
      m_Y = y;
      m_Z = z;
      m_W = w;
    }
    this->Normalize();
  }
  
  /** Set from a vnl_quaternion
   *  After assignment, the quaternion is normalized
   *  in order to get a consistent Versor (unit quaternion). */
  template <typename T>
  void
  Versor<T>::Set(const VnlQuaternionType & quaternion)
  {
    m_X = quaternion.x();
    m_Y = quaternion.y();
    m_Z = quaternion.z();
    m_W = quaternion.r();
    this->Normalize();
  }
  
  /** Set rotation around X axis */
  template <typename T>
  void
  Versor<T>::SetRotationAroundX(ValueType angle)
  {
    const ValueType sinangle2 = std::sin(angle / 2.0);
    const ValueType cosangle2 = std::cos(angle / 2.0);
  
    m_X = sinangle2;
    m_Y = NumericTraits<T>::ZeroValue();
    m_Z = NumericTraits<T>::ZeroValue();
    m_W = cosangle2;
  }
  
  /** Set rotation around Y axis  */
  template <typename T>
  void
  Versor<T>::SetRotationAroundY(ValueType angle)
  {
    const ValueType sinangle2 = std::sin(angle / 2.0);
    const ValueType cosangle2 = std::cos(angle / 2.0);
  
    m_X = NumericTraits<T>::ZeroValue();
    m_Y = sinangle2;
    m_Z = NumericTraits<T>::ZeroValue();
    m_W = cosangle2;
  }
  
  /**  Set rotation around Z axis  */
  template <typename T>
  void
  Versor<T>::SetRotationAroundZ(ValueType angle)
  {
    const ValueType sinangle2 = std::sin(angle / 2.0);
    const ValueType cosangle2 = std::cos(angle / 2.0);
  
    m_X = NumericTraits<T>::ZeroValue();
    m_Y = NumericTraits<T>::ZeroValue();
    m_Z = sinangle2;
    m_W = cosangle2;
  }
  
  namespace
  {
  template <typename InputVectorType, typename ValueType, typename OutputVectorType>
  inline const OutputVectorType
  localTransformVectorMath(const InputVectorType & VectorObject,
                           const ValueType &       inputX,
                           const ValueType &       inputY,
                           const ValueType &       inputZ,
                           const ValueType &       inputW)
  {
    const ValueType xx = inputX * inputX;
    const ValueType yy = inputY * inputY;
    const ValueType zz = inputZ * inputZ;
    const ValueType xy = inputX * inputY;
    const ValueType xz = inputX * inputZ;
    const ValueType xw = inputX * inputW;
    const ValueType yz = inputY * inputZ;
    const ValueType yw = inputY * inputW;
    const ValueType zw = inputZ * inputW;
  
    const ValueType mxx = 1.0 - 2.0 * (yy + zz);
    const ValueType myy = 1.0 - 2.0 * (xx + zz);
    const ValueType mzz = 1.0 - 2.0 * (xx + yy);
    const ValueType mxy = 2.0 * (xy - zw);
    const ValueType mxz = 2.0 * (xz + yw);
    const ValueType myx = 2.0 * (xy + zw);
    const ValueType mzx = 2.0 * (xz - yw);
    const ValueType mzy = 2.0 * (yz + xw);
    const ValueType myz = 2.0 * (yz - xw);
  
    OutputVectorType result;
    result[0] = mxx * VectorObject[0] + mxy * VectorObject[1] + mxz * VectorObject[2];
    result[1] = myx * VectorObject[0] + myy * VectorObject[1] + myz * VectorObject[2];
    result[2] = mzx * VectorObject[0] + mzy * VectorObject[1] + mzz * VectorObject[2];
    return result;
  }
  } // namespace
  
  /** Transform a Vector */
  template <typename T>
  auto
  Versor<T>::Transform(const VectorType & v) const -> VectorType
  {
    return localTransformVectorMath<VectorType, T, typename Versor<T>::VectorType>(
      v, this->m_X, this->m_Y, this->m_Z, this->m_W);
  }
  
  /** Transform a CovariantVector
   *  given that this is an orthogonal transformation
   *  CovariantVectors are transformed as vectors. */
  template <typename T>
  auto
  Versor<T>::Transform(const CovariantVectorType & v) const -> CovariantVectorType
  {
    return localTransformVectorMath<CovariantVectorType, T, typename Versor<T>::CovariantVectorType>(
      v, this->m_X, this->m_Y, this->m_Z, this->m_W);
  }
  
  /** Transform a Point */
  template <typename T>
  auto
  Versor<T>::Transform(const PointType & v) const -> PointType
  {
    return localTransformVectorMath<PointType, T, typename Versor<T>::PointType>(
      v, this->m_X, this->m_Y, this->m_Z, this->m_W);
  }
  
  /** Transform a VnlVector */
  template <typename T>
  auto
  Versor<T>::Transform(const VnlVectorType & v) const -> VnlVectorType
  {
    return localTransformVectorMath<VnlVectorType, T, typename Versor<T>::VnlVectorType>(
      v, this->m_X, this->m_Y, this->m_Z, this->m_W);
  }
  
  /** Get Matrix representation */
  template <typename T>
  Matrix<T, 3, 3>
  Versor<T>::GetMatrix() const
  {
    Matrix<T, 3, 3> matrix;
  
    const RealType xx = m_X * m_X;
    const RealType yy = m_Y * m_Y;
    const RealType zz = m_Z * m_Z;
    const RealType xy = m_X * m_Y;
    const RealType xz = m_X * m_Z;
    const RealType xw = m_X * m_W;
    const RealType yz = m_Y * m_Z;
    const RealType yw = m_Y * m_W;
    const RealType zw = m_Z * m_W;
  
    matrix[0][0] = 1.0 - 2.0 * (yy + zz);
    matrix[1][1] = 1.0 - 2.0 * (xx + zz);
    matrix[2][2] = 1.0 - 2.0 * (xx + yy);
    matrix[0][1] = 2.0 * (xy - zw);
    matrix[0][2] = 2.0 * (xz + yw);
    matrix[1][0] = 2.0 * (xy + zw);
    matrix[2][0] = 2.0 * (xz - yw);
    matrix[2][1] = 2.0 * (yz + xw);
    matrix[1][2] = 2.0 * (yz - xw);
  
    return matrix;
  }
  } // end namespace itk
  
  #endif
  