From d1e19b5946ce5eec64f1b7e2025ee71bba736c3e Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 5 Feb 2020 17:07:11 +0100
Subject: [PATCH] Add plane() to viz::Polygon
---
.../components/ArViz/Client/Elements.h | 34 +++++++++++++++++++
1 file changed, 34 insertions(+)
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index c180ece80..b5dc031fd 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/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;
+ }
};
--
GitLab