/// <summary>
/// Clears the contents of this tree node and its subtrees.
/// </summary>
public void Clear()
{
if (m_leftHashTree != null)
{
m_leftHashTree.Clear();
m_leftHashTree = null;
}
if (m_rightHashTree != null)
{
m_rightHashTree.Clear();
m_rightHashTree = null;
}
if (m_dynamicMeshCube != null)
{
m_dynamicMeshCube.Clear();
}
if (m_meshPrefab != null)
{
GameObject.DestroyImmediate(m_meshPrefab);
m_meshPrefab = null;
}
}
/// <summary>
/// Create a new tree node with a specified parent and hashkey.
/// </summary>
/// <param name="parent">The parent tree node.</param>
/// <param name="hashkey">The spatial hashkey for this tree node.</param>
public VolumetricHashTree(VolumetricHashTree parent, int hashkey)
{
if (parent == null)
{
m_rootHashTree = this;
}
else
{
m_rootHashTree = parent;
}
m_hashKey = hashkey;
}
/// <summary>
/// Create a new tree node with a specified parent and hashkey.
/// </summary>
/// <param name="parent">The parent tree node.</param>
/// <param name="hashkey">The spatial hashkey for this tree node.</param>
public VolumetricHashTree(VolumetricHashTree parent, int hashkey)
{
if (parent == null)
{
m_rootHashTree = this;
}
else
{
m_rootHashTree = parent;
}
m_hashKey = hashkey;
}
/// <summary>
/// Insert 3D point into the hash storage. Creates a new meshing cube at the location if needed.
/// </summary>
/// <returns>The value of the voxel that received the insertion.</returns>
/// <param name="hashkey">Hashkey index of the target node.</param>
/// <param name="p">The 3D point.</param>
/// <param name="obs">Observation vector from the camera to the point.</param>
/// <param name="weight">Weight of the observation.</param>
/// <param name="prefab">Unity Prefab to be instantiated at this node location if needed.</param>
/// <param name="parent">Unity transform of the parent object for the prefab created at this node location.</param>
/// <param name="voxelResolution">Resolution of the voxels for this meshing cube.</param>
private float InsertPoint(int hashkey, Vector3 p, Vector3 obs, float weight, GameObject prefab, Transform parent, int voxelResolution)
{
if (m_hashKey == hashkey)
{
if (m_meshPrefab == null)
{
InstantiatePrefab(hashkey, prefab, parent, voxelResolution);
}
if (m_meshPrefab == null)
{
m_dynamicMeshCube = m_meshPrefab.GetComponent<DynamicMeshCube>();
}
if (m_meshPrefab == null)
{
Debug.Log("Error: cannot find DynamicMeshVolume");
return 0;
}
if (m_dynamicMeshCube.IsRegenerating)
{
return 0;
}
// adjust weight of mutiple voxels along observation ray
float result = m_dynamicMeshCube.InsertPoint(p, obs, weight, ref m_volumeIndex);
Vector3 closerPoint = p - (obs * m_dynamicMeshCube.VoxelSize);
Vector3 furtherPoint = p + (obs * m_dynamicMeshCube.VoxelSize);
// voxel was inside the surface, back out one, and insert in the next closest voxel
if (result > 0)
{
m_dynamicMeshCube.InsertPoint(closerPoint, p, obs, weight);
}
else
{
m_dynamicMeshCube.InsertPoint(furtherPoint, p, obs, weight);
}
if (m_volumeIndex[0] == 0)
{
int neighborHashKey = hashkey - 1;
result = m_rootHashTree.InsertPoint(neighborHashKey, p, obs, weight, prefab, parent, voxelResolution);
}
if (m_volumeIndex[1] == 0)
{
int neighborHashKey = hashkey - m_maximumVolumeIndexDimension;
result = m_rootHashTree.InsertPoint(neighborHashKey, p, obs, weight, prefab, parent, voxelResolution);
}
if (m_volumeIndex[2] == 0)
{
int neighborHashKey = hashkey - (m_maximumVolumeIndexDimension * m_maximumVolumeIndexDimension);
result = m_rootHashTree.InsertPoint(neighborHashKey, p, obs, weight, prefab, parent, voxelResolution);
}
return result;
}
if (hashkey < m_hashKey)
{
if (m_leftHashTree == null)
{
m_leftHashTree = new VolumetricHashTree(m_rootHashTree, hashkey);
}
return m_leftHashTree.InsertPoint(hashkey, p, obs, weight, prefab, parent, voxelResolution);
}
else
{
if (m_rightHashTree == null)
{
m_rightHashTree = new VolumetricHashTree(m_rootHashTree, hashkey);
}
return m_rightHashTree.InsertPoint(hashkey, p, obs, weight, prefab, parent, voxelResolution);
}
}
/// <summary>
/// Clears the contents of this tree node and its subtrees.
/// </summary>
public void Clear()
{
if (m_leftHashTree != null)
{
m_leftHashTree.Clear();
m_leftHashTree = null;
}
if (m_rightHashTree != null)
{
m_rightHashTree.Clear();
m_rightHashTree = null;
}
if (m_dynamicMeshCube != null)
{
m_dynamicMeshCube.Clear();
}
if (m_meshPrefab != null)
{
GameObject.DestroyImmediate(m_meshPrefab);
m_meshPrefab = null;
}
}
/// <summary>
/// Insert 3D point into the hash storage. Creates a new meshing cube at the location if needed.
/// </summary>
/// <returns>The value of the voxel that received the insertion.</returns>
/// <param name="hashkey">Hashkey index of the target node.</param>
/// <param name="p">The 3D point.</param>
/// <param name="obs">Observation vector from the camera to the point.</param>
/// <param name="weight">Weight of the observation.</param>
/// <param name="prefab">Unity Prefab to be instantiated at this node location if needed.</param>
/// <param name="parent">Unity transform of the parent object for the prefab created at this node location.</param>
/// <param name="voxelResolution">Resolution of the voxels for this meshing cube.</param>
private float InsertPoint(int hashkey, Vector3 p, Vector3 obs, float weight, GameObject prefab, Transform parent, int voxelResolution)
{
if (m_hashKey == hashkey)
{
if (m_meshPrefab == null)
{
InstantiatePrefab(hashkey, prefab, parent, voxelResolution);
}
if (m_meshPrefab == null)
{
m_dynamicMeshCube = m_meshPrefab.GetComponent <DynamicMeshCube>();
}
if (m_meshPrefab == null)
{
Debug.Log("Error: cannot find DynamicMeshVolume");
return(0);
}
if (m_dynamicMeshCube.IsRegenerating)
{
return(0);
}
// adjust weight of mutiple voxels along observation ray
float result = m_dynamicMeshCube.InsertPoint(p, obs, weight, ref m_volumeIndex);
Vector3 closerPoint = p - (obs * m_dynamicMeshCube.VoxelSize);
Vector3 furtherPoint = p + (obs * m_dynamicMeshCube.VoxelSize);
// voxel was inside the surface, back out one, and insert in the next closest voxel
if (result > 0)
{
m_dynamicMeshCube.InsertPoint(closerPoint, p, obs, weight);
}
else
{
m_dynamicMeshCube.InsertPoint(furtherPoint, p, obs, weight);
}
if (m_volumeIndex[0] == 0)
{
int neighborHashKey = hashkey - 1;
result = m_rootHashTree.InsertPoint(neighborHashKey, p, obs, weight, prefab, parent, voxelResolution);
}
if (m_volumeIndex[1] == 0)
{
int neighborHashKey = hashkey - m_maximumVolumeIndexDimension;
result = m_rootHashTree.InsertPoint(neighborHashKey, p, obs, weight, prefab, parent, voxelResolution);
}
if (m_volumeIndex[2] == 0)
{
int neighborHashKey = hashkey - (m_maximumVolumeIndexDimension * m_maximumVolumeIndexDimension);
result = m_rootHashTree.InsertPoint(neighborHashKey, p, obs, weight, prefab, parent, voxelResolution);
}
return(result);
}
if (hashkey < m_hashKey)
{
if (m_leftHashTree == null)
{
m_leftHashTree = new VolumetricHashTree(m_rootHashTree, hashkey);
}
return(m_leftHashTree.InsertPoint(hashkey, p, obs, weight, prefab, parent, voxelResolution));
}
else
{
if (m_rightHashTree == null)
{
m_rightHashTree = new VolumetricHashTree(m_rootHashTree, hashkey);
}
return(m_rightHashTree.InsertPoint(hashkey, p, obs, weight, prefab, parent, voxelResolution));
}
}
/// <summary>
/// Raycast through the volume to quickly determine the list of voxels intersected.
/// </summary>
/// <returns>List of populated voxels the ray intersects.</returns>
/// <param name="start">Start point of the ray.</param>
/// <param name="stop">Stop point of the ray.</param>
public List <Voxel> RaycastVoxelHitlist(Vector3 start, Vector3 stop)
{
Vector3 dir = (stop - start).normalized;
List <int> volumeHitKeys = new List <int>();
volumeHitKeys.Add(GetHashKey(start));
// x crosses
if (dir.x > 0)
{
for (float x = Mathf.Ceil(start.x) + m_epsilon; x < stop.x; x += 1)
{
float scale = (x - start.x) / dir.x;
volumeHitKeys.Add(GetHashKey(start + (scale * dir)));
}
}
else
{
for (float x = Mathf.Floor(start.x) - m_epsilon; x > stop.x; x -= 1)
{
float scale = (x - start.x) / dir.x;
volumeHitKeys.Add(GetHashKey(start + (scale * dir)));
}
}
// y crosses
if (dir.y > 0)
{
for (float y = Mathf.Ceil(start.y) + m_epsilon; y < stop.y; y += 1)
{
float scale = (y - start.y) / dir.y;
volumeHitKeys.Add(GetHashKey(start + (scale * dir)));
}
}
else
{
for (float y = Mathf.Floor(start.y) - m_epsilon; y > stop.y; y -= 1)
{
float scale = (y - start.y) / dir.y;
volumeHitKeys.Add(GetHashKey(start + (scale * dir)));
}
}
// z crosses
if (dir.z > 0)
{
for (float z = Mathf.Ceil(start.z) + m_epsilon; z < stop.z; z += 1)
{
float scale = (z - start.z) / dir.z;
volumeHitKeys.Add(GetHashKey(start + (scale * dir)));
}
}
else
{
for (float z = Mathf.Floor(start.z) - m_epsilon; z > stop.z; z -= 1)
{
float scale = (z - start.z) / dir.z;
volumeHitKeys.Add(GetHashKey(start + (scale * dir)));
}
}
List <Voxel> voxelHits = new List <Voxel>();
foreach (int volumeKey in volumeHitKeys)
{
VolumetricHashTree result = Query(volumeKey);
if (result == null)
{
continue;
}
if (result.m_meshPrefab == null)
{
continue;
}
List <Voxel> voxels = result.m_dynamicMeshCube.RayCastVoxelHitlist(start, stop, dir);
foreach (Voxel v in voxels)
{
voxelHits.Add(v);
}
}
return(voxelHits);
}
