Add: Decompose transformation matrices while parsing

This adds a option enum that makes fastgltf decompose node matrices into the TRS components for ease of use further on.

Author: spnda

CMakeLists.txt

@@ -72,5 +72,5 @@ else()
 endif()
 
 add_subdirectory(src)
-add_subdirectory(tests)
 add_subdirectory(examples)
+add_subdirectory(tests)

src/fastgltf.cpp

@@ -1,4 +1,5 @@
 #include <array>
+#include <cmath>
 #include <fstream>
 #include <functional>
 #include <utility>
@@ -495,6 +496,12 @@ fg::Error fg::glTF::validate() {
             return Error::InvalidGltf;
         if (node.meshIndex.has_value() && parsedAsset->meshes.size() <= node.meshIndex.value())
             return Error::InvalidGltf;
+
+        if (!node.hasMatrix) {
+            for (auto& x : node.transform.trs.rotation)
+                if (x > 1.0 || x < -1.0)
+                    return Error::InvalidGltf;
+        }
     }
 
     for (const auto& scene : parsedAsset->scenes) {
@@ -1381,73 +1388,71 @@ void fg::glTF::parseNodes(simdjson::dom::array& nodes) {
             }
         }
 
-        dom::array matrix;
-        if (nodeObject["matrix"].get_array().get(matrix) == SUCCESS) {
+        dom::array array;
+        auto error = nodeObject["matrix"].get_array().get(array);
+        if (error == SUCCESS) {
             node.hasMatrix = true;
             auto i = 0U;
-            for (auto num : matrix) {
+            for (auto num : array) {
                 double val;
                 if (num.get_double().get(val) != SUCCESS) {
                     node.hasMatrix = false;
                     break;
                 }
-                node.matrix[i] = static_cast<float>(val);
+                node.transform.matrix[i] = static_cast<float>(val);
                 ++i;
             }
-        } else {
-            // clang-format off
-            node.matrix = {
-                1.0f, 0.0f, 0.0f, 0.0f,
-                0.0f, 1.0f, 0.0f, 0.0f,
-                0.0f, 0.0f, 1.0f, 0.0f,
-                0.0f, 0.0f, 0.0f, 1.0f,
-            };
-            // clang-format on
-        }
-
-        dom::array scale;
-        if (nodeObject["scale"].get_array().get(scale) == SUCCESS) {
-            auto i = 0U;
-            for (auto num : scale) {
-                double val;
-                if (num.get_double().get(val) != SUCCESS) {
-                    SET_ERROR_RETURN(Error::InvalidGltf)
+
+            if (hasBit(options, Options::DecomposeNodeMatrices)) {
+                node.hasMatrix = false;
+                // Create a copy of the matrix, as we store the transform in a union.
+                auto matrix = node.transform.matrix;
+                decomposeTransformMatrix(matrix, node.transform.trs.scale, node.transform.trs.rotation, node.transform.trs.translation);
+            }
+        } else if (error == NO_SUCH_FIELD) {
+            node.hasMatrix = false;
+            // There's no matrix, let's see if there's scale, rotation, or rotation fields.
+            if (nodeObject["scale"].get_array().get(array) == SUCCESS) {
+                auto i = 0U;
+                for (auto num : array) {
+                    double val;
+                    if (num.get_double().get(val) != SUCCESS) {
+                        SET_ERROR_RETURN(Error::InvalidGltf)
+                    }
+                    node.transform.trs.scale[i] = static_cast<float>(val);
+                    ++i;
                 }
-                node.scale[i] = static_cast<float>(val);
-                ++i;
+            } else {
+                node.transform.trs.scale = {1.0f, 1.0f, 1.0f};
             }
-        } else {
-            node.scale = {1.0f, 1.0f, 1.0f};
-        }
 
-        dom::array translation;
-        if (nodeObject["translation"].get_array().get(translation) == SUCCESS) {
-            auto i = 0U;
-            for (auto num : translation) {
-                double val;
-                if (num.get_double().get(val) != SUCCESS) {
-                    SET_ERROR_RETURN(Error::InvalidGltf)
+            if (nodeObject["translation"].get_array().get(array) == SUCCESS) {
+                auto i = 0U;
+                for (auto num : array) {
+                    double val;
+                    if (num.get_double().get(val) != SUCCESS) {
+                        SET_ERROR_RETURN(Error::InvalidGltf)
+                    }
+                    node.transform.trs.translation[i] = static_cast<float>(val);
+                    ++i;
                 }
-                node.translation[i] = static_cast<float>(val);
-                ++i;
+            } else {
+                node.transform.trs.translation = {0.0f, 0.0f, 0.0f};
             }
-        } else {
-            node.translation = {0.0f, 0.0f, 0.0f};
-        }
 
-        dom::array rotation;
-        if (nodeObject["rotation"].get_array().get(rotation) == SUCCESS) {
-            auto i = 0U;
-            for (auto num : rotation) {
-                double val;
-                if (num.get_double().get(val) != SUCCESS) {
-                    SET_ERROR_RETURN(Error::InvalidGltf)
+            if (nodeObject["rotation"].get_array().get(array) == SUCCESS) {
+                auto i = 0U;
+                for (auto num : array) {
+                    double val;
+                    if (num.get_double().get(val) != SUCCESS) {
+                        SET_ERROR_RETURN(Error::InvalidGltf)
+                    }
+                    node.transform.trs.rotation[i] = static_cast<float>(val);
+                    ++i;
                 }
-                node.rotation[i] = static_cast<float>(val);
-                ++i;
+            } else {
+                node.transform.trs.rotation = {0.0f, 0.0f, 0.0f, 1.0f};
             }
-        } else {
-            node.rotation = {0.0f, 0.0f, 0.0f, 1.0f};
         }
 
         // name is optional.

src/fastgltf_parser.hpp

@@ -114,6 +114,14 @@ namespace fastgltf {
          * like DirectStorage or Metal IO.
          */
         LoadExternalBuffers             = 1 << 4,
+
+        /**
+         * This option makes fastgltf automatically decompose the transformation matrices of nodes
+         * into the translation, rotation, and scale components. This might be useful to have only
+         * TRS components, instead of matrices or TRS, which should simplify working with nodes,
+         * especially with animations.
+         */
+        DecomposeNodeMatrices           = 1 << 5,
     };
     // clang-format on
 

src/fastgltf_types.hpp

@@ -327,12 +327,20 @@ namespace fastgltf {
         std::optional<size_t> cameraIndex;
         std::vector<size_t> children;
 
+        union {
+            struct {
+                std::array<float, 3> translation;
+                std::array<float, 4> rotation;
+                std::array<float, 3> scale;
+            } trs;
+            /**
+             * Ordinary transformation matrix, which cannot skew or shear. Using
+             * Options::DecomposeNodeMatrices all parsed matrices will be decomposed
+             * into the TRS components found above.
+             */
+            std::array<float, 16> matrix;
+        } transform;
         bool hasMatrix = false;
-        std::array<float, 16> matrix;
-
-        std::array<float, 3> scale;
-        std::array<float, 3> translation;
-        std::array<float, 4> rotation;
 
         std::string name;
     };

src/fastgltf_util.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <array>
+#include <cmath>
 #include <type_traits>
 
 namespace fastgltf {
@@ -26,6 +27,47 @@ namespace fastgltf {
         return base - (base % alignment);
     }
 
+    /**
+     * Decomposes a transform matrix into the translation, rotation, and scale components. This
+     * function does not support skew, shear, or perspective. This currently uses a quick algorithm
+     * to calculate the quaternion from the rotation matrix, which might occasionally loose some
+     * precision, though we try to use doubles here.
+     */
+    inline void decomposeTransformMatrix(std::array<float, 16> matrix, std::array<float, 3>& scale, std::array<float, 4>& rotation, std::array<float, 3>& translation) {
+        // Extract the translation. We zero the translation out, as we reuse the matrix as
+        // the rotation matrix at the end.
+        translation = {matrix[12], matrix[13], matrix[14]};
+        matrix[12] = matrix[13] = matrix[14] = 0;
+
+        // Extract the scale. We calculate the euclidean length of the columns. We then
+        // construct a vector with those lengths. My gcc's stdlib doesn't include std::sqrtf
+        // for some reason...
+        auto s1 = sqrtf(matrix[0] * matrix[0] + matrix[1] * matrix[1] +  matrix[2] * matrix[2]);
+        auto s2 = sqrtf(matrix[4] * matrix[4] + matrix[5] * matrix[5] +  matrix[6] * matrix[6]);
+        auto s3 = sqrtf(matrix[8] * matrix[8] + matrix[9] * matrix[9] + matrix[10] * matrix[10]);
+        scale = {s1, s2, s3};
+
+        // Remove the scaling from the matrix, leaving only the rotation. matrix is now the
+        // rotation matrix.
+        matrix[0] /= s1; matrix[1] /= s1;  matrix[2] /= s1;
+        matrix[4] /= s2; matrix[5] /= s2;  matrix[6] /= s2;
+        matrix[8] /= s3; matrix[9] /= s3; matrix[10] /= s3;
+
+        // Construct the quaternion. This algo is copied from here:
+        // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/christian.htm.
+        // glTF orders the components as x,y,z,w
+        auto max = [](float a, float b) -> double { return (a > b) ? a : b; };
+        rotation = {
+            static_cast<float>(std::sqrt(max(0, 1 + matrix[0] - matrix[5] - matrix[10])) / 2),
+            static_cast<float>(std::sqrt(max(0, 1 - matrix[0] + matrix[5] - matrix[10])) / 2),
+            static_cast<float>(std::sqrt(max(0, 1 - matrix[0] - matrix[5] + matrix[10])) / 2),
+            static_cast<float>(std::sqrt(max(0, 1 + matrix[0] + matrix[5] + matrix[10])) / 2),
+        };
+        rotation[0] = std::copysignf(rotation[0], matrix[6] - matrix[9]);
+        rotation[1] = std::copysignf(rotation[1], matrix[8] - matrix[2]);
+        rotation[2] = std::copysignf(rotation[2], matrix[1] - matrix[4]);
+    }
+
     static constexpr std::array<uint32_t, 256> crcHashTable = {
         0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
         0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,

tests/CMakeLists.txt

@@ -3,7 +3,7 @@ set_directory_properties(PROPERTIES EXCLUDE_FROM_ALL TRUE)
 # We want these tests to be a optional executable.
 add_executable(tests EXCLUDE_FROM_ALL)
 target_compile_features(tests PRIVATE cxx_std_20)
-target_link_libraries(tests PRIVATE fastgltf)
+target_link_libraries(tests PRIVATE fastgltf glm::glm)
 compiler_flags(TARGET tests)
 
 if (EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/deps/catch2")

tests/basic_test.cpp

@@ -2,6 +2,11 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/benchmark/catch_benchmark.hpp>
 
+#include <glm/glm.hpp>
+#include <glm/gtc/type_ptr.hpp>
+#include <glm/gtx/quaternion.hpp>
+#include <glm/gtx/matrix_decompose.hpp>
+
 #include "fastgltf_parser.hpp"
 #include "fastgltf_types.hpp"
 
@@ -327,3 +332,87 @@ TEST_CASE("Test allocation callbacks for embedded buffers", "[gltf-loader]") {
         std::free(allocation);
     }
 }
+
+TEST_CASE("Test TRS parsing and optional decomposition", "[gltf-loader]") {
+    SECTION("Test decomposition on glTF asset") {
+        auto jsonData = std::make_unique<fastgltf::JsonData>(path / "transform_matrices.gltf");
+
+        // Parse once without decomposing, once with decomposing the matrix.
+        fastgltf::Parser parser;
+        auto modelWithMatrix = parser.loadGLTF(jsonData.get(), path);
+        REQUIRE(parser.getError() == fastgltf::Error::None);
+        REQUIRE(modelWithMatrix != nullptr);
+
+        REQUIRE(modelWithMatrix->parse(fastgltf::Category::Nodes) == fastgltf::Error::None);
+        auto assetWithMatrix = modelWithMatrix->getParsedAsset();
+
+        auto modelDecomposed = parser.loadGLTF(jsonData.get(), path, fastgltf::Options::DecomposeNodeMatrices);
+        REQUIRE(parser.getError() == fastgltf::Error::None);
+        REQUIRE(modelWithMatrix != nullptr);
+
+        REQUIRE(modelDecomposed->parse(fastgltf::Category::Nodes) == fastgltf::Error::None);
+        auto assetDecomposed = modelDecomposed->getParsedAsset();
+
+        REQUIRE(assetWithMatrix->cameras.size() == 1);
+        REQUIRE(assetDecomposed->cameras.size() == 1);
+        REQUIRE(assetWithMatrix->nodes.size() == 2);
+        REQUIRE(assetDecomposed->nodes.size() == 2);
+        REQUIRE(assetWithMatrix->nodes.back().hasMatrix);
+        REQUIRE(!assetDecomposed->nodes.back().hasMatrix);
+
+        // Get the TRS components from the first node and use them as the test data for decomposing.
+        auto translation = glm::make_vec3(assetWithMatrix->nodes.front().transform.trs.translation.data());
+        auto rotation = glm::make_quat(assetWithMatrix->nodes.front().transform.trs.rotation.data());
+        auto scale = glm::make_vec3(assetWithMatrix->nodes.front().transform.trs.scale.data());
+        auto rotationMatrix = glm::toMat4(rotation);
+        auto transform = glm::translate(glm::mat4(1.0f), translation) * rotationMatrix * glm::scale(glm::mat4(1.0f), scale);
+
+        // Check if the parsed matrix is correct.
+        REQUIRE(glm::make_mat4x4(assetWithMatrix->nodes.back().transform.matrix.data()) == transform);
+
+        // Check if the decomposed components equal the original components.
+        REQUIRE(glm::make_vec3(assetDecomposed->nodes.back().transform.trs.translation.data()) == translation);
+        REQUIRE(glm::make_quat(assetDecomposed->nodes.back().transform.trs.rotation.data()) == rotation);
+        REQUIRE(glm::make_vec3(assetDecomposed->nodes.back().transform.trs.scale.data()) == scale);
+    }
+
+    SECTION("Test decomposition against glm decomposition") {
+        // Some random complex transform matrix from one of the glTF sample models.
+        std::array<float, 16> matrix = {
+            -0.4234085381031037,
+            -0.9059388637542724,
+            -7.575183536001616e-11,
+            0.0,
+            -0.9059388637542724,
+            0.4234085381031037,
+            -4.821281221478735e-11,
+            0.0,
+            7.575183536001616e-11,
+            4.821281221478735e-11,
+            -1.0,
+            0.0,
+            -90.59386444091796,
+            -24.379817962646489,
+            -40.05522918701172,
+            1.0
+        };
+
+        std::array<float, 3> translation = {}, scale = {};
+        std::array<float, 4> rotation = {};
+        fastgltf::decomposeTransformMatrix(matrix, scale, rotation, translation);
+
+        auto glmMatrix = glm::make_mat4x4(matrix.data());
+        glm::vec3 glmScale, glmTranslation, glmSkew;
+        glm::quat glmRotation;
+        glm::vec4 glmPerspective;
+        glm::decompose(glmMatrix, glmScale, glmRotation, glmTranslation, glmSkew, glmPerspective);
+
+        // I use glm::epsilon<float>() * 10 here because some matrices I tested this with resulted
+        // in an error margin greater than the normal epsilon value. I will investigate this in the
+        // future, but I suspect using double in the decompose functions should help mitigate most
+        // of it.
+        REQUIRE(glm::make_vec3(translation.data()) == glmTranslation);
+        REQUIRE(glm::all(glm::epsilonEqual(glm::make_quat(rotation.data()), glmRotation, glm::epsilon<float>() * 10)));
+        REQUIRE(glm::all(glm::epsilonEqual(glm::make_vec3(scale.data()), glmScale, glm::epsilon<float>())));
+    }
+}

tests/gltf/basic_gltf.gltf

@@ -1,5 +1,5 @@
 {
-    "asset": {
-        "version": "2.0"
-    }
+  "asset": {
+    "version": "2.0"
+  }
 }