/// <inheritdoc/>
public PlaneDiscoveryVoxelGrid(
VoxelGridOrientation orientation, Bounds containedBounds, float voxelSize, VoxelPlaneFindingParams planeFindingParams)
: base(orientation, containedBounds, voxelSize, planeFindingParams)
{
m_PlaneAlignment = orientation == VoxelGridOrientation.Up ? MarsPlaneAlignment.HorizontalUp : orientation == VoxelGridOrientation.Down ?
MarsPlaneAlignment.HorizontalDown : MarsPlaneAlignment.Vertical;
m_DebugVoxelSize = Vector3.one * (voxelSize - k_DebugVoxelMargin * 2f);
m_DebugVoxelYOffsetSize = m_DebugVoxelSize;
switch (orientation)
{
case VoxelGridOrientation.Up:
case VoxelGridOrientation.Down:
m_DebugVoxelYOffsetSize.y = k_DebugYOffsetCubeThickness;
break;
case VoxelGridOrientation.Forward:
case VoxelGridOrientation.Back:
m_DebugVoxelYOffsetSize.z = k_DebugYOffsetCubeThickness;
break;
case VoxelGridOrientation.Right:
case VoxelGridOrientation.Left:
m_DebugVoxelYOffsetSize.x = k_DebugYOffsetCubeThickness;
break;
}
}
/// <summary>
/// Constructs an oriented voxel grid of a given size
/// </summary>
/// <param name="orientation">Determines how the grid is divided into layers from which planes can be found</param>
/// <param name="containedBounds">The grid will be just large enough to contain these bounds</param>
/// <param name="voxelSize">Side length of each voxel</param>
/// <param name="planeFindingParams">Parameters used to find planes</param>
protected PlaneVoxelGrid(
VoxelGridOrientation orientation, Bounds containedBounds, float voxelSize, VoxelPlaneFindingParams planeFindingParams)
{
var boundsSize = containedBounds.size;
m_VoxelSize = voxelSize;
m_VoxelSqSize = voxelSize * voxelSize;
m_VoxelHalfSize = voxelSize * 0.5f;
m_WorldXVoxels = Mathf.CeilToInt(boundsSize.x / voxelSize);
m_WorldYVoxels = Mathf.CeilToInt(boundsSize.y / voxelSize);
m_WorldZVoxels = Mathf.CeilToInt(boundsSize.z / voxelSize);
m_GridBoundsMin = containedBounds.min;
m_GridBoundsMax = m_GridBoundsMin + new Vector3(m_WorldXVoxels * voxelSize, m_WorldYVoxels * voxelSize, m_WorldZVoxels * voxelSize);
m_MinVoxelPosition = m_GridBoundsMin + Vector3.one * m_VoxelHalfSize;
m_Orientation = orientation;
m_DebugFilter = $"[{orientation} Plane Finding]";
if (MarsDebugSettings.SimPlaneFindingLogging)
{
Debug.Log($"{m_DebugFilter} Create grid");
}
m_MinPointsPerVoxel = (int)(planeFindingParams.minPointsPerSqMeter * m_VoxelSqSize);
m_MinSideLength = planeFindingParams.minSideLength;
m_CrossLayerMergeDistance = planeFindingParams.crossLayerMergeDistance;
var mergeDistance = planeFindingParams.inLayerMergeDistance;
m_InLayerMergeSqDistance = mergeDistance * mergeDistance;
m_CheckEmptyArea = planeFindingParams.checkEmptyArea;
m_AllowedEmptyAreaCurve = planeFindingParams.allowedEmptyAreaCurve;
switch (orientation)
{
case VoxelGridOrientation.Up:
m_Layers = m_WorldYVoxels;
m_Columns = m_WorldXVoxels;
m_Rows = m_WorldZVoxels;
m_GridOriginPose = new Pose(m_MinVoxelPosition, k_UpGridRotation);
break;
case VoxelGridOrientation.Down:
m_Layers = m_WorldYVoxels;
m_Columns = m_WorldXVoxels;
m_Rows = m_WorldZVoxels;
m_GridOriginPose = new Pose(
m_MinVoxelPosition + new Vector3(0f, voxelSize * (m_WorldYVoxels - 1), voxelSize * (m_WorldZVoxels - 1)),
k_DownGridRotation);
break;
case VoxelGridOrientation.Forward:
m_Layers = m_WorldZVoxels;
m_Columns = m_WorldXVoxels;
m_Rows = m_WorldYVoxels;
m_GridOriginPose = new Pose(
m_MinVoxelPosition + k_Up * (voxelSize * (m_WorldYVoxels - 1)),
k_ForwardGridRotation);
break;
case VoxelGridOrientation.Back:
m_Layers = m_WorldZVoxels;
m_Columns = m_WorldXVoxels;
m_Rows = m_WorldYVoxels;
m_GridOriginPose = new Pose(
m_GridBoundsMax - Vector3.one * m_VoxelHalfSize,
k_BackGridRotation);
break;
case VoxelGridOrientation.Right:
m_Layers = m_WorldXVoxels;
m_Columns = m_WorldZVoxels;
m_Rows = m_WorldYVoxels;
m_GridOriginPose = new Pose(
m_MinVoxelPosition + new Vector3(0f, voxelSize * (m_WorldYVoxels - 1), voxelSize * (m_WorldZVoxels - 1)),
k_RightGridRotation);
break;
case VoxelGridOrientation.Left:
m_Layers = m_WorldXVoxels;
m_Columns = m_WorldZVoxels;
m_Rows = m_WorldYVoxels;
m_GridOriginPose = new Pose(
m_MinVoxelPosition + new Vector3(voxelSize * (m_WorldXVoxels - 1), voxelSize * (m_WorldYVoxels - 1), 0f),
k_LeftGridRotation);
break;
}
m_LayerSize = m_Columns * m_Rows;
m_Voxels = new Dictionary <Vector3Int, PlaneVoxel>();
m_UpdatedVoxelsPerLayer = new List <HashSet <Vector2Int> >(m_Layers);
m_PlanesPerLayer = new List <List <TLayerPlane> >(m_Layers);
m_ModifiedPlanesPerLayer = new List <List <TLayerPlane> >(m_Layers);
m_RemovedPlanesPerLayer = new List <List <TLayerPlane> >(m_Layers);
for (var i = 0; i < m_Layers; ++i)
{
m_UpdatedVoxelsPerLayer.Add(new HashSet <Vector2Int>());
m_PlanesPerLayer.Add(new List <TLayerPlane>());
m_ModifiedPlanesPerLayer.Add(new List <TLayerPlane>());
m_RemovedPlanesPerLayer.Add(new List <TLayerPlane>());
}
}