OctreeNode-inl.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2012-2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
#define SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
#include <array>
#include <cmath>
#include <fstream>
#include "SurgSim/DataStructures/OctreeNodePlyReaderDelegate.h"
#include "SurgSim/DataStructures/PlyReader.h"
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/Timer.h"
namespace SurgSim
{
namespace DataStructures
{
template<class Data>
OctreeNode<Data>::OctreeNode() :
m_isActive(false),
m_hasChildren(false)
{
}
template<class Data>
OctreeNode<Data>::OctreeNode(const SurgSim::Math::Aabbd& boundingBox) :
m_boundingBox(boundingBox),
m_isActive(false),
m_hasChildren(false)
{
}
template<class Data>
SurgSim::DataStructures::OctreeNode<Data>::OctreeNode(const OctreeNode& other)
{
m_boundingBox = other.m_boundingBox;
m_hasChildren = other.m_hasChildren;
m_isActive = other.m_isActive;
// Also copy the data since they are the same type
data = other.data;
for (size_t i = 0; i < other.m_children.size(); i++)
{
if (other.getChild(i) == nullptr)
{
m_children[i] = nullptr;
}
else
{
m_children[i] = std::make_shared<OctreeNode<Data>>(*other.m_children[i]);
}
}
}
template <class Data>
template <class T>
SurgSim::DataStructures::OctreeNode<Data>::OctreeNode(const OctreeNode<T>& other)
{
m_boundingBox = other.getBoundingBox();
m_hasChildren = other.hasChildren();
m_isActive = other.isActive();
for (size_t i = 0; i < m_children.size(); i++)
{
auto child = other.getChild(i);
if (child == nullptr)
{
m_children[i] = nullptr;
}
else
{
m_children[i] = std::make_shared<OctreeNode<Data>>(*child);
}
}
}
template<class Data>
OctreeNode<Data>::~OctreeNode()
{
}
template<class Data>
std::string OctreeNode<Data>::getClassName() const
{
return m_className;
}
template<class Data>
const SurgSim::Math::Aabbd& OctreeNode<Data>::getBoundingBox() const
{
return m_boundingBox;
}
template<class Data>
bool OctreeNode<Data>::isActive() const
{
return m_isActive;
}
template<class Data>
void OctreeNode<Data>::setIsActive(bool isActive)
{
m_isActive = isActive;
}
template<class Data>
bool OctreeNode<Data>::hasChildren() const
{
return m_hasChildren;
}
template<class Data>
void OctreeNode<Data>::subdivide()
{
using SurgSim::Math::Vector3d;
if (! m_hasChildren)
{
Vector3d childsSize = (m_boundingBox.max() - m_boundingBox.min()) / 2.0;
AxisAlignedBoundingBox childsBoundingBox;
for (int i = 0; i < 8; i++)
{
// Use the index to pick one of the eight regions
Vector3d regionIndex = Vector3d(((i & 1) == 0) ? 0 : 1,
((i & 2) == 0) ? 0 : 1,
((i & 4) == 0) ? 0 : 1);
childsBoundingBox.min() = m_boundingBox.min().array() + regionIndex.array() * childsSize.array();
childsBoundingBox.max() = childsBoundingBox.min() + childsSize;
m_children[i] = std::make_shared<OctreeNode<Data>>(childsBoundingBox);
}
m_hasChildren = true;
}
}
template<class Data>
bool OctreeNode<Data>::addData(const SurgSim::Math::Vector3d& position, const int level, const Data& nodeData)
{
return doAddData(position, nodeData, level, 1);
}
template<class Data>
bool OctreeNode<Data>::doAddData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level,
const int currentLevel)
{
if (! m_boundingBox.contains(position))
{
return false;
}
if (currentLevel == level)
{
data = nodeData;
m_isActive = true;
return true;
}
if (! m_hasChildren)
{
subdivide();
}
for (auto child = m_children.begin(); child != m_children.end(); ++child)
{
if ((*child)->doAddData(position, nodeData, level, currentLevel + 1))
{
m_isActive = true;
return true;
}
}
return false;
}
template<class Data>
std::array<std::shared_ptr<OctreeNode<Data>>, 8>& OctreeNode<Data>::getChildren()
{
return m_children;
}
template<class Data>
const std::array<std::shared_ptr<OctreeNode<Data>>, 8>& OctreeNode<Data>::getChildren() const
{
return m_children;
}
template<class Data>
std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getChild(size_t index)
{
return m_children[index];
}
template<class Data>
const std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getChild(size_t index) const
{
return m_children[index];
}
template<class Data>
std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getNode(const OctreePath& path, bool returnLastValid)
{
std::shared_ptr<OctreeNode<Data>> node = this->shared_from_this();
std::shared_ptr<OctreeNode<Data>> previous;
for (auto index = path.cbegin(); index != path.cend(); ++index)
{
previous = std::move(node);
node = previous->getChild(*index);
if (node == nullptr)
{
if (returnLastValid)
{
node = std::move(previous);
break;
}
else
{
SURGSIM_FAILURE() << "Octree path is invalid. Path is longer than octree is deep in this given branch.";
}
}
}
return node;
}
template<class Data>
bool SurgSim::DataStructures::OctreeNode<Data>::doLoad(const std::string& fileName)
{
SurgSim::Framework::Timer timer;
auto delegate = std::make_shared<OctreeNodePlyReaderDelegate<Data>>(this->shared_from_this());
PlyReader reader(fileName);
SURGSIM_ASSERT(reader.isValid()) << "'" << fileName << "' is an invalid .ply file.";
SURGSIM_ASSERT(reader.parseWithDelegate(delegate)) <<
"The input file " << fileName << " does not have the property required by the octree.";
timer.endFrame();
SURGSIM_LOG_INFO(SurgSim::Framework::Logger::getDefaultLogger())
<< "Loading " << fileName << " took " << timer.getCumulativeTime() << "s. ";
return true;
}
}; // namespace DataStructures
}; // namespace SurgSim
#endif // SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
SurgSim::DataStructures::OctreeNode::isActive
bool isActive() const
Is this node active.
Definition: OctreeNode-inl.h:114
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::DataStructures::OctreeNode::m_boundingBox
SurgSim::Math::Aabbd m_boundingBox
The bounding box of the current OctreeNode.
Definition: OctreeNode.h:239
SurgSim::DataStructures::OctreeNode::~OctreeNode
virtual ~OctreeNode()
Destructor.
Definition: OctreeNode-inl.h:97
OctreeNodePlyReaderDelegate.h
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
SurgSim::DataStructures::OctreeNode::setIsActive
void setIsActive(bool isActive)
Set active flag for this octree node.
Definition: OctreeNode-inl.h:120
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::Framework::Timer::getCumulativeTime
double getCumulativeTime() const
Return the sum of the durations over all the stored frames.
Definition: Timer.cpp:66
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SURGSIM_FAILURE
#define SURGSIM_FAILURE()
Report that something very bad has happened and abort program execution.
Definition: Assert.h:95
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.
Timer.h
SurgSim::Framework::Timer
Timer class, measures execution times.
Definition: Timer.h:30
SurgSim::DataStructures::PlyReader
Wrapper for the C .ply file parser This class wraps the main functionality for the original C ...
Definition: PlyReader.h:85
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::Math::Aabbd
Eigen::AlignedBox< double, 3 > Aabbd
Wrapper around the Eigen type.
Definition: Aabb.h:30
SurgSim::DataStructures::OctreeNode::OctreeNode
OctreeNode()
Constructor.
Definition: OctreeNode-inl.h:36
SurgSim::DataStructures::OctreeNode::hasChildren
bool hasChildren() const
Does this node have children.
Definition: OctreeNode-inl.h:126
SurgSim::Framework::Timer::endFrame
void endFrame()
End this frame by storing the duration since the current frame was begun.
Definition: Timer.cpp:45
Assert.h
The header that provides the assertion API.
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::m_hasChildren
bool m_hasChildren
True if the node has children.
Definition: OctreeNode.h:245
SurgSim::DataStructures::OctreeNode::m_isActive
bool m_isActive
True if there is any data inside this node, including data held by children, children&#39;s children...
Definition: OctreeNode.h:242
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
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::AxisAlignedBoundingBox
Eigen::AlignedBox< double, 3 > AxisAlignedBoundingBox
Bounding box type for convenience.
Definition: OctreeNode.h:140
PlyReader.h
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::Framework::Logger::getDefaultLogger
static std::shared_ptr< Logger > getDefaultLogger()
Get default logger.
Definition: Logger.h:116
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::data
Data data
Extra node data.
Definition: OctreeNode.h:224
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::DataStructures::OctreeNode
Octree data structure.
Definition: OctreeNode.h:131
SurgSim::DataStructures::OctreeNode::getClassName
std::string getClassName() const override
Support serialization with a classname.
Definition: OctreeNode-inl.h:102
SurgSim::DataStructures::OctreeNode::m_className
std::string m_className
Definition: OctreeNode.cpp:153
SurgSim::DataStructures::OctreeNode::getBoundingBox
const SurgSim::Math::Aabbd & getBoundingBox() const
Get the bounding box for this octree node.
Definition: OctreeNode-inl.h:108