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

Author: PaddleStroke

src/Base/Placement.cpp

@@ -22,6 +22,7 @@
 
 #include "PreCompiled.h"
 
+#include "Console.h"
 #include "Placement.h"
 #include "Matrix.h"
 #include "Rotation.h"
@@ -209,3 +210,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

@@ -105,6 +105,9 @@ class BaseExport Placement
     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 Placement::toString();
+
 private:
     Vector3<double> _pos;
     Base::Rotation _rot;