obb.cpp
Go to the documentation of this file.
1 #include <geometric_shapes/obb.h>
2 
3 #include <fcl/config.h>
4 
5 #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
6 #include <fcl/BV/OBB.h>
7 typedef fcl::Vec3f FCL_Vec3;
8 typedef fcl::OBB FCL_OBB;
9 #else
10 #include <fcl/math/bv/OBB.h>
13 #endif
14 
15 namespace bodies
16 {
17 #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
18 inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec)
19 {
20  FCL_Vec3 result;
21  Eigen::Map<Eigen::MatrixXd>(result.data.vs, 3, 1) = eigenVec;
22  return result;
23 }
24 #else
25 #define toFcl
26 #endif
27 
28 #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
29 Eigen::Vector3d fromFcl(const FCL_Vec3& fclVec)
30 {
31  return Eigen::Map<const Eigen::MatrixXd>(fclVec.data.vs, 3, 1);
32 }
33 #else
34 #define fromFcl
35 #endif
36 
37 class OBBPrivate : public FCL_OBB
38 {
39 public:
40  using FCL_OBB::OBB;
41 };
42 
44 {
45  this->obb_.reset(new OBBPrivate);
46 }
47 
48 OBB::OBB(const OBB& other) : OBB()
49 {
50  *obb_ = *other.obb_;
51 }
52 
53 OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) : OBB()
54 {
56 }
57 
58 OBB& OBB::operator=(const OBB& other)
59 {
60  *obb_ = *other.obb_;
61  return *this;
62 }
63 
64 void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents)
65 {
66  const auto rotation = pose.linear();
67 
68 #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
69  obb_->axis[0] = toFcl(rotation.col(0));
70  obb_->axis[1] = toFcl(rotation.col(1));
71  obb_->axis[2] = toFcl(rotation.col(2));
72 #else
73  obb_->axis = rotation;
74 #endif
75 
76  obb_->To = toFcl(pose.translation());
77 
78  obb_->extent = { extents[0] / 2.0, extents[1] / 2.0, extents[2] / 2.0 };
79 }
80 
81 void OBB::getExtents(Eigen::Vector3d& extents) const
82 {
83  extents = 2 * fromFcl(obb_->extent);
84 }
85 
86 Eigen::Vector3d OBB::getExtents() const
87 {
88  Eigen::Vector3d extents;
90  return extents;
91 }
92 
93 void OBB::getPose(Eigen::Isometry3d& pose) const
94 {
95  pose = Eigen::Isometry3d::Identity();
96  pose.translation() = fromFcl(obb_->To);
97  // If all axes are zero, we report the rotation as identity
98  // This happens if OBB is default-constructed
99 #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
100  if (!obb_->axis[0].isZero() && !obb_->axis[3].isZero() && !obb_->axis[2].isZero())
101  {
102  pose.linear().col(0) = fromFcl(obb_->axis[0]);
103  pose.linear().col(1) = fromFcl(obb_->axis[1]);
104  pose.linear().col(2) = fromFcl(obb_->axis[2]);
105  }
106 #else
107  if (!obb_->axis.isApprox(fcl::Matrix3d::Zero()))
108  {
109  pose.linear() = obb_->axis;
110  }
111 #endif
112 }
113 
114 Eigen::Isometry3d OBB::getPose() const
115 {
116  Eigen::Isometry3d pose;
117  getPose(pose);
118  return pose;
119 }
120 
122 {
123  AABB result;
124  toAABB(result);
125  return result;
126 }
127 
128 void OBB::toAABB(AABB& aabb) const
129 {
130  aabb.extendWithTransformedBox(getPose(), getExtents());
131 }
132 
134 {
135  *this->obb_ += *box.obb_;
136  return this;
137 }
138 
139 bool OBB::contains(const Eigen::Vector3d& point)
140 {
141  return obb_->contain(toFcl(point));
142 }
143 
144 bool OBB::overlaps(const bodies::OBB& other)
145 {
146  return obb_->overlap(*other.obb_);
147 }
148 
149 OBB::~OBB() = default;
150 
151 } // namespace bodies
obb.h
bodies::OBB::getExtents
Eigen::Vector3d getExtents() const
Get the extents of the OBB.
Definition: obb.cpp:86
bodies::OBB::toAABB
AABB toAABB() const
Convert this OBB to an axis-aligned BB.
Definition: obb.cpp:121
bodies::OBB::~OBB
virtual ~OBB()
FCL_Vec3
fcl::Vector3d FCL_Vec3
Definition: obb.cpp:11
bodies::OBB::contains
bool contains(const Eigen::Vector3d &point)
Check if this OBB contains the given point.
Definition: obb.cpp:139
bodies::OBB::OBB
OBB()
Definition: obb.cpp:43
fromFcl
#define fromFcl
Definition: obb.cpp:34
toFcl
#define toFcl
Definition: obb.cpp:25
bodies::OBB::extendApprox
OBB * extendApprox(const OBB &box)
Add the other OBB to this one and compute an approximate enclosing OBB.
Definition: obb.cpp:133
bodies::OBB::obb_
std::unique_ptr< OBBPrivate > obb_
PIMPL pointer.
Definition: obb.h:160
fcl::Vector3d
Vector3< double > Vector3d
fcl::OBB
bodies::OBB::overlaps
bool overlaps(const OBB &other)
Check whether this and the given OBBs have nonempty intersection.
Definition: obb.cpp:144
bodies::OBB
Represents an oriented bounding box.
Definition: obb.h:84
FCL_OBB
fcl::OBB< double > FCL_OBB
Definition: obb.cpp:12
point
std::chrono::system_clock::time_point point
bodies::OBB::getPose
Eigen::Isometry3d getPose() const
Get the pose of the OBB.
Definition: obb.cpp:114
OBB.h
bodies
This set of classes allows quickly detecting whether a given point is inside an object or not....
Definition: aabb.h:42
bodies::OBBPrivate
Definition: obb.cpp:37
bodies::OBB::setPoseAndExtents
void setPoseAndExtents(const Eigen::Isometry3d &pose, const Eigen::Vector3d &extents)
Set both the pose and extents of the OBB.
Definition: obb.cpp:64
bodies::OBB::operator=
OBB & operator=(const OBB &other)
Definition: obb.cpp:58
aabb
SaPCollisionManager< S >::SaPAABB * aabb
extents
std::array< S, 6 > & extents()
fcl::OBB::OBB
OBB()
bodies::AABB
Represents an axis-aligned bounding box.
Definition: aabb.h:77


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Thu Aug 25 2022 02:41:02