quaternion.hpp

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
//
//
//                          License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2020, Huawei Technologies Co., Ltd. All rights reserved.
// Third party copyrights are property of their respective owners.
//
// 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
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
// 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.
//
// Author: Liangqian Kong <chargerKong@126.com>
//         Longbu Wang <riskiest@gmail.com>
#ifndef OPENCV_CORE_QUATERNION_HPP
#define OPENCV_CORE_QUATERNION_HPP
 
#include <opencv2/core.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <iostream>
namespace cv
{
 
enum QuatAssumeType
{
    QUAT_ASSUME_NOT_UNIT,
    QUAT_ASSUME_UNIT
};
 
class QuatEnum
{
public:
    enum EulerAnglesType
    {
        INT_XYZ, 
        INT_XZY, 
        INT_YXZ, 
        INT_YZX, 
        INT_ZXY, 
        INT_ZYX, 
        INT_XYX, 
        INT_XZX, 
        INT_YXY, 
        INT_YZY, 
        INT_ZXZ, 
        INT_ZYZ, 
 
        EXT_XYZ, 
        EXT_XZY, 
        EXT_YXZ, 
        EXT_YZX, 
        EXT_ZXY, 
        EXT_ZYX, 
        EXT_XYX, 
        EXT_XZX, 
        EXT_YXY, 
        EXT_YZY,  
        EXT_ZXZ, 
        EXT_ZYZ, 
        #ifndef CV_DOXYGEN
            EULER_ANGLES_MAX_VALUE
        #endif
    };
 
};
 
template <typename _Tp> class Quat;
template <typename _Tp> std::ostream& operator<<(std::ostream&, const Quat<_Tp>&);
 
template <typename _Tp>
class Quat
{
    static_assert(std::is_floating_point<_Tp>::value, "Quaternion only make sense with type of float or double");
    using value_type = _Tp;
public:
    static constexpr _Tp CV_QUAT_EPS = (_Tp)1.e-6;
    static constexpr _Tp CV_QUAT_CONVERT_THRESHOLD = (_Tp)1.e-6;
 
    Quat();
 
    explicit Quat(const Vec<_Tp, 4> &coeff);
 
    Quat(_Tp w, _Tp x, _Tp y, _Tp z);
 
    static Quat<_Tp> createFromAngleAxis(const _Tp angle, const Vec<_Tp, 3> &axis);
 
    static Quat<_Tp> createFromRotMat(InputArray R);
 
    static Quat<_Tp> createFromRvec(InputArray rvec);
 
    static Quat<_Tp> createFromEulerAngles(const Vec<_Tp, 3> &angles, QuatEnum::EulerAnglesType eulerAnglesType);
 
    static Quat<_Tp> createFromYRot(const _Tp theta);
 
    static Quat<_Tp> createFromXRot(const _Tp theta);
 
    static Quat<_Tp> createFromZRot(const _Tp theta);
 
    _Tp at(size_t index) const;
 
    Quat<_Tp> conjugate() const;
 
    template <typename T>
    friend Quat<T> exp(const Quat<T> &q);
 
    Quat<_Tp> exp() const;
 
    template <typename T>
    friend Quat<T> log(const Quat<T> &q, QuatAssumeType assumeUnit);
 
    Quat<_Tp> log(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    template <typename T>
    friend Quat<T> power(const Quat<T> &q, const T x, QuatAssumeType assumeUnit);
 
    Quat<_Tp> power(const _Tp x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    template <typename T>
    friend Quat<T> sqrt(const Quat<T> &q, QuatAssumeType assumeUnit);
 
    Quat<_Tp> sqrt(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    template <typename T>
    friend Quat<T> power(const Quat<T> &p, const Quat<T> &q, QuatAssumeType assumeUnit);
 
    Quat<_Tp> power(const Quat<_Tp> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    template <typename T>
    friend Quat<T> crossProduct(const Quat<T> &p, const Quat<T> &q);
 
    Quat<_Tp> crossProduct(const Quat<_Tp> &q) const;
 
    _Tp norm() const;
 
    Quat<_Tp> normalize() const;
 
    template <typename T>
    friend Quat<T> inv(const Quat<T> &q, QuatAssumeType assumeUnit);
 
    Quat<_Tp> inv(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    template <typename T>
    friend Quat<T> sinh(const Quat<T> &q);
 
    Quat<_Tp> sinh() const;
 
    template <typename T>
    friend Quat<T> cosh(const Quat<T> &q);
 
    Quat<_Tp> cosh() const;
 
    template <typename T>
    friend Quat<T> tanh(const Quat<T> &q);
 
    Quat<_Tp> tanh() const;
 
    template <typename T>
    friend Quat<T> sin(const Quat<T> &q);
 
    Quat<_Tp> sin() const;
 
    template <typename T>
    friend Quat<T> cos(const Quat<T> &q);
 
    Quat<_Tp> cos() const;
 
    template <typename T>
    friend Quat<T> tan(const Quat<T> &q);
 
    Quat<_Tp> tan() const;
 
    template <typename T>
    friend Quat<T> asin(const Quat<T> &q);
 
    Quat<_Tp> asin() const;
 
    template <typename T>
    friend Quat<T> acos(const Quat<T> &q);
 
    Quat<_Tp> acos() const;
 
    template <typename T>
    friend Quat<T> atan(const Quat<T> &q);
 
    Quat<_Tp> atan() const;
 
    template <typename T>
    friend Quat<T> asinh(const Quat<T> &q);
 
    Quat<_Tp> asinh() const;
 
    template <typename T>
    friend Quat<T> acosh(const Quat<T> &q);
 
    Quat<_Tp> acosh() const;
 
    template <typename T>
    friend Quat<T> atanh(const Quat<T> &q);
 
    Quat<_Tp> atanh() const;
 
    bool isNormal(_Tp eps=CV_QUAT_EPS) const;
 
    void assertNormal(_Tp eps=CV_QUAT_EPS) const;
 
    Matx<_Tp, 3, 3> toRotMat3x3(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    Matx<_Tp, 4, 4> toRotMat4x4(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    Vec<_Tp, 4> toVec() const;
 
    Vec<_Tp, 3> toRotVec(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    _Tp getAngle(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    Vec<_Tp, 3> getAxis(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
 
    _Tp dot(Quat<_Tp> q) const;
 
    static Quat<_Tp> lerp(const Quat<_Tp> &q0, const Quat &q1, const _Tp t);
 
    static Quat<_Tp> nlerp(const Quat<_Tp> &q0, const Quat &q1, const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
    static Quat<_Tp> slerp(const Quat<_Tp> &q0, const Quat &q1, const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT, bool directChange=true);
 
    static Quat<_Tp> squad(const Quat<_Tp> &q0, const Quat<_Tp> &s0,
                            const Quat<_Tp> &s1, const Quat<_Tp> &q1,
                            const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT,
                            bool directChange=true);
 
    static Quat<_Tp> interPoint(const Quat<_Tp> &q0, const Quat<_Tp> &q1,
                                 const Quat<_Tp> &q2, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
    static Quat<_Tp> spline(const Quat<_Tp> &q0, const Quat<_Tp> &q1,
                            const Quat<_Tp> &q2, const Quat<_Tp> &q3,
                            const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
    Quat<_Tp> operator-() const;
 
    bool operator==(const Quat<_Tp>&) const;
 
    Quat<_Tp> operator+(const Quat<_Tp>&) const;
 
    Quat<_Tp>& operator+=(const Quat<_Tp>&);
 
    Quat<_Tp> operator-(const Quat<_Tp>&) const;
 
    Quat<_Tp>& operator-=(const Quat<_Tp>&);
 
    Quat<_Tp>& operator*=(const Quat<_Tp>&);
 
    Quat<_Tp>& operator*=(const _Tp s);
 
    Quat<_Tp> operator*(const Quat<_Tp>&) const;
 
    Quat<_Tp> operator/(const _Tp s) const;
 
    Quat<_Tp> operator/(const Quat<_Tp>&) const;
 
    Quat<_Tp>& operator/=(const _Tp s);
 
    Quat<_Tp>& operator/=(const Quat<_Tp>&);
 
    _Tp& operator[](std::size_t n);
 
    const _Tp& operator[](std::size_t n) const;
 
    template <typename T>
    friend Quat<T> cv::operator-(const T s, const Quat<T>&);
 
    template <typename T>
    friend Quat<T> cv::operator-(const Quat<T>&, const T s);
 
    template <typename T>
    friend Quat<T> cv::operator+(const T s, const Quat<T>&);
 
    template <typename T>
    friend Quat<T> cv::operator+(const Quat<T>&, const T s);
 
    template <typename T>
    friend Quat<T> cv::operator*(const T s, const Quat<T>&);
 
    template <typename T>
    friend Quat<T> cv::operator*(const Quat<T>&, const T s);
 
    template <typename S>
    friend std::ostream& cv::operator<<(std::ostream&, const Quat<S>&);
 
    Vec<_Tp, 3> toEulerAngles(QuatEnum::EulerAnglesType eulerAnglesType);
 
    _Tp w, x, y, z;
 
};
 
template <typename T>
Quat<T> inv(const Quat<T> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
template <typename T>
Quat<T> sinh(const Quat<T> &q);
 
template <typename T>
Quat<T> cosh(const Quat<T> &q);
 
template <typename T>
Quat<T> tanh(const Quat<T> &q);
 
template <typename T>
Quat<T> sin(const Quat<T> &q);
 
template <typename T>
Quat<T> cos(const Quat<T> &q);
 
template <typename T>
Quat<T> tan(const Quat<T> &q);
 
template <typename T>
Quat<T> asinh(const Quat<T> &q);
 
template <typename T>
Quat<T> acosh(const Quat<T> &q);
 
template <typename T>
Quat<T> atanh(const Quat<T> &q);
 
template <typename T>
Quat<T> asin(const Quat<T> &q);
 
template <typename T>
Quat<T> acos(const Quat<T> &q);
 
template <typename T>
Quat<T> atan(const Quat<T> &q);
 
template <typename T>
Quat<T> power(const Quat<T> &q, const Quat<T> &p, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
template <typename T>
Quat<T> exp(const Quat<T> &q);
 
template <typename T>
Quat<T> log(const Quat<T> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
template <typename T>
Quat<T> power(const Quat<T>& q, const T x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
template <typename T>
Quat<T> crossProduct(const Quat<T> &p, const Quat<T> &q);
 
template <typename S>
Quat<S> sqrt(const Quat<S> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
 
template <typename T>
Quat<T> operator*(const T, const Quat<T>&);
 
template <typename T>
Quat<T> operator*(const Quat<T>&, const T);
 
template <typename S>
std::ostream& operator<<(std::ostream&, const Quat<S>&);
 
using Quatd = Quat<double>;
using Quatf = Quat<float>;
 
}
 
#include "opencv2/core/quaternion.inl.hpp"
 
#endif /* OPENCV_CORE_QUATERNION_HPP */