OctreeNode-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
17 #define SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
18 
19 #include <array>
20 #include <cmath>
21 #include <fstream>
22 
24 
25 namespace SurgSim
26 {
27 
28 namespace DataStructures
29 {
30 
31 template<class Data>
33  m_isActive(false),
34  m_hasChildren(false)
35 {
36 }
37 
38 
39 template<class Data>
41  m_boundingBox(boundingBox),
42  m_isActive(false),
43  m_hasChildren(false)
44 {
45 }
46 
47 template<class Data>
49 {
50  m_boundingBox = other.m_boundingBox;
51  m_hasChildren = other.m_hasChildren;
52  m_isActive = other.m_isActive;
53 
54  // Also copy the data since they are the same type
55  data = other.data;
56 
57  for (size_t i = 0; i < other.m_children.size(); i++)
58  {
59  if (other.getChild(i) == nullptr)
60  {
61  m_children[i] = nullptr;
62  }
63  else
64  {
65  m_children[i] = std::make_shared<OctreeNode<Data>>(*other.m_children[i]);
66  }
67  }
68 }
69 
70 template <class Data>
71 template <class T>
73 {
74  m_boundingBox = other.getBoundingBox();
75  m_hasChildren = other.hasChildren();
76  m_isActive = other.isActive();
77 
78  for (size_t i = 0; i < m_children.size(); i++)
79  {
80  auto child = other.getChild(i);
81  if (child == nullptr)
82  {
83  m_children[i] = nullptr;
84  }
85  else
86  {
87  m_children[i] = std::make_shared<OctreeNode<Data>>(*child);
88  }
89  }
90 }
91 
92 template<class Data>
94 {
95 }
96 
97 template<class Data>
99 {
100  return m_boundingBox;
101 }
102 
103 template<class Data>
105 {
106  return m_isActive;
107 }
108 
109 template<class Data>
110 void OctreeNode<Data>::setIsActive(bool isActive)
111 {
112  m_isActive = isActive;
113 }
114 
115 template<class Data>
117 {
118  return m_hasChildren;
119 }
120 
121 template<class Data>
123 {
125 
126  if (! m_hasChildren)
127  {
128  Vector3d childsSize = (m_boundingBox.max() - m_boundingBox.min()) / 2.0;
129  AxisAlignedBoundingBox childsBoundingBox;
130  for (int i = 0; i < 8; i++)
131  {
132  // Use the index to pick one of the eight regions
133  Vector3d regionIndex = Vector3d(((i & 1) == 0) ? 0 : 1,
134  ((i & 2) == 0) ? 0 : 1,
135  ((i & 4) == 0) ? 0 : 1);
136  childsBoundingBox.min() = m_boundingBox.min().array() + regionIndex.array() * childsSize.array();
137  childsBoundingBox.max() = childsBoundingBox.min() + childsSize;
138  m_children[i] = std::make_shared<OctreeNode<Data>>(childsBoundingBox);
139  }
140  m_hasChildren = true;
141  }
142 }
143 
144 template<class Data>
145 bool OctreeNode<Data>::addData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level)
146 {
147  return doAddData(position, nodeData, level, 1);
148 }
149 
150 template<class Data>
151 bool OctreeNode<Data>::doAddData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level,
152  const int currentLevel)
153 {
154  if (! m_boundingBox.contains(position))
155  {
156  return false;
157  }
158 
159  if (currentLevel == level)
160  {
161  data = nodeData;
162  m_isActive = true;
163  return true;
164  }
165 
166  if (! m_hasChildren)
167  {
168  subdivide();
169  }
170  for (auto child = m_children.begin(); child != m_children.end(); ++child)
171  {
172  if ((*child)->doAddData(position, nodeData, level, currentLevel + 1))
173  {
174  m_isActive = true;
175  return true;
176  }
177  }
178  return false;
179 }
180 
181 template<class Data>
182 std::array<std::shared_ptr<OctreeNode<Data> >, 8>& OctreeNode<Data>::getChildren()
183 {
184  return m_children;
185 }
186 
187 template<class Data>
188 const std::array<std::shared_ptr<OctreeNode<Data> >, 8>& OctreeNode<Data>::getChildren() const
189 {
190  return m_children;
191 }
192 
193 template<class Data>
194 std::shared_ptr<OctreeNode<Data> > OctreeNode<Data>::getChild(size_t index)
195 {
196  return m_children[index];
197 }
198 
199 template<class Data>
200 const std::shared_ptr<OctreeNode<Data> > OctreeNode<Data>::getChild(size_t index) const
201 {
202  return m_children[index];
203 }
204 
205 template<class Data>
206 std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getNode(const OctreePath& path, bool returnLastValid)
207 {
208  std::shared_ptr<OctreeNode<Data>> node = this->shared_from_this();
209  std::shared_ptr<OctreeNode<Data>> previous;
210  for (auto index = path.cbegin(); index != path.cend(); ++index)
211  {
212  previous = std::move(node);
213  node = previous->getChild(*index);
214  if (node == nullptr)
215  {
216  if (returnLastValid)
217  {
218  node = std::move(previous);
219  break;
220  }
221  else
222  {
223  SURGSIM_FAILURE() << "Octree path is invalid. Path is longer than octree is deep in this given branch.";
224  }
225  }
226  }
227  return node;
228 }
229 
230 template<class Data>
231 bool SurgSim::DataStructures::OctreeNode<Data>::doLoad(const std::string& filePath)
232 {
233  std::ifstream octreeData(filePath, std::ios::in);
234  SURGSIM_ASSERT(octreeData) << "Could not open file (" << filePath << ")" << std::endl;
235 
236  SurgSim::Math::Vector3d spacing, boundsMin, boundsMax;
237  std::array<int, 3> dimensions;
238  octreeData >> dimensions[0] >> dimensions[1] >> dimensions[2];
239  octreeData >> spacing[0] >> spacing[1] >> spacing[2];
240  octreeData >> boundsMin[0] >> boundsMax[0] >> boundsMin[1] >> boundsMax[1] >> boundsMin[2] >> boundsMax[2];
241 
242  int maxDimension = dimensions[0];
243  maxDimension = maxDimension >= dimensions[1] ?
244  (maxDimension >= dimensions[2] ? maxDimension : dimensions[2]) :
245  (dimensions[1] >= dimensions[2] ? dimensions[1] : dimensions[2]);
246 
247  int numLevels = static_cast<int>(std::ceil(std::log(maxDimension) / std::log(2.0)));
248  SurgSim::Math::Vector3d octreeDimensions = SurgSim::Math::Vector3d::Ones() * std::pow(2.0, numLevels);
249 
250  m_boundingBox.min() = boundsMin;
251  m_boundingBox.max() = boundsMin.array() + octreeDimensions.array() * spacing.array();
252 
253  SurgSim::Math::Vector3d position;
254  while (octreeData >> position[0] >> position[1] >> position[2])
255  {
256  addData(position, Data(), numLevels);
257  }
258 
259  return true;
260 }
261 
262 }; // namespace DataStructures
263 
264 }; // namespace SurgSim
265 
266 #endif // SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
bool addData(const SurgSim::Math::Vector3d &position, const Data &nodeData, const int level)
Add data to a node in this octree The octree will build the octree as necessary to add the node at th...
Definition: OctreeNode-inl.h:145
Definition: DriveElementFromInputBehavior.cpp:27
SurgSim::Math::Aabbd m_boundingBox
The bounding box of the current OctreeNode.
Definition: OctreeNode.h:233
virtual ~OctreeNode()
Destructor.
Definition: OctreeNode-inl.h:93
std::shared_ptr< OctreeNode< Data > > getChild(size_t index)
Get a child of this node (non const version)
Definition: OctreeNode-inl.h:194
void setIsActive(bool isActive)
Set active flag for this octree node.
Definition: OctreeNode-inl.h:110
virtual bool doLoad(const std::string &filePath) override
Derived classes will overwrite this method to do actual loading.
Definition: OctreeNode-inl.h:231
const SurgSim::Math::Aabbd & getBoundingBox() const
Get the bounding box for this octree node.
Definition: OctreeNode-inl.h:98
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
#define SURGSIM_FAILURE()
Report that something very bad has happened and abort program execution.
Definition: Assert.h:95
std::vector< size_t > OctreePath
Typedef of octree path The path is a vector of children indexes (each within 0 to 7) that lead to the...
Definition: OctreeNode.h:43
virtual std::shared_ptr< OctreeNode< Data > > getNode(const OctreePath &path, bool returnLastValid=false)
Get the node at the supplied path.
Definition: OctreeNode-inl.h:206
Eigen::AlignedBox< double, 3 > Aabbd
Wrapper around the Eigen type.
Definition: Aabb.h:30
bool isActive() const
Is this node active.
Definition: OctreeNode-inl.h:104
OctreeNode()
Constructor.
Definition: OctreeNode-inl.h:32
The header that provides the assertion API.
bool doAddData(const SurgSim::Math::Vector3d &position, const Data &nodeData, const int level, const int currentLevel)
Recursive function that does the adding of the data to the octree.
Definition: OctreeNode-inl.h:151
bool m_hasChildren
True if the node has children.
Definition: OctreeNode.h:239
bool m_isActive
True if there is any data inside this node, including data held by children, children's children...
Definition: OctreeNode.h:236
std::array< std::shared_ptr< OctreeNode< Data > >, 8 > & getChildren()
Get the children of this node (non const version)
Definition: OctreeNode-inl.h:182
Eigen::AlignedBox< double, 3 > AxisAlignedBoundingBox
Bounding box type for convenience.
Definition: OctreeNode.h:136
void subdivide()
Subdivide the node into 8 equal regions.
Definition: OctreeNode-inl.h:122
std::array< std::shared_ptr< OctreeNode< Data > >, 8 > m_children
The children of this node.
Definition: OctreeNode.h:242
Data data
Extra node data.
Definition: OctreeNode.h:218
bool hasChildren() const
Does this node have children.
Definition: OctreeNode-inl.h:116
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:56
Octree data structure.
Definition: OctreeNode.h:128