Placement: Adds a helper to print the value of the placement.

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);
+}

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;