From 5386c9ec69b79a68a586ec8f9f2d262b64c84c88 Mon Sep 17 00:00:00 2001 From: Dion Moult Date: Fri, 1 May 2026 15:15:37 +1000 Subject: [PATCH] ifcviewer: add stage 3+4 data model and compose helpers to Federation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds the structs that were briefly in src/ifcviewer/Federation.{h,cpp} two commits ago, now folded into the merged Federation alongside the file persistence layer: - FederationConfig: federation-wide unit ({prefix, name}). Default METRE; one-of an IfcSIUnit name with optional prefix or an IfcConversionBasedUnit name. - FederationOrigin: stage 3 — XYZ in federation unit + Z-rot. Composes to R_z · T(-xyz_meters), nominating a point as origin. - AFrame + ModelTransform: stage 4 intent — A (model project or map unit, per a_frame), B and pivot (federation unit), full intrinsic-XYZ Euler rotation in degrees. - ModelUnits: per-model project_length_to_meters / map_unit_to_meters cached at load time. Free functions composeFederationOrigin and composeModelTransform return Eigen::Matrix4d in metres. composeModelTransform takes the model's stage-2 georef matrix so it can lift `a` into metres when authored in ModelLocal. Federation gains config_, origin_ members + setters that emit dirtyChanged. Each Model carries a transform_intent. JSON I/O emits config / origin always; transform_intent only when non-default. Schema stays "ifcfed/1" — additive, optional, sane defaults. Five new tests: round-trip of the new fields, default-omission behaviour, two compose smoke tests for FederationOrigin, and one verifying the "pivot at B preserves A→B" invariant of composeModelTransform. All 36 ctest cases pass. Co-Authored-By: Claude Opus 4.7 --- src/ifcviewer/Federation.cpp | 178 ++++++++++++++++++++++++ src/ifcviewer/Federation.h | 114 +++++++++++++-- src/ifcviewer/tests/CMakeLists.txt | 8 ++ src/ifcviewer/tests/test_federation.cpp | 127 +++++++++++++++++ 4 files changed, 417 insertions(+), 10 deletions(-) diff --git a/src/ifcviewer/Federation.cpp b/src/ifcviewer/Federation.cpp index 166a48910d..b7237dcde5 100644 --- a/src/ifcviewer/Federation.cpp +++ b/src/ifcviewer/Federation.cpp @@ -18,6 +18,7 @@ ********************************************************************************/ #include "Federation.h" +#include "Unit.h" #include #include @@ -29,9 +30,33 @@ #include #include +#include + namespace { constexpr const char* kSchema = "ifcfed/1"; +constexpr double kPi = 3.14159265358979323846; +constexpr double kDegToRad = kPi / 180.0; + +Eigen::Matrix4d translation4(const Eigen::Vector3d& t) { + Eigen::Matrix4d M = Eigen::Matrix4d::Identity(); + M(0, 3) = t.x(); + M(1, 3) = t.y(); + M(2, 3) = t.z(); + return M; +} + +// Intrinsic XYZ Euler: R = R_z · R_y · R_x. +Eigen::Matrix4d eulerXYZ(const Eigen::Vector3d& rxyz_rad) { + const Eigen::Matrix3d R3 = + (Eigen::AngleAxisd(rxyz_rad.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rxyz_rad.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rxyz_rad.x(), Eigen::Vector3d::UnitX())).matrix(); + Eigen::Matrix4d R = Eigen::Matrix4d::Identity(); + R.block<3, 3>(0, 0) = R3; + return R; +} + QString resolvePath(const QString& fed_dir, const QString& stored) { if (stored.isEmpty()) return stored; QFileInfo fi(stored); @@ -52,6 +77,61 @@ QString relativizePath(const QString& fed_dir, const QString& abs_path) { } } // namespace +// === Stage-3/4 compose helpers === + +double federationUnitToMeters(const FederationConfig& cfg) { + return convert(1.0, cfg.unit_prefix, cfg.unit_name, "", "METRE"); +} + +Eigen::Matrix4d composeFederationOrigin(const FederationOrigin& origin, + const FederationConfig& cfg) { + const double u = federationUnitToMeters(cfg); + const Eigen::Vector3d xyz_m = origin.xyz * u; + const double rz_rad = origin.rz_deg * kDegToRad; + const Eigen::Matrix3d Rz = + Eigen::AngleAxisd(rz_rad, Eigen::Vector3d::UnitZ()).matrix(); + Eigen::Matrix4d Rz4 = Eigen::Matrix4d::Identity(); + Rz4.block<3, 3>(0, 0) = Rz; + return Rz4 * translation4(-xyz_m); +} + +Eigen::Matrix4d composeModelTransform(const ModelTransform& xf, + const FederationConfig& fed_cfg, + const ModelUnits& model_units, + const Eigen::Matrix4d& stage2_meters) { + const double u_fed = federationUnitToMeters(fed_cfg); + + Eigen::Vector3d A_m; + if (xf.a_frame == AFrame::ModelLocal) { + // a is in the model's project length unit, expressed in the + // pre-stage2 frame. Convert to metres, then lift through stage 2. + const Eigen::Vector4d a_h( + xf.a.x() * model_units.project_length_to_meters, + xf.a.y() * model_units.project_length_to_meters, + xf.a.z() * model_units.project_length_to_meters, + 1.0); + A_m = (stage2_meters * a_h).head<3>(); + } else { + // a is in the model's map unit, expressed in the post-stage2 frame. + A_m = xf.a * model_units.map_unit_to_meters; + } + + const Eigen::Vector3d B_m = xf.b * u_fed; + const Eigen::Vector3d pivot_m = xf.pivot * u_fed; + + const Eigen::Matrix4d R_local = eulerXYZ(xf.rxyz_deg * kDegToRad); + const Eigen::Matrix4d R_at_pivot = + translation4(pivot_m) * R_local * translation4(-pivot_m); + + const Eigen::Vector4d Ah(A_m.x(), A_m.y(), A_m.z(), 1.0); + const Eigen::Vector3d RA = (R_at_pivot * Ah).head<3>(); + const Eigen::Matrix4d T = translation4(B_m - RA); + + return T * R_at_pivot; +} + +// === Federation class === + Federation::Federation(QObject* parent) : QObject(parent) {} QString Federation::generateId() { @@ -68,11 +148,36 @@ void Federation::clear() { created_ = QDateTime(); modified_ = QDateTime(); models_.clear(); + config_ = FederationConfig{}; + origin_ = FederationOrigin{}; has_home_view_ = false; home_view_ = HomeView{}; setDirty(false); } +void Federation::setConfig(const FederationConfig& c) { + if (config_.unit_name == c.unit_name && config_.unit_prefix == c.unit_prefix) + return; + config_ = c; + setDirty(true); +} + +void Federation::setOrigin(const FederationOrigin& o) { + if (origin_.xyz == o.xyz && origin_.rz_deg == o.rz_deg) return; + origin_ = o; + setDirty(true); +} + +void Federation::setModelTransform(const QString& fed_id, + const ModelTransform& xf) { + for (auto& m : models_) { + if (m.id != fed_id) continue; + m.transform_intent = xf; + setDirty(true); + return; + } +} + void Federation::markClean() { setDirty(false); } @@ -163,6 +268,23 @@ bool Federation::load(const QString& path, created_ = QDateTime::fromString(root.value("created").toString(), Qt::ISODate); modified_ = QDateTime::fromString(root.value("modified").toString(), Qt::ISODate); + if (QJsonValue cv = root.value("config"); cv.isObject()) { + QJsonObject co = cv.toObject(); + QJsonObject uo = co.value("unit").toObject(); + config_.unit_name = uo.value("name").toString("METRE").toStdString(); + config_.unit_prefix = uo.value("prefix").toString("").toStdString(); + } + + if (QJsonValue ov = root.value("origin"); ov.isObject()) { + QJsonObject oo = ov.toObject(); + QJsonArray xyz = oo.value("xyz").toArray(); + if (xyz.size() == 3) { + origin_.xyz = Eigen::Vector3d( + xyz[0].toDouble(), xyz[1].toDouble(), xyz[2].toDouble()); + } + origin_.rz_deg = oo.value("rz_deg").toDouble(0.0); + } + QJsonArray arr = root.value("models").toArray(); for (int i = 0; i < arr.size(); ++i) { if (!arr[i].isObject()) { @@ -198,6 +320,22 @@ bool Federation::load(const QString& path, if (m.display_name.isEmpty()) m.display_name = QFileInfo(m.source_path).fileName(); + if (QJsonValue tv = mo.value("transform_intent"); tv.isObject()) { + QJsonObject to = tv.toObject(); + const QString af = to.value("a_frame").toString("ModelGlobal"); + m.transform_intent.a_frame = + (af == "ModelLocal") ? AFrame::ModelLocal : AFrame::ModelGlobal; + auto readVec3 = [](QJsonArray ja) { + if (ja.size() != 3) return Eigen::Vector3d::Zero().eval(); + return Eigen::Vector3d( + ja[0].toDouble(), ja[1].toDouble(), ja[2].toDouble()); + }; + m.transform_intent.a = readVec3(to.value("a").toArray()); + m.transform_intent.b = readVec3(to.value("b").toArray()); + m.transform_intent.rxyz_deg = readVec3(to.value("rxyz_deg").toArray()); + m.transform_intent.pivot = readVec3(to.value("pivot").toArray()); + } + QJsonValue vv = mo.value("visible"); if (vv.isBool()) m.visible = vv.toBool(); @@ -238,6 +376,24 @@ bool Federation::save(const QString& path, QString* err) { root["created"] = created_.toUTC().toString(Qt::ISODate); root["modified"] = modified_.toUTC().toString(Qt::ISODate); + { + QJsonObject co, uo; + uo["name"] = QString::fromStdString(config_.unit_name); + uo["prefix"] = QString::fromStdString(config_.unit_prefix); + co["unit"] = uo; + root["config"] = co; + } + { + QJsonObject oo; + QJsonArray xyz; + xyz.append(origin_.xyz.x()); + xyz.append(origin_.xyz.y()); + xyz.append(origin_.xyz.z()); + oo["xyz"] = xyz; + oo["rz_deg"] = origin_.rz_deg; + root["origin"] = oo; + } + QJsonArray arr; for (const auto& m : models_) { QJsonObject mo; @@ -254,6 +410,28 @@ bool Federation::save(const QString& path, QString* err) { } mo["source"] = so; + // Skip transform_intent when it's at defaults (identity placement). + const ModelTransform def; + const ModelTransform& xf = m.transform_intent; + const bool xf_is_default = + xf.a_frame == def.a_frame && xf.a == def.a && xf.b == def.b && + xf.rxyz_deg == def.rxyz_deg && xf.pivot == def.pivot; + if (!xf_is_default) { + QJsonObject to; + to["a_frame"] = (xf.a_frame == AFrame::ModelLocal) + ? "ModelLocal" : "ModelGlobal"; + auto writeVec3 = [](const Eigen::Vector3d& v) { + QJsonArray a; + a.append(v.x()); a.append(v.y()); a.append(v.z()); + return a; + }; + to["a"] = writeVec3(xf.a); + to["b"] = writeVec3(xf.b); + to["rxyz_deg"] = writeVec3(xf.rxyz_deg); + to["pivot"] = writeVec3(xf.pivot); + mo["transform_intent"] = to; + } + if (!m.visible) mo["visible"] = false; arr.append(mo); diff --git a/src/ifcviewer/Federation.h b/src/ifcviewer/Federation.h index f3174582cc..721ecfba34 100644 --- a/src/ifcviewer/Federation.h +++ b/src/ifcviewer/Federation.h @@ -20,24 +20,109 @@ #ifndef FEDERATION_H #define FEDERATION_H +#include + #include #include #include #include #include +#include #include +// === Stage-3/4 data model === +// +// A federation places one or more IFC models in a shared scene. Each model's +// final per-instance transform is composed as +// +// stage3 · stage4 · stage2 · placement_stage1 +// +// where: +// - stage1 is per-mesh vertex rebasing (load-time, immutable) +// - stage2 is the per-model georef matrix from IfcMapConversion etc. +// (load-time, immutable; can be toggled off) +// - stage3 is the federation-wide false origin (mutable, federation-scope) +// - stage4 is the per-model placement within the federation (mutable, per-model) +// +// All composed matrices are in metres. User-authored numbers are stored in +// their source units (model project unit / model map unit / federation unit) +// to round-trip without precision loss; conversion happens in the compose +// helpers. + +// Federation-wide unit; the value space for FederationOrigin.xyz and +// ModelTransform::{b, pivot}. +struct FederationConfig { + // IfcSIUnit name ("METRE") or IfcConversionBasedUnit name ("foot", "inch"). + std::string unit_name = "METRE"; + // SI prefix ("MILLI", "KILO", ...) — empty for unprefixed or for + // conversion-based units. + std::string unit_prefix = ""; +}; + +// Stage 3 — the federation false origin. Authoring intent is "nominate this +// XYZ as the new origin, with optional Z-axis heading rotation". Composed +// as stage3 = R_z(rz_deg) · T(-xyz_in_metres). +struct FederationOrigin { + Eigen::Vector3d xyz = Eigen::Vector3d::Zero(); // federation unit + double rz_deg = 0.0; +}; + +// Frame in which ModelTransform.a is expressed. +// ModelLocal — pre-stage2 model coordinates, in the model's project length unit +// ModelGlobal — post-stage2 model coordinates, in the model's map unit +enum class AFrame { ModelLocal, ModelGlobal }; + +// Stage 4 — the per-model placement within the federation. Authoring intent +// is "rotate the model around `pivot`, then translate so that point `a` lands +// at point `b`". Composed as +// R_local = R_z(rz) · R_y(ry) · R_x(rx) [intrinsic XYZ] +// R_at_pivot = T(pivot_m) · R_local · T(-pivot_m) +// stage4 = T(b_m - R_at_pivot · a_m) · R_at_pivot +struct ModelTransform { + AFrame a_frame = AFrame::ModelGlobal; + Eigen::Vector3d a = Eigen::Vector3d::Zero(); // model project / map unit + Eigen::Vector3d b = Eigen::Vector3d::Zero(); // federation unit + Eigen::Vector3d rxyz_deg = Eigen::Vector3d::Zero(); // degrees, intrinsic XYZ + Eigen::Vector3d pivot = Eigen::Vector3d::Zero(); // federation unit +}; + +// Per-model unit scales captured at load time. project_length_to_meters +// comes from calculateUnitScale(file, "LENGTHUNIT"); map_unit_to_meters from +// siScaleFromNamedUnit(getMapUnit(file)) and falls back to the project length +// scale when the model has no MapUnit. +struct ModelUnits { + double project_length_to_meters = 1.0; + double map_unit_to_meters = 1.0; +}; + +// 1 federation_unit -> N metres. +double federationUnitToMeters(const FederationConfig&); + +// Compose stage 3 (federation false origin) into a 4x4 matrix in metres. +Eigen::Matrix4d composeFederationOrigin(const FederationOrigin&, + const FederationConfig&); + +// Compose stage 4 (per-model placement within the federation) into a 4x4 +// matrix in metres. `stage2_meters` is the model's georef matrix (e.g. +// helmertMetersFromParameters · inv(wcs_meters)) — needed to lift `a` into +// metres when a_frame == ModelLocal. Pass identity when stage 2 is disabled +// or absent. +Eigen::Matrix4d composeModelTransform(const ModelTransform&, + const FederationConfig& fed_cfg, + const ModelUnits& model_units, + const Eigen::Matrix4d& stage2_meters); + +// === Federation persistence (.ifcfed) === +// // In-memory representation of an .ifcfed file (IFC federation). // // A federation is a named, ordered list of model sources plus an optional -// "home view" camera state. Source paths can be relative (resolved against -// the .ifcfed's directory) or absolute. Save() reserialises paths relative -// when they live under the federation file's directory tree, absolute -// otherwise — Save As recomputes against the new location. -// -// Round-trip-only fields today (no UI to edit, but preserved across load/ -// save): per-model `visible`, future cloud `source.kind`s. +// "home view" camera state, a federation-wide unit + false origin, and per +// model an optional transform intent. Source paths can be relative +// (resolved against the .ifcfed's directory) or absolute. Save() reserialises +// paths relative when they live under the federation file's directory tree, +// absolute otherwise — Save As recomputes against the new location. class Federation : public QObject { Q_OBJECT public: @@ -49,10 +134,11 @@ public: }; struct Model { - QString id; // stable, persisted + QString id; // stable, persisted QString display_name; - QString source_kind = "local"; // future: "http", "speckle", ... - QString source_path; // resolved absolute when kind == "local" + QString source_kind = "local"; // future: "http", "speckle", ... + QString source_path; // resolved absolute when kind == "local" + ModelTransform transform_intent; // stage 4 bool visible = true; }; @@ -70,6 +156,10 @@ public: void setHomeView(const HomeView& hv); void clearHomeView(); + void setConfig(const FederationConfig&); + void setOrigin(const FederationOrigin&); + void setModelTransform(const QString& fed_id, const ModelTransform&); + // Accessors const std::vector& models() const { return models_; } const Model* findById(const QString& fed_id) const; @@ -79,6 +169,8 @@ public: QString name() const { return name_; } bool hasHomeView() const { return has_home_view_; } const HomeView& homeView() const { return home_view_; } + const FederationConfig& config() const { return config_; } + const FederationOrigin& origin() const { return origin_; } signals: void dirtyChanged(bool dirty); @@ -93,6 +185,8 @@ private: QDateTime created_; QDateTime modified_; std::vector models_; + FederationConfig config_; + FederationOrigin origin_; bool has_home_view_ = false; HomeView home_view_; bool dirty_ = false; diff --git a/src/ifcviewer/tests/CMakeLists.txt b/src/ifcviewer/tests/CMakeLists.txt index 70436a9b73..bcf72ac87e 100644 --- a/src/ifcviewer/tests/CMakeLists.txt +++ b/src/ifcviewer/tests/CMakeLists.txt @@ -53,9 +53,15 @@ add_ifcviewer_unit_test(test_instanced_geometry) # enable AUTOMOC for the Q_OBJECT moc-generation. find_package(Qt${QT_VERSION} COMPONENTS Core Gui Test REQUIRED PATHS ${QT_DIR}) +find_package(Eigen3 REQUIRED) + add_executable(test_federation test_federation.cpp ${IFCVIEWER_SRC}/Federation.cpp + # Federation pulls in Unit::convert for federation_unit_to_meters; compile + # Unit.cpp directly so the test doesn't have to link the whole IfcViewer + # library (which would drag in Qt6::OpenGL, OpenCASCADE, etc.). + ${IFCVIEWER_SRC}/Unit.cpp ) set_target_properties(test_federation PROPERTIES AUTOMOC ON) target_include_directories(test_federation PRIVATE ${IFCVIEWER_SRC}) @@ -64,5 +70,7 @@ target_link_libraries(test_federation PRIVATE Qt${QT_VERSION}::Core Qt${QT_VERSION}::Gui # Federation::HomeView uses QVector3D from QtGui Qt${QT_VERSION}::Test # QSignalSpy + Eigen3::Eigen # Federation.h: composed matrices use Eigen + IfcParse # Unit.cpp uses express::Base / file APIs ) catch_discover_tests(test_federation) diff --git a/src/ifcviewer/tests/test_federation.cpp b/src/ifcviewer/tests/test_federation.cpp index 6226464220..42b2bfb4e6 100644 --- a/src/ifcviewer/tests/test_federation.cpp +++ b/src/ifcviewer/tests/test_federation.cpp @@ -304,3 +304,130 @@ TEST_CASE("load on malformed JSON fails with an error", "[federation]") { REQUIRE_FALSE(fed.load(bad, &warnings, &err)); REQUIRE_FALSE(err.isEmpty()); } + +TEST_CASE("config / origin / transform_intent round-trip through save+load", + "[federation]") { + ensureQApp(); + QTemporaryDir tmp; + REQUIRE(tmp.isValid()); + + QString src1 = writeStubFile(tmp.filePath("models/wall.ifc")); + QString fed_path = tmp.filePath("project.ifcfed"); + + Federation src; + QString id1 = src.addModel(src1, "Wall"); + + FederationConfig cfg; + cfg.unit_name = "FOOT"; + cfg.unit_prefix = ""; + src.setConfig(cfg); + + FederationOrigin org; + org.xyz = Eigen::Vector3d(100.0, 200.0, 30.0); + org.rz_deg = 45.0; + src.setOrigin(org); + + ModelTransform xf; + xf.a_frame = AFrame::ModelLocal; + xf.a = Eigen::Vector3d(1.0, 2.0, 3.0); + xf.b = Eigen::Vector3d(4.0, 5.0, 6.0); + xf.rxyz_deg = Eigen::Vector3d(90.0, 0.0, 0.0); + xf.pivot = Eigen::Vector3d(7.0, 8.0, 9.0); + src.setModelTransform(id1, xf); + + QString err; + REQUIRE(src.save(fed_path, &err)); + REQUIRE(err.isEmpty()); + + Federation dst; + QStringList warnings; + REQUIRE(dst.load(fed_path, &warnings, &err)); + REQUIRE(err.isEmpty()); + REQUIRE(warnings.isEmpty()); + + REQUIRE(dst.config().unit_name == "FOOT"); + REQUIRE(dst.config().unit_prefix == ""); + + REQUIRE(dst.origin().xyz == org.xyz); + REQUIRE(dst.origin().rz_deg == 45.0); + + REQUIRE(dst.models().size() == 1); + const auto& m = dst.models()[0]; + REQUIRE(m.id == id1); + REQUIRE(m.transform_intent.a_frame == AFrame::ModelLocal); + REQUIRE(m.transform_intent.a == xf.a); + REQUIRE(m.transform_intent.b == xf.b); + REQUIRE(m.transform_intent.rxyz_deg == xf.rxyz_deg); + REQUIRE(m.transform_intent.pivot == xf.pivot); +} + +TEST_CASE("default ModelTransform is omitted from saved JSON", "[federation]") { + ensureQApp(); + QTemporaryDir tmp; + REQUIRE(tmp.isValid()); + + QString src1 = writeStubFile(tmp.filePath("models/wall.ifc")); + QString fed_path = tmp.filePath("project.ifcfed"); + + Federation src; + src.addModel(src1, "Wall"); + QString err; + REQUIRE(src.save(fed_path, &err)); + + QJsonObject root = readJsonFile(fed_path); + QJsonArray models = root.value("models").toArray(); + REQUIRE(models.size() == 1); + REQUIRE_FALSE(models[0].toObject().contains("transform_intent")); +} + +TEST_CASE("composeFederationOrigin moves the nominated point to the origin", + "[federation][compose]") { + FederationConfig cfg; // METRE, no prefix + FederationOrigin org; + org.xyz = Eigen::Vector3d(10.0, 20.0, 5.0); + org.rz_deg = 0.0; + + Eigen::Matrix4d M = composeFederationOrigin(org, cfg); + + // The nominated point (10, 20, 5) should map to (0, 0, 0). + Eigen::Vector4d p(10.0, 20.0, 5.0, 1.0); + Eigen::Vector4d r = M * p; + REQUIRE(std::abs(r.x()) < 1e-9); + REQUIRE(std::abs(r.y()) < 1e-9); + REQUIRE(std::abs(r.z()) < 1e-9); +} + +TEST_CASE("composeFederationOrigin scales by federation unit", + "[federation][compose]") { + FederationConfig cfg; + cfg.unit_name = "FOOT"; // 1 ft = 0.3048 m + FederationOrigin org; + org.xyz = Eigen::Vector3d(1.0, 0.0, 0.0); // 1 foot in fed coords + + Eigen::Matrix4d M = composeFederationOrigin(org, cfg); + // Translation column should be -1 ft = -0.3048 m. + REQUIRE(std::abs(M(0, 3) - (-0.3048)) < 1e-9); +} + +TEST_CASE("composeModelTransform with pivot=B keeps A landing on B", + "[federation][compose]") { + // A in ModelGlobal frame, federation in metres, model has identity stage 2. + FederationConfig fed_cfg; // METRE + ModelUnits mu; // 1.0 / 1.0 (already in metres) + Eigen::Matrix4d stage2 = Eigen::Matrix4d::Identity(); + + ModelTransform xf; + xf.a_frame = AFrame::ModelGlobal; + xf.a = Eigen::Vector3d(5.0, 0.0, 0.0); + xf.b = Eigen::Vector3d(100.0, 50.0, 10.0); + xf.rxyz_deg = Eigen::Vector3d(0.0, 0.0, 30.0); + xf.pivot = xf.b; // pivot at B preserves A->B regardless of rotation + + Eigen::Matrix4d M = composeModelTransform(xf, fed_cfg, mu, stage2); + + Eigen::Vector4d a(xf.a.x(), xf.a.y(), xf.a.z(), 1.0); + Eigen::Vector4d r = M * a; + REQUIRE(std::abs(r.x() - xf.b.x()) < 1e-9); + REQUIRE(std::abs(r.y() - xf.b.y()) < 1e-9); + REQUIRE(std::abs(r.z() - xf.b.z()) < 1e-9); +}