Skip to content

[Type] Quat: factorize matrix code #6030

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
hugtalbot merged 2 commits into from
Mar 25, 2026
Merged

[Type] Quat: factorize matrix code #6030

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
431491c
quat: factorize/regroup code building matrix
fredroy Mar 18, 2026
6213b3c
Merge branch 'master' into quat_buildrotmat
hugtalbot Mar 25, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
137 changes: 42 additions & 95 deletions Sofa/framework/Type/src/sofa/type/Quat.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,35 +29,6 @@
#include <iosfwd>
#include <cassert>

namespace // anonymous
{

template<typename QuatReal, typename OtherReal>
constexpr void getOpenGlMatrix(const QuatReal& q, OtherReal* m)
{
m[0 * 4 + 0] = static_cast<OtherReal>(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
m[1 * 4 + 0] = static_cast<OtherReal>(2.0 * (q[0] * q[1] - q[2] * q[3]));
m[2 * 4 + 0] = static_cast<OtherReal>(2.0 * (q[2] * q[0] + q[1] * q[3]));
m[3 * 4 + 0] = static_cast<OtherReal>(0.0);

m[0 * 4 + 1] = static_cast<OtherReal>(2.0 * (q[0] * q[1] + q[2] * q[3]));
m[1 * 4 + 1] = static_cast<OtherReal>(1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]));
m[2 * 4 + 1] = static_cast<OtherReal>(2.0 * (q[1] * q[2] - q[0] * q[3]));
m[3 * 4 + 1] = static_cast<OtherReal>(0.0);

m[0 * 4 + 2] = static_cast<OtherReal>(2.0 * (q[2] * q[0] - q[1] * q[3]));
m[1 * 4 + 2] = static_cast<OtherReal>(2.0 * (q[1] * q[2] + q[0] * q[3]));
m[2 * 4 + 2] = static_cast<OtherReal>(1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]));
m[3 * 4 + 2] = static_cast<OtherReal>(0.0);

m[0 * 4 + 3] = static_cast<OtherReal>(0.0);
m[1 * 4 + 3] = static_cast<OtherReal>(0.0);
m[2 * 4 + 3] = static_cast<OtherReal>(0.0);
m[3 * 4 + 3] = static_cast<OtherReal>(1.0);
}

}

namespace sofa::type
{

Expand All @@ -73,6 +44,24 @@ class Quat
typedef type::Mat<3,3, Real> Mat3x3;
typedef type::Mat<4,4, Real> Mat4x4;

/// Compute the 3x3 rotation matrix from this quaternion and write
/// each coefficient through the provided setter(row, col, value).
template<typename Setter>
constexpr void computeRotationMatrix(Setter&& set) const
{
set(0,0, 1 - 2 * (_q[1] * _q[1] + _q[2] * _q[2]));
set(0,1, 2 * (_q[0] * _q[1] - _q[2] * _q[3]));
set(0,2, 2 * (_q[2] * _q[0] + _q[1] * _q[3]));

set(1,0, 2 * (_q[0] * _q[1] + _q[2] * _q[3]));
set(1,1, 1 - 2 * (_q[2] * _q[2] + _q[0] * _q[0]));
set(1,2, 2 * (_q[1] * _q[2] - _q[0] * _q[3]));

set(2,0, 2 * (_q[2] * _q[0] - _q[1] * _q[3]));
set(2,1, 2 * (_q[1] * _q[2] + _q[0] * _q[3]));
set(2,2, 1 - 2 * (_q[1] * _q[1] + _q[0] * _q[0]));
}

public:
typedef Real value_type;
typedef sofa::Size Size;
Expand Down Expand Up @@ -157,44 +146,37 @@ class Quat
void fromMatrix(const Mat3x3 &m);

/// Convert the quaternion into an orientation matrix
void toMatrix(Mat3x3 &m) const
constexpr void toMatrix(Mat3x3 &m) const
{
m(0,0) = (1 - 2 * (_q[1] * _q[1] + _q[2] * _q[2]));
m(0,1) = (2 * (_q[0] * _q[1] - _q[2] * _q[3]));
m(0,2) = (2 * (_q[2] * _q[0] + _q[1] * _q[3]));

m(1,0) = (2 * (_q[0] * _q[1] + _q[2] * _q[3]));
m(1,1) = (1 - 2 * (_q[2] * _q[2] + _q[0] * _q[0]));
m(1,2) = (2 * (_q[1] * _q[2] - _q[0] * _q[3]));

m(2,0) = (2 * (_q[2] * _q[0] - _q[1] * _q[3]));
m(2,1) = (2 * (_q[1] * _q[2] + _q[0] * _q[3]));
m(2,2) = (1 - 2 * (_q[1] * _q[1] + _q[0] * _q[0]));
computeRotationMatrix([&m](auto i, auto j, Real v) { m(i,j) = v; });
}

/// Convert the quaternion into an orientation homogeneous matrix
/// The homogeneous part is set to 0,0,0,1
constexpr void toHomogeneousMatrix(Mat4x4 &m) const
{
m(0,0) = (1 - 2 * (_q[1] * _q[1] + _q[2] * _q[2]));
m(0,1) = (2 * (_q[0] * _q[1] - _q[2] * _q[3]));
m(0,2) = (2 * (_q[2] * _q[0] + _q[1] * _q[3]));
m(0,3) = 0.0;

m(1,0) = (2 * (_q[0] * _q[1] + _q[2] * _q[3]));
m(1,1) = (1 - 2 * (_q[2] * _q[2] + _q[0] * _q[0]));
m(1,2) = (2 * (_q[1] * _q[2] - _q[0] * _q[3]));
m(1,3) = 0.0;

m(2,0) = (2 * (_q[2] * _q[0] - _q[1] * _q[3]));
m(2,1) = (2 * (_q[1] * _q[2] + _q[0] * _q[3]));
m(2,2) = (1 - 2 * (_q[1] * _q[1] + _q[0] * _q[0]));
m(2,3) = 0.0;

m(3,0) = 0.0f;
m(3,1) = 0.0f;
m(3,2) = 0.0f;
m(3,3) = 1.0f;
computeRotationMatrix([&m](auto i, auto j, Real v) { m(i,j) = v; });
m(0,3) = 0; m(1,3) = 0; m(2,3) = 0;
m(3,0) = 0; m(3,1) = 0; m(3,2) = 0; m(3,3) = 1;
}

/// Builds a 4x4 rotation matrix from this quaternion.
constexpr void buildRotationMatrix(Real m[4][4]) const
{
computeRotationMatrix([&m](auto i, auto j, Real v) { m[i][j] = v; });
m[0][3] = 0; m[1][3] = 0; m[2][3] = 0;
m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
}

template<typename OtherReal>
constexpr void writeOpenGlMatrix(OtherReal* m) const
{
// OpenGL uses column-major order: m[col*4 + row]
computeRotationMatrix([&m](auto i, auto j, Real v) {
m[j * 4 + i] = static_cast<OtherReal>(v);
});
m[3] = 0; m[7] = 0; m[11] = 0;
m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1;
}

/// Apply the rotation to a given vector
Expand Down Expand Up @@ -347,41 +329,6 @@ class Quat
negate()). */
void slerp(const Quat& a, const Quat& b, Real t, bool allowFlip=true);

/// A useful function, builds a rotation matrix in Matrix based on
/// given quaternion.
constexpr void buildRotationMatrix(Real m[4][4]) const
{
m[0][0] = (1 - 2 * (_q[1] * _q[1] + _q[2] * _q[2]));
m[0][1] = (2 * (_q[0] * _q[1] - _q[2] * _q[3]));
m[0][2] = (2 * (_q[2] * _q[0] + _q[1] * _q[3]));
m[0][3] = 0;

m[1][0] = (2 * (_q[0] * _q[1] + _q[2] * _q[3]));
m[1][1] = (1 - 2 * (_q[2] * _q[2] + _q[0] * _q[0]));
m[1][2] = (2 * (_q[1] * _q[2] - _q[0] * _q[3]));
m[1][3] = 0;

m[2][0] = (2 * (_q[2] * _q[0] - _q[1] * _q[3]));
m[2][1] = (2.0f * (_q[1] * _q[2] + _q[0] * _q[3]));
m[2][2] = (1.0f - 2.0f * (_q[1] * _q[1] + _q[0] * _q[0]));
m[2][3] = 0;

m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1;
}

constexpr void writeOpenGlMatrix(double* m) const
{
return getOpenGlMatrix<Quat, double>(*this, m);
}

constexpr void writeOpenGlMatrix(float* m) const
{
return getOpenGlMatrix<Quat, float>(*this, m);
}

/// This function computes a quaternion based on an axis (defined by
/// the given vector) and an angle about which to rotate. The angle is
/// expressed in radians.
Expand Down
Loading