/// <summary>
/// Recursive function that traverses through nodes in order to check for a ray intersection.
/// </summary>
/// <param name="ray">The ray that will be used in the intersection query.</param>
/// <param name="currentNodeSize">The node size the current recursive traversal is on.</param>
/// <param name="currentNodeCentre">The node centre the current recursive traversal is on.</param>
/// <param name="currentNodeId">The node ID the current recursive traversal is on.</param>
/// <returns></returns>
private Vector3?GetRayIntersectionRecursive(ref Ray ray, float currentNodeSize, Vector3 currentNodeCentre,
uint currentNodeId)
{
// Check if the ray intersects the current nodes bounds
Bounds bounds = new Bounds(currentNodeCentre,
new Vector3(currentNodeSize, currentNodeSize, currentNodeSize));
if (!bounds.IntersectRay(ray))
{
return(null);
}
// If the ray intersects the current node, check if the current node is occupied
OctoMapNode currentNode = _nodes[currentNodeId];
if (CheckNodeOccupied(currentNode))
{
return(currentNodeCentre);
}
// If the ray intersects the current node but the node is not occupied, check its children if it has any
if (currentNode.ChildArrayId != null)
{
for (int i = 0; i < 8; i++)
{
uint childId = _nodeChildren[currentNode.ChildArrayId.Value][i];
float newNodeSize = currentNodeSize / 2;
Vector3 newNodeCentre = GetBestFitChildNodeCentre(i, newNodeSize, currentNodeCentre);
Vector3?intersectedNodeCentre =
GetRayIntersectionRecursive(ref ray, newNodeSize, newNodeCentre, childId);
if (intersectedNodeCentre != null)
{
return(intersectedNodeCentre);
}
}
}
return(null);
}
/// <summary>
/// Recursive function that traverses through node children and writes the relationships to a compact bit stream.
/// </summary>
/// <param name="bitStream">The bitstream to write to.</param>
/// <param name="currentNodeId">The node ID the current recursive traversal is on.</param>
private void ConvertToBitStreamRecursive(BitStream bitStream, uint currentNodeId)
{
OctoMapNode currentNode = _nodes[currentNodeId];
if (currentNode.ChildArrayId != null)
{
// Build up the current nodes bit stream based on status of its children
for (int i = 0; i < 8; i++)
{
OctoMapNode childNode = _nodes[_nodeChildren[currentNode.ChildArrayId.Value][i]];
if (childNode.ChildArrayId != null) // INNER NODE
{
bitStream.WriteBit(1);
bitStream.WriteBit(1);
}
else if (CheckNodeFree(childNode)) // FREE
{
bitStream.WriteBit(1);
bitStream.WriteBit(0);
}
else if (CheckNodeOccupied(childNode)) // OCCUPIED
{
bitStream.WriteBit(0);
bitStream.WriteBit(1);
}
else // UNKNOWN
{
bitStream.WriteBit(0);
bitStream.WriteBit(0);
}
}
for (int i = 0; i < 8; i++)
{
uint childId = _nodeChildren[currentNode.ChildArrayId.Value][i];
ConvertToBitStreamRecursive(bitStream, childId);
}
}
}
/// <summary>
/// Recursively traverses though the octomap in order to find leaf nodes and then adds them to a list
/// </summary>
/// <param name="positions">The list of node of node centre positions</param>
/// <param name="sizes">The list of node sizes</param>
/// <param name="currentNodeSize">The node size the current recursive traversal is on.</param>
/// <param name="currentNodeCentre">The node centre the current recursive traversal is on.</param>
/// <param name="currentNodeId">The node ID the current recursive traversal is on.</param>
private void GatherOctoMapNodes(List <Vector3> positions, List <float> sizes, float currentNodeSize,
Vector3 currentNodeCentre,
uint currentNodeId)
{
OctoMapNode currentNode = _nodes[currentNodeId];
if (CheckNodeOccupied(currentNode))
{
positions.Add(currentNodeCentre);
sizes.Add(currentNodeSize);
}
if (currentNode.ChildArrayId != null)
{
for (int i = 0; i < 8; i++)
{
uint childId = _nodeChildren[currentNode.ChildArrayId.Value][i];
float newNodeSize = currentNodeSize / 2;
Vector3 newNodeCentre = GetBestFitChildNodeCentre(i, newNodeSize, currentNodeCentre);
GatherOctoMapNodes(positions, sizes, newNodeSize, newNodeCentre, childId);
}
}
}
/// <summary>
/// Returns true or false depending on whether a specific node is not occupied. Can be extended further.
/// </summary>
/// <param name="octoMapNode">The node that will be checked</param>
/// <returns>True or false depending on whether a specific node is not occupied.</returns>
private static bool CheckNodeFree(OctoMapNode octoMapNode)
{
return(octoMapNode.Occupied <= -1);
}
/// <summary>
/// Returns true or false depending on whether a specific node is occupied. Can be extended further.
/// </summary>
/// <param name="octoMapNode">The node that will be checked</param>
/// <returns>True or false depending on whether a specific node is occupied.</returns>
private static bool CheckNodeOccupied(OctoMapNode octoMapNode)
{
return(octoMapNode.Occupied >= 1);
}
/// <summary>
/// Sets the occupied state of a node to -1. Can be extended further.
/// </summary>
/// <param name="octoMapNode">The node to set the occupied state on</param>
/// <returns>The same node but with the occupied state changed</returns>
private static OctoMapNode DecreaseNodeOccupation(OctoMapNode octoMapNode)
{
octoMapNode.Occupied = -1;
return(octoMapNode);
}
/// <summary>
/// The recursive function that adds a new point to the OctoMap.
/// </summary>
/// <param name="point">The point to add to the OctoMap.</param>
/// <param name="currentNodeSize">The node size the current recursive traversal is on.</param>
/// <param name="currentNodeCentre">The node centre the current recursive traversal is on.</param>
/// <param name="currentNodeId">The node ID the current recursive traversal is on.</param>
private void AddPointRecursive(ref Vector3 point, float currentNodeSize, Vector3 currentNodeCentre,
uint currentNodeId)
{
OctoMapNode node = _nodes[currentNodeId];
// If we're at the deepest level possible, this current node becomes a leaf node.
if (currentNodeSize < _minimumNodeSize)
{
_nodes[currentNodeId] = IncreaseNodeOccupation(node);
return;
}
// If the node doesn't encompass this point, return out early
Bounds bounds = new Bounds(currentNodeCentre,
new Vector3(currentNodeSize, currentNodeSize, currentNodeSize));
if (!bounds.Contains(point))
{
return;
}
// If this node doesn't have any children, new children need to be generated
if (node.ChildArrayId == null)
{
node.ChildArrayId = GenerateChildren();
_nodes[currentNodeId] = node;
}
Debug.Assert(_nodes[currentNodeId].ChildArrayId != null);
// Now handle the new object we're adding now
int bestFitChild = BestFitChildIndex(point, currentNodeCentre);
uint childNodeId = _nodeChildren[node.ChildArrayId.Value][bestFitChild];
float newNodeSize = currentNodeSize / 2;
Vector3 newNodeCentre = GetBestFitChildNodeCentre(bestFitChild, newNodeSize, currentNodeCentre);
AddPointRecursive(ref point, newNodeSize, newNodeCentre, childNodeId);
// Loop through all children and check if they all share the same occupied state
bool childrenOccupancy = false;
for (int i = 0; i < 8; i++)
{
OctoMapNode child = _nodes[_nodeChildren[node.ChildArrayId.Value][i]];
if (child.ChildArrayId != null)
{
return;
}
bool occupancy = CheckNodeOccupied(child);
if (i == 0)
{
childrenOccupancy = occupancy;
}
else
{
if (occupancy != childrenOccupancy)
{
return;
}
}
}
// If the code reaches here that means all the children share the same state
// Remove all the child nodes
for (int i = 0; i < 8; i++)
{
var childArrayId = _nodes[currentNodeId].ChildArrayId;
if (childArrayId != null)
{
uint childId = _nodeChildren[childArrayId.Value][i];
_nodes.Remove(childId);
}
else
{
Debug.Log("Failed to remove node from node dictionary");
}
}
var arrayId = _nodes[currentNodeId].ChildArrayId;
if (arrayId != null)
{
// Remove the node children array
_nodeChildren.Remove(arrayId.Value);
}
else
{
Debug.Log("Failed to remove child array from child dictionary.");
}
// Set the current nodes occupancy state to that of its children
OctoMapNode currentNode = _nodes[currentNodeId];
currentNode.ChildArrayId = null;
currentNode = childrenOccupancy ? IncreaseNodeOccupation(currentNode) : DecreaseNodeOccupation(currentNode);
_nodes[currentNodeId] = currentNode;
}
