16 #ifndef SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H 17 #define SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H 28 namespace DataStructures
57 for (
size_t i = 0; i < other.
m_children.size(); i++)
87 m_children[i] = std::make_shared<OctreeNode<Data>>(*child);
130 for (
int i = 0; i < 8; i++)
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);
147 return doAddData(position, nodeData, level, 1);
152 const int currentLevel)
159 if (currentLevel == level)
172 if ((*child)->doAddData(position, nodeData, level, currentLevel + 1))
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)
212 previous = std::move(node);
213 node = previous->getChild(*index);
218 node = std::move(previous);
223 SURGSIM_FAILURE() <<
"Octree path is invalid. Path is longer than octree is deep in this given branch.";
233 std::ifstream octreeData(filePath, std::ios::in);
234 SURGSIM_ASSERT(octreeData) <<
"Could not open file (" << filePath <<
")" << std::endl;
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];
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]);
247 int numLevels =
static_cast<int>(std::ceil(std::log(maxDimension) / std::log(2.0)));
251 m_boundingBox.max() = boundsMin.array() + octreeDimensions.array() * spacing.array();
254 while (octreeData >> position[0] >> position[1] >> position[2])
256 addData(position, Data(), numLevels);
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
string(TOUPPER ${DEVICE}DEVICE_UPPER_CASE) option(BUILD_DEVICE_ $
Definition: CMakeLists.txt:35
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