OOBB.hpp
Go to the documentation of this file.
1 // ========================================================================================
2 // ApproxMVBB
3 // Copyright (C) 2014 by Gabriel Nützi <nuetzig (at) imes (d0t) mavt (d0t) ethz (døt) ch>
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8 // ========================================================================================
9 
10 #ifndef ApproxMVBB_OOBB_hpp
11 #define ApproxMVBB_OOBB_hpp
12 
14 #include ApproxMVBB_TypeDefs_INCLUDE_FILE
15 #include ApproxMVBB_AABB_INCLUDE_FILE
16 #include "ApproxMVBB/AABB.hpp"
17 
18 namespace ApproxMVBB{
19 
20 
22 public:
23 
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
28 
29  OOBB() {
30  this->reset();
31  }
32 
33  OOBB(const OOBB &) = default;
34 
35  OOBB(const Vector3 & l,
36  const Vector3 & u,
37  const Matrix33 & A_IK);
38 
40  OOBB(AABB3d & aabb){
41  m_minPoint = aabb.m_minPoint;
42  m_maxPoint = aabb.m_maxPoint;
43  m_q_KI.setIdentity();
44  }
45 
47  OOBB & operator=(AABB3d & aabb){
48  m_minPoint = aabb.m_minPoint;
49  m_maxPoint = aabb.m_maxPoint;
50  m_q_KI.setIdentity();
51  return *this;
52  }
53 
54  inline void setZAxisLongest(){
55  Vector3::Index i;
56  maxExtent(i);
57  if(i<2){
58  switchZAxis(static_cast<unsigned int>(i));
59  }
60  }
61 
63  void switchZAxis(unsigned int i);
64 
65  void reset();
66 
68  template<typename Derived>
69  OOBB & unite(const MatrixBase<Derived> & p){
70  m_maxPoint(0) = std::max(m_maxPoint(0),p(0));
71  m_maxPoint(1) = std::max(m_maxPoint(1),p(1));
72  m_maxPoint(2) = std::max(m_maxPoint(2),p(2));
73  m_minPoint(0) = std::min( m_minPoint(0),p(0));
74  m_minPoint(1) = std::min( m_minPoint(1),p(1));
75  m_minPoint(2) = std::min( m_minPoint(2),p(2));
76  return *this;
77  }
78 
79  inline Vector3 center(){ return 0.5*(m_maxPoint + m_minPoint);}
80 
81  inline Array3 extent() const{
82  return (m_maxPoint - m_minPoint).array();
83  }
84 
85  inline PREC maxExtent() const{
86  return (m_maxPoint - m_minPoint).maxCoeff();
87  }
88 
89  inline PREC maxExtent(Vector3::Index & i) const{
90  return (m_maxPoint - m_minPoint).maxCoeff(&i);
91  }
92 
93  inline bool isEmpty() const {
94  return m_maxPoint(0) <= m_minPoint(0) || m_maxPoint(1) <= m_minPoint(1) || m_maxPoint(2) <= m_minPoint(2);
95  }
96 
103  template<typename Derived, bool coordinateSystemIsI = true>
104  inline bool overlaps(const MatrixBase<Derived> &p) const {
105  if(coordinateSystemIsI){
106  // p is in I frame
107  Vector3 t = m_q_KI.inverse() * p; // A_IK^T * I_p
108  return ((t.array() >= m_minPoint.array()) && (t.array() <= m_maxPoint.array())).all();
109  }else{
110  // p is in K Frame!!
111  return ((p.array() >= m_minPoint.array()) && (p.array() <= m_maxPoint.array())).all();
112  }
113  }
114 
116  void expandToMinExtentRelative(PREC p = 0.1, PREC defaultExtent = 0.1, PREC eps = 1e-10);
117 
119  void expandToMinExtentAbsolute(PREC minExtent);
120 
121  inline void expand(PREC d) {
122  ApproxMVBB_ASSERTMSG(d>=0,"d>=0")
123  m_minPoint -= Vector3(d,d,d);
124  m_maxPoint += Vector3(d,d,d);
125  }
126 
127  inline void expand(Vector3 d) {
128  ApproxMVBB_ASSERTMSG(d(0)>=0 && d(1)>=0 && d(2)>=0,"d>=0")
129  m_minPoint -= d;
130  m_maxPoint += d;
131  }
132 
133  inline PREC volume() const {
134  Vector3 d = m_maxPoint- m_minPoint;
135  return d(0) * d(1) * d(2);
136  }
137 
139  inline Vector3 getDirection(unsigned int i) const{
140  ApproxMVBB_ASSERTMSG(i<3,"Index wrong: " << i)
141  Vector3 d = Vector3::Zero();
142  d(i) = 1.0;
143  return m_q_KI * d; // A_IK* d;
144  }
145 
152  template<bool coordinateSystemIsI = true>
153  inline Vector3List getCornerPoints() const{
154  Vector3List points(8);
155  Array3 ex = extent();
156  points[0] = m_minPoint /*+ Array3(0,0,0) * extent*/ ;
157  points[1] = m_minPoint + (Array3(1.0, 0.0, 0.0) * ex).matrix();
158  points[2] = m_minPoint + (Array3(0.0, 1.0, 0.0) * ex).matrix();
159  points[3] = m_minPoint + (Array3(1.0, 1.0, 0.0) * ex).matrix();
160 
161  points[4] = m_minPoint + (Array3(0.0, 0.0, 1.0) * ex).matrix();
162  points[5] = m_minPoint + (Array3(1.0, 0.0, 1.0) * ex).matrix();
163  points[6] = m_minPoint + (Array3(0.0, 1.0, 1.0) * ex).matrix();
164  points[7] = m_maxPoint /*+ Array3(1,1,1) * extent */;
165 
166  if(coordinateSystemIsI){
167  for(auto & p : points){
168  p = (m_q_KI * p).eval(); // I_p = A_IK * K_p
169  }
170  }
171 
172  return points;
173  }
174 
178 };
179 
180 }
181 
182 #endif // OOBB_hpp
#define ApproxMVBB_ASSERTMSG(condition, message)
An Assert Macro to use within C++ code.
Eigen::Quaternion< Scalar > Quaternion
PREC volume() const
Definition: OOBB.hpp:133
VectorStat< Dim > m_minPoint
Definition: AABB.hpp:254
These are some container definitions.
OOBB & operator=(AABB3d &aabb)
Definition: OOBB.hpp:47
Vector3 m_maxPoint
in K Frame
Definition: OOBB.hpp:177
StdVecAligned< Vector3 > Vector3List
Vector3 getDirection(unsigned int i) const
Definition: OOBB.hpp:139
Array3 extent() const
Definition: OOBB.hpp:81
OOBB & unite(const MatrixBase< Derived > &p)
Definition: OOBB.hpp:69
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ApproxMVBB_DEFINE_MATRIX_TYPES ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES OOBB()
Definition: OOBB.hpp:29
#define APPROXMVBB_EXPORT
Definition: Platform.hpp:60
VectorStat< Dim > m_maxPoint
Definition: AABB.hpp:255
PREC maxExtent() const
Definition: OOBB.hpp:85
OOBB(AABB3d &aabb)
Definition: OOBB.hpp:40
Eigen::Matrix< Scalar, 3, 3 > Matrix33
Quaternion m_q_KI
Rotation of frame I to frame K, corresponds to a transformation A_IK;.
Definition: OOBB.hpp:175
Vector3 m_minPoint
in K Frame
Definition: OOBB.hpp:176
Eigen::Array< Scalar, 3, 1 > Array3
#define ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES
void expand(Vector3 d)
Definition: OOBB.hpp:127
Vector3List getCornerPoints() const
Definition: OOBB.hpp:153
bool isEmpty() const
Definition: OOBB.hpp:93
bool overlaps(const MatrixBase< Derived > &p) const
Definition: OOBB.hpp:104
Eigen::Matrix< Scalar, 3, 1 > Vector3
PREC maxExtent(Vector3::Index &i) const
Definition: OOBB.hpp:89
void setZAxisLongest()
Definition: OOBB.hpp:54
#define ApproxMVBB_DEFINE_MATRIX_TYPES
Definition: TypeDefs.hpp:26
Vector3 center()
Definition: OOBB.hpp:79
void expand(PREC d)
Definition: OOBB.hpp:121


asr_approx_mvbb
Author(s): Gassner Nikolai
autogenerated on Mon Jun 10 2019 12:38:08