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 
26 #include "SurgSim/Framework/Log.h"
28 
29 namespace SurgSim
30 {
31 
32 namespace DataStructures
33 {
34 
35 template<class Data>
37  m_isActive(false),
38  m_hasChildren(false)
39 {
40 }
41 
42 
43 template<class Data>
45  m_boundingBox(boundingBox),
46  m_isActive(false),
47  m_hasChildren(false)
48 {
49 }
50 
51 template<class Data>
53 {
54  m_boundingBox = other.m_boundingBox;
55  m_hasChildren = other.m_hasChildren;
56  m_isActive = other.m_isActive;
57 
58  // Also copy the data since they are the same type
59  data = other.data;
60 
61  for (size_t i = 0; i < other.m_children.size(); i++)
62  {
63  if (other.getChild(i) == nullptr)
64  {
65  m_children[i] = nullptr;
66  }
67  else
68  {
69  m_children[i] = std::make_shared<OctreeNode<Data>>(*other.m_children[i]);
70  }
71  }
72 }
73 
74 template <class Data>
75 template <class T>
77 {
78  m_boundingBox = other.getBoundingBox();
79  m_hasChildren = other.hasChildren();
80  m_isActive = other.isActive();
81 
82  for (size_t i = 0; i < m_children.size(); i++)
83  {
84  auto child = other.getChild(i);
85  if (child == nullptr)
86  {
87  m_children[i] = nullptr;
88  }
89  else
90  {
91  m_children[i] = std::make_shared<OctreeNode<Data>>(*child);
92  }
93  }
94 }
95 
96 template<class Data>
98 {
99 }
100 
101 template<class Data>
103 {
104  return m_className;
105 }
106 
107 template<class Data>
109 {
110  return m_boundingBox;
111 }
112 
113 template<class Data>
115 {
116  return m_isActive;
117 }
118 
119 template<class Data>
120 void OctreeNode<Data>::setIsActive(bool isActive)
121 {
122  m_isActive = isActive;
123 }
124 
125 template<class Data>
127 {
128  return m_hasChildren;
129 }
130 
131 template<class Data>
133 {
135 
136  if (! m_hasChildren)
137  {
138  Vector3d childsSize = (m_boundingBox.max() - m_boundingBox.min()) / 2.0;
139  AxisAlignedBoundingBox childsBoundingBox;
140  for (int i = 0; i < 8; i++)
141  {
142  // Use the index to pick one of the eight regions
143  Vector3d regionIndex = Vector3d(((i & 1) == 0) ? 0 : 1,
144  ((i & 2) == 0) ? 0 : 1,
145  ((i & 4) == 0) ? 0 : 1);
146  childsBoundingBox.min() = m_boundingBox.min().array() + regionIndex.array() * childsSize.array();
147  childsBoundingBox.max() = childsBoundingBox.min() + childsSize;
148  m_children[i] = std::make_shared<OctreeNode<Data>>(childsBoundingBox);
149  }
150  m_hasChildren = true;
151  }
152 }
153 
154 template<class Data>
155 bool OctreeNode<Data>::addData(const SurgSim::Math::Vector3d& position, const int level, const Data& nodeData)
156 {
157  return doAddData(position, nodeData, level, 1);
158 }
159 
160 template<class Data>
161 bool OctreeNode<Data>::doAddData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level,
162  const int currentLevel)
163 {
164  if (! m_boundingBox.contains(position))
165  {
166  return false;
167  }
168 
169  if (currentLevel == level)
170  {
171  data = nodeData;
172  m_isActive = true;
173  return true;
174  }
175 
176  if (! m_hasChildren)
177  {
178  subdivide();
179  }
180  for (auto child = m_children.begin(); child != m_children.end(); ++child)
181  {
182  if ((*child)->doAddData(position, nodeData, level, currentLevel + 1))
183  {
184  m_isActive = true;
185  return true;
186  }
187  }
188  return false;
189 }
190 
191 template<class Data>
192 std::array<std::shared_ptr<OctreeNode<Data>>, 8>& OctreeNode<Data>::getChildren()
193 {
194  return m_children;
195 }
196 
197 template<class Data>
198 const std::array<std::shared_ptr<OctreeNode<Data>>, 8>& OctreeNode<Data>::getChildren() const
199 {
200  return m_children;
201 }
202 
203 template<class Data>
204 std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getChild(size_t index)
205 {
206  return m_children[index];
207 }
208 
209 template<class Data>
210 const std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getChild(size_t index) const
211 {
212  return m_children[index];
213 }
214 
215 template<class Data>
216 std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getNode(const OctreePath& path, bool returnLastValid)
217 {
218  std::shared_ptr<OctreeNode<Data>> node = this->shared_from_this();
219  std::shared_ptr<OctreeNode<Data>> previous;
220  for (auto index = path.cbegin(); index != path.cend(); ++index)
221  {
222  previous = std::move(node);
223  node = previous->getChild(*index);
224  if (node == nullptr)
225  {
226  if (returnLastValid)
227  {
228  node = std::move(previous);
229  break;
230  }
231  else
232  {
233  SURGSIM_FAILURE() << "Octree path is invalid. Path is longer than octree is deep in this given branch.";
234  }
235  }
236  }
237  return node;
238 }
239 
240 template<class Data>
242 {
244  auto delegate = std::make_shared<OctreeNodePlyReaderDelegate<Data>>(this->shared_from_this());
245 
246  PlyReader reader(fileName);
247  SURGSIM_ASSERT(reader.isValid()) << "'" << fileName << "' is an invalid .ply file.";
248  SURGSIM_ASSERT(reader.parseWithDelegate(delegate)) <<
249  "The input file " << fileName << " does not have the property required by the octree.";
250  timer.endFrame();
252  << "Loading " << fileName << " took " << timer.getCumulativeTime() << "s. ";
253  return true;
254 }
255 
256 }; // namespace DataStructures
257 
258 }; // namespace SurgSim
259 
260 #endif // SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
OctreeNodePlyReaderDelegate.h
SurgSim::DataStructures::OctreeNode::m_children
std::array< std::shared_ptr< OctreeNode< Data > >, 8 > m_children
The children of this node.
Definition: OctreeNode.h:248
SurgSim::DataStructures::OctreeNode::getNode
virtual std::shared_ptr< OctreeNode< Data > > getNode(const OctreePath &path, bool returnLastValid=false)
Get the node at the supplied path.
Definition: OctreeNode-inl.h:216
SurgSim::Framework::Timer
Timer class, measures execution times.
Definition: Timer.h:31
SurgSim::DataStructures::OctreeNode::~OctreeNode
virtual ~OctreeNode()
Destructor.
Definition: OctreeNode-inl.h:97
SURGSIM_LOG_INFO
#define SURGSIM_LOG_INFO(logger)
Logs a message to the specified logger at the INFO level.
Definition: LogMacros.h:86
SurgSim::DataStructures::OctreeNode::doLoad
bool doLoad(const std::string &filePath) override
Derived classes will overwrite this method to do actual loading.
Definition: OctreeNode-inl.h:241
SurgSim::DataStructures::OctreeNode::getChildren
std::array< std::shared_ptr< OctreeNode< Data > >, 8 > & getChildren()
Get the children of this node (non const version)
Definition: OctreeNode-inl.h:192
PlyReader.h
Assert.h
The header that provides the assertion API.
SurgSim::DataStructures::OctreeNode::OctreeNode
OctreeNode()
Constructor.
Definition: OctreeNode-inl.h:36
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SURGSIM_FAILURE
#define SURGSIM_FAILURE()
Report that something very bad has happened and abort program execution.
Definition: Assert.h:95
SurgSim::DataStructures::OctreeNode::isActive
bool isActive() const
Is this node active.
Definition: OctreeNode-inl.h:114
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::DataStructures::OctreePath
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
Log.h
The convenience header that provides the entirety of the logging API.
SurgSim::DataStructures::OctreeNode::getClassName
std::string getClassName() const override
Support serialization with a classname.
Definition: OctreeNode-inl.h:102
SurgSim::Framework::Logger::getDefaultLogger
static std::shared_ptr< Logger > getDefaultLogger()
Get default logger.
Definition: Logger.h:116
SurgSim::DataStructures::OctreeNode::m_boundingBox
SurgSim::Math::Aabbd m_boundingBox
The bounding box of the current OctreeNode.
Definition: OctreeNode.h:239
SurgSim::Math::Aabbd
Eigen::AlignedBox< double, 3 > Aabbd
Wrapper around the Eigen type.
Definition: Aabb.h:30
SurgSim::DataStructures::OctreeNode::getChild
std::shared_ptr< OctreeNode< Data > > getChild(size_t index)
Get a child of this node (non const version)
Definition: OctreeNode-inl.h:204
Timer.h
SurgSim::Framework::Timer::endFrame
void endFrame()
End this frame by storing the duration since the current frame was begun.
Definition: Timer.cpp:45
SurgSim::Framework::Timer::getCumulativeTime
double getCumulativeTime() const
Return the sum of the durations over all the stored frames.
Definition: Timer.cpp:66
SurgSim::DataStructures::PlyReader
Wrapper for the C .ply file parser This class wraps the main functionality for the original C ....
Definition: PlyReader.h:86
SurgSim::DataStructures::OctreeNode::addData
bool addData(const SurgSim::Math::Vector3d &position, const int level, const Data &nodeData=Data())
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:155
SurgSim::DataStructures::OctreeNode::subdivide
void subdivide()
Subdivide the node into 8 equal regions.
Definition: OctreeNode-inl.h:132
SurgSim::DataStructures::OctreeNode
Octree data structure.
Definition: OctreeNode.h:133
SurgSim::DataStructures::OctreeNode::hasChildren
bool hasChildren() const
Does this node have children.
Definition: OctreeNode-inl.h:126
SurgSim::DataStructures::OctreeNode::m_hasChildren
bool m_hasChildren
True if the node has children.
Definition: OctreeNode.h:245
SurgSim::DataStructures::OctreeNode::doAddData
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:161
SurgSim::DataStructures::OctreeNode::AxisAlignedBoundingBox
Eigen::AlignedBox< double, 3 > AxisAlignedBoundingBox
Bounding box type for convenience.
Definition: OctreeNode.h:140
SurgSim::DataStructures::OctreeNode::setIsActive
void setIsActive(bool isActive)
Set active flag for this octree node.
Definition: OctreeNode-inl.h:120
SurgSim::DataStructures::OctreeNode::m_isActive
bool m_isActive
True if there is any data inside this node, including data held by children, children's children,...
Definition: OctreeNode.h:242
string
string(TOUPPER ${DEVICE} DEVICE_UPPER_CASE) option(BUILD_DEVICE_$
Definition: CMakeLists.txt:38
SurgSim::DataStructures::OctreeNode::data
Data data
Extra node data.
Definition: OctreeNode.h:224
SurgSim::DataStructures::OctreeNode::getBoundingBox
const SurgSim::Math::Aabbd & getBoundingBox() const
Get the bounding box for this octree node.
Definition: OctreeNode-inl.h:108