Add plane() to viz::Polygon

source/RobotAPI/components/ArViz/Client/Elements.h

@@ -12,6 +12,12 @@
 #include "elements/PointCloud.h"
 
 
+namespace Eigen
+{
+    using Hyperplane3f = Hyperplane<float, 3>;
+}
+
+
 namespace armarx::viz
 {
     using data::ColoredPoint;
@@ -172,6 +178,11 @@ namespace armarx::viz
     public:
         using ElementOps::ElementOps;
 
+        Polygon& clear()
+        {
+            data_->points.clear();
+        }
+
         Polygon& lineColor(Color color)
         {
             data_->lineColor = color;
@@ -205,6 +216,29 @@ namespace armarx::viz
 
             return *this;
         }
+
+        /**
+         * @brief Add points representing a plane as rectangle.
+         * @param plane The plane.
+         * @param at Center of rectangle, is projected onto plane.
+         * @param size Extents of rectangle.
+         */
+        Polygon& plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
+        {
+            const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
+                                               Eigen::Vector3f::UnitZ(), plane.normal());
+
+            const Eigen::Vector3f x = 0.5f * size.x() * (ori * Eigen::Vector3f::UnitX());
+            const Eigen::Vector3f y = 0.5f * size.y() * (ori * Eigen::Vector3f::UnitY());
+
+            const Eigen::Vector3f& origin = plane.projection(at);
+            addPoint(origin + x + y);
+            addPoint(origin - x + y);
+            addPoint(origin - x - y);
+            addPoint(origin + x - y);
+
+            return *this;
+        }
     };