diff --git a/.clang-format b/.clang-format index 59c388ac6..a7709c168 100644 --- a/.clang-format +++ b/.clang-format @@ -36,7 +36,7 @@ AllowAllArgumentsOnNextLine: true AllowAllParametersOfDeclarationOnNextLine: false AllowShortBlocksOnASingleLine: Never AllowShortCaseLabelsOnASingleLine: true -AllowShortCompoundRequirementOnASingleLine: true +#AllowShortCompoundRequirementOnASingleLine: true #uncomment when clang-format has version 18 AllowShortEnumsOnASingleLine: true AllowShortFunctionsOnASingleLine: InlineOnly AllowShortIfStatementsOnASingleLine: Never diff --git a/include/vclib/space/quaternion.h b/include/vclib/space/quaternion.h index 950bd05e4..3700462e6 100644 --- a/include/vclib/space/quaternion.h +++ b/include/vclib/space/quaternion.h @@ -29,45 +29,237 @@ namespace vcl { +/** + * @brief Quaternion class. + * + * This class represents a quaternion, `w + xi + yj + zk` vector with a scalar + * part and a vector part. + * + * Note that the scalar part is stored in the `w` component, while the vector + * part is stored in the `x`, `y` and `z` components. + * + * @note Note that internally the coefficients are stored in the following + * order: [x, y, z, w]. This means that accessing to the first component of the + * quaternion (index 0) will return the `x` component, and the last component + * (index 3) will return the `w` component. + * + * @tparam Scalar: the scalar used to represent the quaternion. + */ template -class Quaternion : public Point4 +class Quaternion { - using Base = Point4; + Eigen::Quaternion q = Eigen::Quaternion(1, 0, 0, 0); public: + /** + * @brief Constructs a quaternion representing the identity rotation + * (w = 1, 0, 0, 0). + */ Quaternion() = default; + /** + * @brief Constructs and initializes the quaternion `w+xi+yj+zk` from its + * four coefficients w, x, y and z. + * + * @note Note the order of the arguments: the real w coefficient first, + * while internally the coefficients are stored in the following order: [x, + * y, z, w] + * + * @param[in] w: the real coefficient. + * @param[in] x: the first imaginary coefficient. + * @param[in] y: the second imaginary coefficient. + * @param[in] z: the third imaginary coefficient. + */ Quaternion( const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : - Base(w, x, y, z) + q(w, x, y, z) { } - Quaternion(const Point4& p) : Base(p) {} - Quaternion(const Scalar& s, const Point3& v) : - Base(s, v(0), v(1), v(2)) + q(s, v(0), v(1), v(2)) + { + } + + Quaternion(const Eigen::Quaternion& qq) { q << qq; } + + Quaternion(const Matrix33& rotMatrix) : q(rotMatrix) {} + + Quaternion(const Matrix44& rotMatrix) : q(rotMatrix) {} + + /** + * @brief Constructs the quaternion that will represent the rotation between + * the two arbitrary vectors a and b. + * + * In other words, the built rotation represent a rotation sending the line + * of direction a to the line of direction b, both lines passing through the + * origin. + * + * @param[in] a: first input vector. + * @param[in] b: second input vector. + */ + Quaternion(const Point3& a, const Point3& b) + { + q.setFromTwoVectors(a.eigenVector(), b.eigenVector()); + } + + Scalar& w() { return q.w(); } + + const Scalar& w() const { return q.w(); } + + Scalar& x() { return q.x(); } + + const Scalar& x() const { return q.x(); } + + Scalar& y() { return q.y(); } + + const Scalar& y() const { return q.y(); } + + Scalar& z() { return q.z(); } + + const Scalar& z() const { return q.z(); } + + /** + * @brief Casts the Quaternion object to a different scalar type. + * + * The function returns a new Quaternion object with each scalar value + * casted to a different type. + * + * @tparam S: The scalar type to cast to. + * + * @return A new Quaternion object with each scalar value casted to a + * different type. + */ + template + const Quaternion cast() const + { + if constexpr (std::is_same_v) { + return *this; + } + else { + return Quaternion(q.template cast()); + } + } + + /** + * @brief Returns the conjugate of the quaternion, which represents the + * opposite rotation. + * + * The conjugate of the quaternion is equal to the multiplicative inverse if + * the quaternion is normalized. + * + * @return The conjugate of the quaternion. + */ + Quaternion conjugate() const + { + return Quaternion(q.conjugate()); + } + + /** + * @brief Returns the dot product between this quaternion and the given + * quaternion. + * + * The dot product between two unit quaternions corresponds to the cosine of + * half the angle between the two rotations. + * + * @param[in] q2: the quaternion to compute the dot product with. + * @return The dot product between this quaternion and the given quaternion. + */ + Scalar dot(const Quaternion& q2) const { return q.dot(q2.q); } + + /** + * @brief Returns the inverse of the quaternion, which represents the + * inverse rotation. + * + * The inverse of the quaternion is equal to the multiplicative inverse. + * + * @note Note that in most cases, i.e., if you simply want the opposite + * rotation, and/or the quaternion is normalized, then it is enough to use + * the conjugate. + * + * @return The inverse of the quaternion. + */ + Quaternion inverse() const { + return Quaternion(q.inverse()); } - Scalar& w() { return Base::x(); } + Scalar norm() const { return q.norm(); } - const Scalar& w() const { return Base::x(); } + Scalar squaredNorm() const { return q.squaredNorm(); } - Scalar& x() { return Base::y(); } + constexpr uint size() const { return 4; } - const Scalar& x() const { return Base::y(); } + void setIdentity() { q.setIdentity(); } - Scalar& y() { return Base::z(); } + void set(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) + { + q.w() = w; + q.x() = x; + q.y() = y; + q.z() = z; + } + + void setFromTwoVectors(const Point3& a, const Point3& b) + { + q.setFromTwoVectors(a.eigenVector(), b.eigenVector()); + } + + Quaternion normalized() const + { + return Quaternion(q.normalized()); + } + + void normalize() { q.normalize(); } + + const Eigen::Quaternion& eigenQuaternion() const { return q; } + + /** + * @brief Computes the hash value of the quaternion. + * + * This function computes a hash value of the quaternion, which can be used + * for hashing and comparison purposes. + * + * @return The hash value of the quaternion. + */ + std::size_t hash() const + { + std::size_t h = 0; + for (size_t i = 0; i < 4; ++i) + vcl::hashCombine(h, q(i)); + return h; + } - const Scalar& y() const { return Base::z(); } + Scalar& operator()(uint i) { return q.coeffs()[i]; } - Scalar& z() { return Base::w(); } + const Scalar& operator()(uint i) const { return q.coeffs()[i]; } - const Scalar& z() const { return Base::w(); } + Scalar& operator[](uint i) { return q.coeffs()[i]; } + + const Scalar& operator[](uint i) const { return q.coeffs()[i]; } + + bool operator==(const Quaternion& q2) const { return q == q2.q; } + + bool operator!=(const Quaternion& q2) const { return q != q2.q; } + + Quaternion operator*(const Quaternion& q2) + { + return Quaternion(q * q2.q); + } + + Quaternion& operator*=(const Quaternion& q2) + { + q *= q2.q; + return *this; + } + + Matrix33 toRotationMatrix() const + { + return q.toRotationMatrix(); + } }; } // namespace vcl