quaternion.h

/*****************************************************************************
 * VCLib                                                                     *
 * Visual Computing Library                                                  *
 *                                                                           *
 * Copyright(C) 2021-2025                                                    *
 * Visual Computing Lab                                                      *
 * ISTI - Italian National Research Council                                  *
 *                                                                           *
 * All rights reserved.                                                      *
 *                                                                           *
 * This program is free software; you can redistribute it and/or modify      *
 * it under the terms of the Mozilla Public License Version 2.0 as published *
 * by the Mozilla Foundation; either version 2 of the License, or            *
 * (at your option) any later version.                                       *
 *                                                                           *
 * This program is distributed in the hope that it will be useful,           *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the              *
 * Mozilla Public License Version 2.0                                        *
 * (https://www.mozilla.org/en-US/MPL/2.0/) for more details.                *
 ****************************************************************************/
 
#ifndef VCL_SPACE_CORE_QUATERNION_H
#define VCL_SPACE_CORE_QUATERNION_H
 
#include "matrix.h"
#include "point.h"
 
namespace vcl {
 
template<typename Scalar>
class Quaternion : public Eigen::Quaternion<Scalar>
{
    using Base = Eigen::Quaternion<Scalar>;
 
public:
    // inherit Base constructors
    using Base::Base;
 
    // inherit Base operators
    using Base::operator*;
    using Base::operator*=;
 
    using ScalarType = Scalar;
 
    Quaternion() : Base(1, 0, 0, 0) {}
 
    template<typename S, int Options>
    Quaternion(const Eigen::Quaternion<S, Options>& q) : Base(q)
    {
    }
 
    Quaternion(const Scalar& angle, const Point3<Scalar>& axis) :
            Base(Eigen::AngleAxis<Scalar>(angle, axis))
    {
    }
 
    Quaternion(const Matrix44<Scalar>& rotMatrix) :
            Base(Matrix33<Scalar>(rotMatrix.block(0, 0, 3, 3)))
    {
    }
 
    Quaternion(const Point3<Scalar>& a, const Point3<Scalar>& b)
    {
        Base::setFromTwoVectors(a, b);
    }
 
    template<typename S>
    auto cast() const
    {
        if constexpr (std::is_same_v<Scalar, S>) {
            return *this;
        }
        else {
            return Quaternion<S>(Base::template cast<S>());
        }
    }
 
    constexpr uint size() const { return 4; }
 
    // void setIdentity() { mQ.setIdentity(); }
 
    void set(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z)
    {
        Base::w() = w;
        Base::x() = x;
        Base::y() = y;
        Base::z() = z;
    }
 
    void setFromAngleAxis(const Scalar& angle, const Point3<Scalar>& axis)
    {
        *this = Base(Eigen::AngleAxis<Scalar>(angle, axis));
    }
 
    std::size_t hash() const
    {
        std::size_t h = 0;
        for (size_t i = 0; i < 4; ++i)
            hashCombine(h, Base::operator()(i));
        return h;
    }
 
    Point3<Scalar> operator*(const Point3<Scalar>& p) const
    {
        Eigen::Matrix<Scalar, 3, 1> fc = Base::vec().cross(p);
 
        Eigen::Matrix<Scalar, 3, 1> fd = p * Base::w();
 
        Eigen::Matrix<Scalar, 3, 1> s = fc + fd;
 
        Eigen::Matrix<Scalar, 3, 1> sc = Base::vec().cross(s);
 
        return Point3<Scalar>(p + sc * 2.0);
    }
};
 
using Quaternionf = Quaternion<float>;
 
using Quaterniond = Quaternion<double>;
 
} // namespace vcl
 
#endif // VCL_SPACE_CORE_QUATERNION_H