mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
shapes.h
Go to the documentation of this file.
1// clang: MatousFormat
8#ifndef SHAPES_H
9#define SHAPES_H
10
11#include <optional>
12
13#include <Eigen/Dense>
14
15namespace mrs_lib
16{
17 namespace geometry
18 {
19
20 /* class Ray //{ */
21
25 class Ray
26 {
27 public:
31 Ray();
32
36 ~Ray();
37
44 Ray(Eigen::Vector3d p1, Eigen::Vector3d p2);
45
46 private:
47 Eigen::Vector3d point1;
48 Eigen::Vector3d point2;
49
50 public:
56 const Eigen::Vector3d p1() const;
62 const Eigen::Vector3d p2() const;
63
69 const Eigen::Vector3d direction() const;
70
71 public:
80 static Ray twopointCast(Eigen::Vector3d pointFrom, Eigen::Vector3d pointTo);
81
90 static Ray directionCast(Eigen::Vector3d origin, Eigen::Vector3d direction);
91 };
92
93 //}
94
95 /* class Triangle //{ */
100 {
101 public:
105 Triangle();
106
110 ~Triangle();
111
119 Triangle(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
120
121 private:
122 Eigen::Vector3d point1;
123 Eigen::Vector3d point2;
124 Eigen::Vector3d point3;
125
126 public:
132 const Eigen::Vector3d a() const;
133
139 const Eigen::Vector3d b() const;
140
146 const Eigen::Vector3d c() const;
147
153 const Eigen::Vector3d center() const;
154
161 const Eigen::Vector3d normal() const;
162
168 const std::vector<Eigen::Vector3d> vertices() const;
169
170 public:
179 const std::optional<Eigen::Vector3d> intersectionRay(Ray r, double epsilon = 1e-4) const;
180 };
181 //}
182
183 /* class Rectangle //{ */
184
189 {
190 public:
194 Rectangle();
195
199 ~Rectangle();
200
206 Rectangle(std::vector<Eigen::Vector3d> points);
207
216 Rectangle(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c, Eigen::Vector3d d);
217
218 private:
219 Eigen::Vector3d point1;
220 Eigen::Vector3d point2;
221 Eigen::Vector3d point3;
222 Eigen::Vector3d point4;
223
224 public:
230 const Eigen::Vector3d a() const;
231
237 const Eigen::Vector3d b() const;
238
244 const Eigen::Vector3d c() const;
245
251 const Eigen::Vector3d d() const;
252
258 const Eigen::Vector3d center() const;
259
266 const Eigen::Vector3d normal() const;
267
273 const std::vector<Eigen::Vector3d> vertices() const;
274
280 const std::vector<Triangle> triangles() const;
281
290 const std::optional<Eigen::Vector3d> intersectionRay(Ray r, double epsilon = 1e-4) const;
291
299 bool isFacing(Eigen::Vector3d point) const;
300
301
309 double solidAngleRelativeTo(Eigen::Vector3d point) const;
310 };
311
312 //}
313
314 /* class Cuboid //{ */
318 class Cuboid
319 {
320 public:
324 Cuboid();
325
329 ~Cuboid();
330
343 Cuboid(Eigen::Vector3d p0, Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3, Eigen::Vector3d p4, Eigen::Vector3d p5, Eigen::Vector3d p6,
344 Eigen::Vector3d p7);
350 Cuboid(std::vector<Eigen::Vector3d> points);
351
359 Cuboid(Eigen::Vector3d center, Eigen::Vector3d size, Eigen::Quaterniond orientation);
360
361 private:
365 enum
366 {
367 FRONT = 0,
368 BACK = 1,
369 LEFT = 2,
370 RIGHT = 3,
371 BOTTOM = 4,
372 TOP = 5,
373 };
374
375 private:
376 std::vector<Eigen::Vector3d> points;
377
385 std::vector<Eigen::Vector3d> lookupPoints(int face_idx) const;
386
387 public:
393 const std::vector<Eigen::Vector3d> vertices() const;
394
400 const Eigen::Vector3d center() const;
401
409 const Rectangle getRectangle(int face_idx) const;
410
419 const std::vector<Eigen::Vector3d> intersectionRay(Ray r, double epsilon = 1e-4) const;
420 };
421 //}
422
423 /* class Ellipse //{ */
428 {
429 public:
433 Ellipse();
434
438 ~Ellipse();
439
448 Ellipse(Eigen::Vector3d center, Eigen::Quaterniond orientation, double a, double b);
449
450 private:
451 double major_semi;
452 double minor_semi;
453 Eigen::Vector3d center_point;
454 Eigen::Quaterniond absolute_orientation;
455
456 public:
462 double a() const;
463
469 double b() const;
470
476 const Eigen::Vector3d center() const;
477
483 const Eigen::Quaterniond orientation() const;
484 };
485 //}
486
487 /* class Cylinder //{ */
492 {
493 public:
497 Cylinder();
498
502 ~Cylinder();
503
512 Cylinder(Eigen::Vector3d center, double radius, double height, Eigen::Quaterniond orientation);
513
514 private:
515 Eigen::Vector3d center_point;
516 double radius;
517 double height;
518 Eigen::Quaterniond absolute_orientation;
519
520 public:
524 enum
525 {
526 BOTTOM = 0,
527 TOP = 1,
528 };
529
530 public:
536 const Eigen::Vector3d center() const;
537
543 const Eigen::Quaterniond orientation() const;
544
550 double r() const;
551
557 double h() const;
558
566 const Ellipse getCap(int index) const;
567 };
568 //}
569
570 /* class Cone //{ */
574 class Cone
575 {
576 public:
580 Cone();
581
585 ~Cone();
586
595 Cone(Eigen::Vector3d origin_point, double angle, double height, Eigen::Vector3d orientation);
596
597 private:
598 Eigen::Vector3d origin_point;
599 double angle;
600 double height;
601 Eigen::Vector3d absolute_direction;
602
603 public:
609 const Eigen::Vector3d origin() const;
610
616 const Eigen::Vector3d direction() const;
617
623 const Eigen::Vector3d center() const;
624
630 double theta() const;
631
637 double h() const;
638
644 const Ellipse getCap() const;
645
653 const std::optional<Eigen::Vector3d> projectPoint(const Eigen::Vector3d& point) const;
654 };
655 //}
656 } // namespace geometry
657} // namespace mrs_lib
658
659#endif // SHAPES_H
geometric representaiton of a cone
Definition shapes.h:575
const Eigen::Vector3d center() const
getter for the center point. Center point lies in half of the body height
Definition shapes.cpp:585
Cone()
constructor without setting the internal variables
Definition shapes.cpp:557
const Ellipse getCap() const
getter for the cap of the cone
Definition shapes.cpp:600
double h() const
getter for body height
Definition shapes.cpp:595
const Eigen::Vector3d origin() const
getter for the tip point
Definition shapes.cpp:575
double theta() const
getter for angle between body height and body side
Definition shapes.cpp:590
const std::optional< Eigen::Vector3d > projectPoint(const Eigen::Vector3d &point) const
Project a 3D point orthogonally onto the Cone surface.
Definition shapes.cpp:609
const Eigen::Vector3d direction() const
getter for the direction. Normalized direction from origin towards base
Definition shapes.cpp:580
~Cone()
destructor
Definition shapes.cpp:561
geometric representation of a cuboid
Definition shapes.h:319
Cuboid()
constructor for initialization with all points set to [0,0,0]
Definition shapes.cpp:295
const Rectangle getRectangle(int face_idx) const
getter for one side corresponding to a provided index
Definition shapes.cpp:411
~Cuboid()
virtual destructor
Definition shapes.cpp:353
const Eigen::Vector3d center() const
getter for the center point
Definition shapes.cpp:416
const std::vector< Eigen::Vector3d > intersectionRay(Ray r, double epsilon=1e-4) const
calculate the intersection between this cuboid and a provided ray within a given tolerance....
Definition shapes.cpp:428
const std::vector< Eigen::Vector3d > vertices() const
getter for all vertices (vector3) of this cuboid
Definition shapes.cpp:406
geometric representation of a cylinder
Definition shapes.h:492
double h() const
getter for the body height
Definition shapes.cpp:527
const Eigen::Vector3d center() const
getter for the center point
Definition shapes.cpp:512
const Ellipse getCap(int index) const
getter for a cap corresponding to a provided index
Definition shapes.cpp:532
~Cylinder()
destructor
Definition shapes.cpp:498
double r() const
getter for cap radius
Definition shapes.cpp:522
const Eigen::Quaterniond orientation() const
getter for the orientation
Definition shapes.cpp:517
Cylinder()
constructor without setting the internal variables
Definition shapes.cpp:494
geometric representation of an ellipse
Definition shapes.h:428
const Eigen::Vector3d center() const
getter for the center point
Definition shapes.cpp:477
const Eigen::Quaterniond orientation() const
getter for the orientation
Definition shapes.cpp:482
~Ellipse()
destructor
Definition shapes.cpp:453
double b() const
getter for minor semi-axis
Definition shapes.cpp:472
Ellipse()
constructor for initialization without setting internal variables
Definition shapes.cpp:449
double a() const
getter for major semi-axis
Definition shapes.cpp:467
geometric representation of a ray. Instantiate it by two input Vector3. Use static methods for from-t...
Definition shapes.h:26
const Eigen::Vector3d direction() const
get the direction of ray (normalized)
Definition shapes.cpp:41
static Ray twopointCast(Eigen::Vector3d pointFrom, Eigen::Vector3d pointTo)
static method for generating new rays by raycasting from-to
Definition shapes.cpp:48
const Eigen::Vector3d p2() const
get the end point
Definition shapes.cpp:36
const Eigen::Vector3d p1() const
get the origin point
Definition shapes.cpp:31
Ray()
constructor without initialization of internal variables
Definition shapes.cpp:13
static Ray directionCast(Eigen::Vector3d origin, Eigen::Vector3d direction)
static method for generating new rays by raycasting origin-direction
Definition shapes.cpp:53
~Ray()
destructor
Definition shapes.cpp:25
geometric representation of a rectangle (can represent any quadrilateral)
Definition shapes.h:189
const std::vector< Eigen::Vector3d > vertices() const
getter for all the points of this rectangle provided as std::vector
Definition shapes.cpp:224
const std::optional< Eigen::Vector3d > intersectionRay(Ray r, double epsilon=1e-4) const
calculate an intersection of this rectangle with a given ray with given tolerance
Definition shapes.cpp:247
const std::vector< Triangle > triangles() const
getter for the triangles forming this rectangle
Definition shapes.cpp:234
const Eigen::Vector3d a() const
getter for first point
Definition shapes.cpp:192
const Eigen::Vector3d normal() const
getter for the normal vector. It originates in the center of the Rectangle, length is normalized,...
Definition shapes.cpp:217
bool isFacing(Eigen::Vector3d point) const
check if the normal is facing a given point, i.e. if the point lies in the same half-space as the rec...
Definition shapes.cpp:261
~Rectangle()
destructor
Definition shapes.cpp:186
double solidAngleRelativeTo(Eigen::Vector3d point) const
compute the solid angle of this rectangle relative to a given sphere center
Definition shapes.cpp:271
const Eigen::Vector3d b() const
getter for the second point
Definition shapes.cpp:197
const Eigen::Vector3d c() const
getter for the third point
Definition shapes.cpp:202
const Eigen::Vector3d center() const
getter for center point
Definition shapes.cpp:212
const Eigen::Vector3d d() const
getter for the fourth point
Definition shapes.cpp:207
Rectangle()
constructor for initialization with points set to [0,0,0], [1,0,0], [1,1,0] and [0,...
Definition shapes.cpp:162
geometric representation of a triangle. Instantiate a new triangle by providing three vertices
Definition shapes.h:100
Triangle()
constructor for initialization with default values (points [0,0,0], [1,0,0] and [0,...
Definition shapes.cpp:64
const Eigen::Vector3d center() const
get position on the triangle center
Definition shapes.cpp:106
const std::optional< Eigen::Vector3d > intersectionRay(Ray r, double epsilon=1e-4) const
calculate an intersection of this triangle with a given ray with given tolerance
Definition shapes.cpp:122
const Eigen::Vector3d b() const
getter for second point
Definition shapes.cpp:89
const Eigen::Vector3d a() const
getter for first point
Definition shapes.cpp:84
const Eigen::Vector3d normal() const
get normal vector of this triangle. The vector origin is placed at triangle center,...
Definition shapes.cpp:99
~Triangle()
destructor
Definition shapes.cpp:78
const std::vector< Eigen::Vector3d > vertices() const
get a vector of all vertices
Definition shapes.cpp:111
const Eigen::Vector3d c() const
getter for third point
Definition shapes.cpp:94
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24