From 1289997dfd604cb24f060f905f04ec3728d964e4 Mon Sep 17 00:00:00 2001
From: PaddleStroke <pierrelouis.boyer@gmail.com>
Date: Fri, 13 Dec 2024 17:44:54 +0100
Subject: [PATCH] Placement: Adds a helper to print the value of the placement.

---
 src/Base/Placement.cpp | 21 +++++++++++++++++++++
 src/Base/Placement.h   |  5 +++++
 2 files changed, 26 insertions(+)

diff --git a/src/Base/Placement.cpp b/src/Base/Placement.cpp
index 6e335d495e..d894abe386 100644
--- a/src/Base/Placement.cpp
+++ b/src/Base/Placement.cpp
@@ -22,6 +22,8 @@
 
 #include "PreCompiled.h"
 
+#include <fmt/format.h>
+
 #include "Placement.h"
 #include "Matrix.h"
 #include "Rotation.h"
@@ -209,3 +211,22 @@ Placement Placement::sclerp(const Placement& p0, const Placement& p1, double t,
     Placement trf = p0.inverse() * p1;
     return p0 * trf.pow(t, shorten);
 }
+
+std::string Placement::toString()
+{
+    Base::Vector3d pos = getPosition();
+    Base::Rotation rot = getRotation();
+
+    Base::Vector3d axis;
+    double angle;
+    rot.getRawValue(axis, angle);
+
+    return fmt::format("position ({.1f}, {.1f}, {.1f}), axis ({.1f}, {.1f}, {.1f}), angle {.1f}\n",
+                       pos.x,
+                       pos.y,
+                       pos.z,
+                       axis.x,
+                       axis.y,
+                       axis.z,
+                       angle);
+}
diff --git a/src/Base/Placement.h b/src/Base/Placement.h
index 39c11d470f..ca4f47ce5f 100644
--- a/src/Base/Placement.h
+++ b/src/Base/Placement.h
@@ -23,6 +23,8 @@
 #ifndef BASE_PLACEMENT_H
 #define BASE_PLACEMENT_H
 
+#include <string>
+
 #include "Rotation.h"
 #include "Vector3D.h"
 
@@ -105,6 +107,9 @@ public:
     static Placement
     sclerp(const Placement& p0, const Placement& p1, double t, bool shorten = true);
 
+    /// Returns string representation of the placement, useful for debugging
+    std::string toString();
+
 private:
     Vector3<double> _pos;
     Base::Rotation _rot;