TQuat.hpp

Go to the documentation of this file.

/**********************************************************************************/
/* This file is part of Berserk Engine project                                    */
/* https://github.com/EgorOrachyov/Berserk                                        */
/**********************************************************************************/
/* MIT License                                                                    */
/*                                                                                */
/* Copyright (c) 2018 - 2021 Egor Orachyov                                        */
/*                                                                                */
/* Permission is hereby granted, free of charge, to any person obtaining a copy   */
/* of this software and associated documentation files (the "Software"), to deal  */
/* in the Software without restriction, including without limitation the rights   */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      */
/* copies of the Software, and to permit persons to whom the Software is          */
/* furnished to do so, subject to the following conditions:                       */
/*                                                                                */
/* The above copyright notice and this permission notice shall be included in all */
/* copies or substantial portions of the Software.                                */
/*                                                                                */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE  */
/* SOFTWARE.                                                                      */
/**********************************************************************************/
 
#ifndef BERSERK_TQUAT_H
#define BERSERK_TQUAT_H
 
#include <core/Config.hpp>
#include <core/Crc32.hpp>
#include <core/Typedefs.hpp>
#include <core/math/TMatMxN.hpp>
#include <core/math/TVecN.hpp>
 
#include <ostream>
 
BRK_NS_BEGIN
 
template<typename T>
class TQuat {
public:
    using Vec = TVecN<T, 3>;
    using Mat = TMatMxN<T, 4, 4>;
 
    TQuat() = default;
 
    TQuat(T s, const Vec &v) {
        scalar = s;
        vec = v;
    }
 
    TQuat(T s, T x, T y, T z) {
        scalar = s;
        vec = {x, y, z};
    }
 
    TQuat(const Vec &axis, T angle) {
        scalar = MathUtils::Cos(angle * (T) 0.5);
        vec = axis.Normalized() * MathUtils::Sin(angle * (T) 0.5);
    }
 
    TQuat(T roll, T yaw, T pitch) {
        *this = TQuat({1, 0, 0}, roll) *
                TQuat({0, 1, 0}, yaw) *
                TQuat({0, 0, 1}, pitch);
    }
 
    explicit TQuat(const Mat &mat) {
        T q[4];// notation: x[0] y[1] z[2] w[3]
        auto &v = mat.values;
 
        T trace = v[0] + v[5] + v[10];
 
        // Matrix 4x4 indexes
        // 0  1  2  3
        // 4  5  6  7
        // 8  9  10 11
        // 12 13 14 15
 
        // Check the diagonal
        if (trace > 0.0) {
            // positive diagonal
 
            T s = MathUtils::Sqrt(trace + 1.0f);
            q[3] = static_cast<T>(s * 0.5);
 
            T t = 0.5f / s;
            q[0] = (v[9] - v[6]) * t;
            q[1] = (v[2] - v[8]) * t;
            q[2] = (v[4] - v[1]) * t;
        } else {
            // negative diagonal
 
            int32 i = 0;
            if (v[5] > v[0]) i = 1;
            if (v[10] > v[4 * i + i]) i = 2;
 
            static const int32 NEXT[3] = {1, 2, 0};
            int32 j = NEXT[i];
            int32 k = NEXT[j];
 
            auto s = (T) MathUtils::Sqrt(v[i * 4 + i] - (v[j * 4 + j] + v[k * 4 + k]) + 1.0);
 
            T t;
            if (s == 0.0) t = s;
            else
                t = static_cast<T>(0.5 / s);
 
            q[i] = s * 0.5f;
            q[3] = (v[k * 4 + j] - v[j * 4 + k]) * t;
            q[j] = (v[j * 4 + i] - v[i * 4 + j]) * t;
            q[k] = (v[k * 4 + i] - v[i * 4 + k]) * t;
        }
 
        scalar = q[3];
        vec = {q[0], q[1], q[2]};
    }
 
    TQuat(const TQuat &other) = default;
    TQuat(TQuat &&other) noexcept = default;
    ~TQuat() = default;
 
    TQuat &operator=(const TQuat &other) = default;
    TQuat &operator=(TQuat &&other) noexcept = default;
 
    void Identity() {
        scalar = 1;
        vec.Zero();
    }
 
    void Zero() {
        scalar = 0;
        vec.Zero();
    }
 
    TQuat operator*(const TQuat &other) const {
        auto s = other.scalar;
        auto v = other.vec;
 
        return TQuat(scalar * s - Vec::Dot(vec, v), v * scalar + vec * s + Vec::Cross(vec, v));
    }
 
    TQuat &operator*=(T a) {
        scalar /= a;
        vec /= a;
        return *this;
    }
 
    TQuat &operator/=(T a) {
        scalar /= a;
        vec /= a;
        return *this;
    }
 
    T Length2() const {
        return scalar * scalar + vec.Length2();
    }
 
    T Length() const {
        T l2 = Length2();
        return MathUtils::Sqrt(l2);
    }
 
    TQuat Normalized() const {
        TQuat r = *this;
        return r.Normalize();
    }
 
    TQuat &Normalize() {
        T len2 = Length2();
 
        if (len2 <= MathUtils::THRESH_ZERO_NORM_SQUARED) {
            Zero();
        } else {
            T len = MathUtils::Sqrt(len2);
            *this /= len;
        }
 
        return *this;
    }
 
    TQuat Inverse() const {
        T len2 = Length2();
 
        if (len2 <= MathUtils::THRESH_ZERO_NORM_SQUARED) {
            TQuat q;
            q.Zero();
            return q;
        } else {
            T len = MathUtils::Sqrt(len2);
            return TQuat(scalar / len, -vec / len);
        }
    }
 
    TQuat Conjugate() const {
        return TQuat(scalar, -vec);
    }
 
    Vec Rotate(const Vec &v) const {
        TQuat q(0, v);
        TQuat r = *this * q * Conjugate();
        return r.vec;
    }
 
    Vec RotateReverse(const Vec &v) const {
        TQuat q(0, v);
        TQuat r = Conjugate() * q * *this;
        return r.vec;
    }
 
    Vec GetAxisX() const {
        return Rotate({1, 0, 0});
    }
 
    Vec GetAxisY() const {
        return Rotate({0, 1, 0});
    }
 
    Vec GetAxisZ() const {
        return Rotate({0, 0, 1});
    }
 
    T GetAngle() const {
        return (T) 2 * MathUtils::Acos(scalar);
    }
 
    Mat AsMatrix() const {
        T x = vec[0];
        T y = vec[1];
        T z = vec[2];
 
        T xx = x * x;
        T xy = x * y;
        T xz = x * z;
        T xw = x * scalar;
 
        T yy = y * y;
        T yz = y * z;
        T yw = y * scalar;
 
        T zz = z * z;
        T zw = z * scalar;
 
        Mat r;
        auto &v = r.values;
 
        v[0] = 1 - 2 * (yy + zz);
        v[1] = 2 * (xy - zw);
        v[2] = 2 * (xz + yw);
        v[3] = 0;
        v[4] = 2 * (xy + zw);
        v[5] = 1 - 2 * (xx + zz);
        v[6] = 2 * (yz - xw);
        v[7] = 0;
        v[8] = 2 * (xz - yw);
        v[9] = 2 * (yz + xw);
        v[10] = 1 - 2 * (xx + yy);
        v[11] = 0;
        v[12] = 0;
        v[13] = 0;
        v[14] = 0;
        v[15] = 1;
 
        return r;
    }
 
    void GetAxisAngle(Vec &axis, T &angle) const {
        angle = (T) 2 * MathUtils::Acos(scalar);
        T s = MathUtils::Max((T) (1.0 - scalar * scalar), (T) 0.0);
 
        if (s > MathUtils::THRESH_ZERO_NORM_SQUARED)
            axis = vec / MathUtils::Sqrt(s);
        else
            axis = vec({0, 1, 0});
    }
 
    Crc32Hash Hash() const {
        Crc32Builder builder;
        builder.Hash(&vec, sizeof(vec));
        builder.Hash(&scalar, sizeof(scalar));
        return builder.GetHash();
    }
 
    bool operator==(const TQuat &other) const {
        return vec == other.vec && scalar == other.scalar;
    }
 
    bool operator!=(const TQuat &other) const {
        return vec != other.vec || scalar != other.scalar;
    }
 
    static TQuat Rotation(const Vec &axis, T angle) {
        return TQuat(axis, angle);
    }
 
    static T Dot(const TQuat &a, const TQuat &b) {
        return a.scalar * b.scalar + Vec::Dot(a.vec, b.vec);
    }
 
    static T Angle(const TQuat &a, const TQuat &b) {
        return MathUtils::Acos(Dot(a.Normalized(), b.Normalized()));
    }
 
    static TQuat Lerp(T t, const TQuat &a, const TQuat &b) {
        TQuat q;
        q.scalar = MathUtils::Lerp(t, a.scalar, b.scalar);
        q.vec = Vec::Lerp(t, a.vec, b.vec);
        return q;
    }
 
    static TQuat Slerp(T t, const TQuat &a, const TQuat &b) {
        T ang = angle(a, b);
 
        if (ang <= MathUtils::THRESH_FLOAT32) {
            return lerp(t, a, b);
        }
 
        TQuat r;
        T angleSin = MathUtils::Sin(ang);
        T angle1 = MathUtils::Sin(ang * (1 - t)) / angleSin;
        T angle2 = MathUtils::Sin(ang * t) / angleSin;
 
        r.scalar = a.scalar * angle1 + b.scalar * angle2;
        r.vec = a.vec * angle1 + b.vec * angle2;
 
        return r;
    }
 
    static TQuat Slerp(T t, T ang, const TQuat &a, const TQuat &b) {
        if (ang <= MathUtils::THRESH_FLOAT32) {
            return Lerp(t, a, b);
        }
 
        TQuat r;
        T angleSin = MathUtils::Sin(ang);
        T angle1 = MathUtils::Sin(ang * (1 - t)) / angleSin;
        T angle2 = MathUtils::Sin(ang * t) / angleSin;
 
        r.scalar = a.scalar * angle1 + b.scalar * angle2;
        r.vec = a.vec * angle1 + b.vec * angle2;
 
        return r;
    }
 
    static TQuat LookAt(const TVecN<T, 3> &direction, const TVecN<T, 3> &up) {
        auto Z = (-direction).Normalized();
        auto X = TVecN<T, 3>::Cross(up, Z).Normalized();
        auto Y = TVecN<T, 3>::Cross(Z, X);
 
        TMatMxN<T, 4, 4> m;
 
        for (int i = 0; i < 3; i++) {
            m.values[0 + i] = X[i];
            m.values[4 + i] = Y[i];
            m.values[8 + i] = Z[i];
        }
 
        return TQuat(m);
    }
 
    static TQuat FaceAt(const TVecN<T, 3> &direction, const TVecN<T, 3> &up) {
        auto Z = direction.Normalized();
        auto X = TVecN<T, 3>::Cross(up, Z).Normalized();
        auto Y = TVecN<T, 3>::Cross(Z, X);
 
        TMatMxN<T, 4, 4> m;
 
        for (uint32 i = 0; i < 3; i++) {
            m.values[0 + i] = X[i];
            m.values[4 + i] = Y[i];
            m.values[8 + i] = Z[i];
        }
 
        return TQuat(m).Inverse();
    }
 
public:
    T scalar = 1;
    Vec vec;
};
 
using Quatf = TQuat<float>;
 
BRK_NS_END
 
namespace std {
 
    template<typename T>
    struct hash<BRK_NS::TQuat<T>> {
    public:
        std::size_t operator()(const BRK_NS::TQuat<T> &quat) const {
            return quat.Hash();
        }
    };
 
}// namespace std
 
template<typename T>
inline std::ostream &operator<<(std::ostream &ostream, const BRK_NS::TQuat<T> &quat) {
    ostream << "(" << quat.scalar << "," << quat.vec[0] << "," << quat.vec[1] << "," << quat.vec[2] << ")";
    return ostream;
}
 
#endif//BERSERK_TQUAT_H