Skip to content

Commit

Permalink
quaternion implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
alemuntoni committed Oct 27, 2023
1 parent 7070f12 commit fdee67d
Show file tree
Hide file tree
Showing 2 changed files with 207 additions and 15 deletions.
2 changes: 1 addition & 1 deletion .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
220 changes: 206 additions & 14 deletions include/vclib/space/quaternion.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<typename Scalar>
class Quaternion : public Point4<Scalar>
class Quaternion
{
using Base = Point4<Scalar>;
Eigen::Quaternion<Scalar> q = Eigen::Quaternion<Scalar>(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<Scalar>& p) : Base(p) {}

Quaternion(const Scalar& s, const Point3<Scalar>& v) :
Base(s, v(0), v(1), v(2))
q(s, v(0), v(1), v(2))
{
}

Quaternion(const Eigen::Quaternion<Scalar>& qq) { q << qq; }

Quaternion(const Matrix33<Scalar>& rotMatrix) : q(rotMatrix) {}

Quaternion(const Matrix44<Scalar>& 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<Scalar>& a, const Point3<Scalar>& 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<typename S>
const Quaternion<S> cast() const
{
if constexpr (std::is_same_v<Scalar, S>) {
return *this;
}
else {
return Quaternion<S>(q.template cast<S>());
}
}

/**
* @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<Scalar> conjugate() const
{
return Quaternion<Scalar>(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<Scalar>& 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<Scalar> inverse() const
{
return Quaternion<Scalar>(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<Scalar>& a, const Point3<Scalar>& b)
{
q.setFromTwoVectors(a.eigenVector(), b.eigenVector());
}

Quaternion<Scalar> normalized() const
{
return Quaternion<Scalar>(q.normalized());
}

void normalize() { q.normalize(); }

const Eigen::Quaternion<Scalar>& 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<Scalar>& q2) const { return q == q2.q; }

bool operator!=(const Quaternion<Scalar>& q2) const { return q != q2.q; }

Quaternion<Scalar> operator*(const Quaternion<Scalar>& q2)
{
return Quaternion<Scalar>(q * q2.q);
}

Quaternion& operator*=(const Quaternion<Scalar>& q2)
{
q *= q2.q;
return *this;
}

Matrix33<Scalar> toRotationMatrix() const
{
return q.toRotationMatrix();
}
};

} // namespace vcl
Expand Down

0 comments on commit fdee67d

Please sign in to comment.