Elements.cpp
Go to the documentation of this file.
1 #include "Elements.h"
2 
6 
7 #include <SimoxUtility/math/normal/normal_to_mat4.h>
8 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
9 #include <SimoxUtility/math/pose/transform.h>
10 
11 
12 namespace armarx::viz
13 {
14 
15  struct Convert
16  {
17  static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
18  {
19  dir = dir.normalized();
20  Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
21  Eigen::Vector3f cross = naturalDir.cross(dir);
22  float dot = naturalDir.dot(dir);
23  float angle = std::acos(dot);
24  if (cross.squaredNorm() < 1.0e-12)
25  {
26  // Directions are almost colinear ==> Do no rotation
27  cross = Eigen::Vector3f::UnitX();
28  if (dot < 0)
29  {
30  angle = M_PI;
31  }
32  else
33  {
34  angle = 0.0f;
35  }
36  }
37  Eigen::Vector3f axis = cross.normalized();
38  Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
39 
40  return ori;
41  }
42  };
43 
46 
47  Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
48  const std::string& relativeObjectsDirectory)
49  {
50  return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
51  }
52 
53  Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
54  const std::string& relativeObjectsDirectory)
55  {
56  ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
58  return this->file(file.package, file.relativePath);
59  }
60 
61  Object& Object::alpha(float alpha)
62  {
63  if (alpha < 1)
64  {
65  overrideColor(simox::Color::white().with_alpha(alpha));
66  }
67  return *this;
68  }
69 
70  Box& Box::set(const simox::OrientedBoxBase<float>& b)
71  {
72  size(b.dimensions());
73  return pose(b.transformation_centered());
74  }
75 
76  Box& Box::set(const simox::OrientedBoxBase<double>& b)
77  {
78  size(b.dimensions<float>());
79  return pose(b.transformation_centered<float>());
80  }
81 
82  Cylinder& Cylinder::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
83  {
84  position((to + from) / 2);
85  orientation(Convert::directionToQuaternion((to - from).normalized()));
86  height((to - from).norm());
87 
88  return *this;
89  }
90 
91  Cylinder& Cylinder::direction(Eigen::Vector3f direction)
92  {
94 
95  return *this;
96  }
97 
98  Arrow& Arrow::direction(Eigen::Vector3f dir)
99  {
101  }
102 
103  ArrowCircle& ArrowCircle::normal(Eigen::Vector3f dir)
104  {
105  Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
106  Eigen::Vector3f cross = naturalDir.cross(dir);
107  float angle = std::acos(naturalDir.dot(dir));
108  if (cross.squaredNorm() < 1.0e-12f)
109  {
110  // Directions are almost colinear ==> Angle is either 0 or 180 deg
111  cross = Eigen::Vector3f::UnitX();
112  // Keep angle
113  }
114  Eigen::Vector3f axis = cross.normalized();
115  Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
116 
117  return orientation(ori);
118  }
119 
120  Polygon& Polygon::points(const std::vector<Eigen::Vector3f>& ps)
121  {
122  auto& points = data_->points;
123  points.clear();
124  points.reserve(ps.size());
125  for (auto& p : ps)
126  {
127  points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
128  }
129 
130  return *this;
131  }
132 
133  Polygon& Polygon::plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
134  {
135  const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
136  Eigen::Vector3f::UnitZ(), plane.normal());
137  return this->plane(plane.projection(at), ori, size);
138  }
139 
140  Polygon& Polygon::plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
141  {
142  const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
143  const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
144 
145  addPoint(center + x + y);
146  addPoint(center - x + y);
147  addPoint(center - x - y);
148  addPoint(center + x - y);
149 
150  return *this;
151  }
152 
153  Polygon& Polygon::circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation)
154  {
155  const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
156  ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
157 
158  const float angle = 2 * M_PI / tessellation;
159  const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
160 
161  Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
162  addPoint(simox::math::transform_position(pose, lastLocalPoint));
163  while (--tessellation)
164  {
165  const Eigen::Vector3f localPoint = rot * lastLocalPoint;
166  addPoint(simox::math::transform_position(pose, localPoint));
167  lastLocalPoint = localPoint;
168  }
169  return *this;
170  }
171 
172 
173 }
174 
175 
armarx::viz::Convert
Definition: Elements.cpp:15
armarx::viz::Polygon::points
Polygon & points(std::vector< Eigen::Vector3f > const &ps)
Definition: Elements.cpp:120
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:11
armarx::ObjectFinder::DefaultObjectsPackageName
static const std::string DefaultObjectsPackageName
Definition: ObjectFinder.h:28
armarx::viz::Cylinder::fromTo
Cylinder & fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
Definition: Elements.cpp:82
armarx::viz::Object::fileByObjectFinder
Object & fileByObjectFinder(const armarx::ObjectID &objectID, const std::string &objectsPackage=DefaultObjectsPackage, const std::string &relativeObjectsDirectory=DefaultRelativeObjectsDirectory)
Set the file so it could be found using armarx::ObjectFinder (also on remote machine).
Definition: Elements.cpp:53
armarx::viz::Object::file
Object & file(std::string const &project, std::string const &filename)
Definition: Elements.h:328
armarx::viz::Arrow
Definition: Elements.h:198
armarx::PackageFileLocation
Definition: ObjectInfo.h:22
armarx::viz::Cylinder::height
Cylinder & height(float h)
Definition: Elements.h:85
armarx::viz::ArrowCircle::normal
ArrowCircle & normal(Eigen::Vector3f dir)
Definition: Elements.cpp:103
armarx::viz::ElementOps< Polygon, data::ElementPolygon >::data_
IceInternal::Handle< data::ElementPolygon > data_
Definition: ElementOps.h:281
armarx::viz::Object::alpha
Object & alpha(float alpha)
Definition: Elements.cpp:61
armarx::viz::Convert::directionToQuaternion
static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
Definition: Elements.cpp:17
armarx::viz::Object::DefaultRelativeObjectsDirectory
static const std::string DefaultRelativeObjectsDirectory
Definition: Elements.h:324
Elements.h
armarx::viz::ElementOps< Cylinder, data::ElementCylinder >::position
Cylinder & position(float x, float y, float z)
Definition: ElementOps.h:127
armarx::viz::Cylinder::direction
Cylinder & direction(Eigen::Vector3f direction)
Definition: Elements.cpp:91
armarx::viz::ElementOps< Box, data::ElementBox >::pose
Eigen::Matrix4f pose() const
Definition: ElementOps.h:179
armarx::viz::Object
Definition: Elements.h:321
armarx::viz::Polygon::addPoint
Polygon & addPoint(Eigen::Vector3f p)
Definition: Elements.h:294
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::viz::Polygon::clear
Polygon & clear()
Definition: Elements.h:262
armarx::viz::Cylinder
Definition: Elements.h:74
Eigen::Hyperplane3f
Hyperplane< float, 3 > Hyperplane3f
Definition: Elements.h:38
ObjectID.h
armarx::viz::ElementOps< Cylinder, data::ElementCylinder >::orientation
Cylinder & orientation(Eigen::Quaternionf const &ori)
Definition: ElementOps.h:140
armarx::viz::Arrow::direction
Arrow & direction(Eigen::Vector3f dir)
Definition: Elements.cpp:98
armarx::viz::Polygon::plane
Polygon & plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
Add points representing a plane as rectangle.
Definition: Elements.cpp:133
cross
Point cross(const Point &x, const Point &y)
Definition: point.hpp:33
armarx::viz::Box
Definition: Elements.h:51
ObjectInfo.h
armarx::viz::Polygon
Definition: Elements.h:258
armarx::viz::Object::overrideColor
Object & overrideColor(Color c)
Definition: Elements.h:369
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
armarx::viz::Polygon::circle
Polygon & circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation=64)
Definition: Elements.cpp:153
armarx::ObjectInfo::simoxXML
PackageFileLocation simoxXML() const
Definition: ObjectInfo.cpp:109
armarx::PackageFileLocation::package
std::string package
Name of the ArmarX package.
Definition: ObjectInfo.h:25
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::Quaternion< float, 0 >
armarx::viz::Box::size
Box & size(Eigen::Vector3f const &s)
Definition: Elements.h:55
dot
double dot(const Point &x, const Point &y)
Definition: point.hpp:53
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
armarx::viz::Object::DefaultObjectsPackage
static const std::string DefaultObjectsPackage
Definition: Elements.h:323
armarx::viz::Box::set
Box & set(const simox::OrientedBoxBase< float > &b)
Definition: Elements.cpp:70
armarx::viz
This file is part of ArmarX.
Definition: ArVizStorage.cpp:416
armarx::ObjectFinder::DefaultObjectsDirectory
static const std::string DefaultObjectsDirectory
Definition: ObjectFinder.h:29
armarx::ObjectInfo
Accessor for the object files.
Definition: ObjectInfo.h:37
ObjectFinder.h
norm
double norm(const Point &a)
Definition: point.hpp:94
armarx::viz::ArrowCircle
Definition: Elements.h:229