Verified Commit 304e44bf authored by Sebastian Höffner's avatar Sebastian Höffner
Browse files

Transforming boolean occupancy grid into string grid, containing the objects...

Transforming boolean occupancy grid into string grid, containing the objects occupying the respective cells
parent 9aab4651
......@@ -8,7 +8,7 @@ namespace dlu
[CustomEditor(typeof(OccupancyGrid))]
public class OccupancyGridEditor : Editor
{
private bool[,,] occupancy;
private List<string>[,,] occupancy;
// This seems not to work? Maybe needs to serialize on MonoBehaviour?
[SerializeField]
......
......@@ -11,13 +11,13 @@ public class OccupancyGrid : MonoBehaviour
public Vector3 boundsMax = new Vector3(1.55f, 1.35f, 1.55f);
public Vector3 resolution = new Vector3(0.05f, 0.05f, 0.05f);
private bool[,,] occupancy;
private List<string>[,,] occupancy;
public bool[,,] GenerateOccupancyGrid() {
public List<string>[,,] GenerateOccupancyGrid() {
return GenerateOccupancyGrid(new HashSet<string>());
}
public bool[,,] GenerateOccupancyGrid(HashSet<string> excluded)
public List<string>[,,] GenerateOccupancyGrid(HashSet<string> excluded)
{
Vector3 halfResolution = resolution * 0.5f;
Vector3 inverseResolution = new Vector3(1 / resolution.x, 1 / resolution.y, 1 / resolution.z);
......@@ -30,7 +30,7 @@ public class OccupancyGrid : MonoBehaviour
.ToList();
ignored.ForEach(go => go.SetActive(false));
occupancy = new bool[gridSize.x, gridSize.y, gridSize.z];
occupancy = new List<string>[gridSize.x, gridSize.y, gridSize.z];
for (long ix = 0; ix < occupancy.GetLongLength(0); ++ix)
{
......@@ -39,7 +39,21 @@ public class OccupancyGrid : MonoBehaviour
for (long iz = 0; iz < occupancy.GetLongLength(2); ++iz)
{
Vector3 gridCell = idx2coord(ix, iy, iz, resolution);
occupancy[ix, iy, iz] = Physics.CheckBox(gridCell, halfResolution, Quaternion.identity, Physics.DefaultRaycastLayers, QueryTriggerInteraction.Ignore);
Collider[] hitColliders = Physics.OverlapBox(gridCell, halfResolution, Quaternion.identity, Physics.DefaultRaycastLayers, QueryTriggerInteraction.Ignore);
occupancy[ix, iy, iz] = new List<string>(hitColliders.Length);
foreach (Collider c in hitColliders)
{
InstanceIdentifier ii = c.gameObject.GetComponent<InstanceIdentifier>();
if (ii == null)
{
occupancy[ix, iy, iz].Add("Unknown");
}
else
{
occupancy[ix, iy, iz].Add(ii.name);
}
}
}
}
}
......@@ -68,7 +82,7 @@ public class OccupancyGrid : MonoBehaviour
{
for (long iz = 0; iz < occupancy.GetLongLength(2); ++iz)
{
Gizmos.color = occupancy[ix, iy, iz] ? Color.red : Color.green;
Gizmos.color = occupancy[ix, iy, iz].Count > 0 ? Color.red : Color.green;
Vector3 gridCell = idx2coord(ix, iy, iz, resolution);
Gizmos.DrawWireCube(gridCell, resolution);
// Gizmos.DrawCube(gridCell, resolution * 0.8f);
......@@ -90,7 +104,7 @@ public class OccupancyGrid : MonoBehaviour
{
for (long iz = 0; iz < occupancy.GetLongLength(2); ++iz)
{
if (occupancy[ix, iy, iz])
if (occupancy[ix, iy, iz].Count > 0)
{
Vector3 gridCell = idx2coord(ix, iy, iz, resolution);
Gizmos.DrawWireCube(gridCell, halfResolution);
......
......@@ -93,7 +93,7 @@ public class SceneController : MonoBehaviour
}
}
occupancyGrid.resolution = Vector3.one * resolution;
bool[,,] occupancy = occupancyGrid.GenerateOccupancyGrid(excluded);
List<string>[,,] occupancy = occupancyGrid.GenerateOccupancyGrid(excluded);
JSONObject jsonObject = new JSONObject();
jsonObject["max_index_x"] = occupancy.GetLongLength(0);
......@@ -106,12 +106,18 @@ public class SceneController : MonoBehaviour
{
for (long iz = 0; iz < occupancy.GetLongLength(2); ++iz)
{
if (occupancy[ix, iy, iz])
if (occupancy[ix, iy, iz].Count > 0)
{
JSONObject coord = new JSONObject();
coord["x"] = ix;
coord["y"] = iy;
coord["z"] = iz;
JSONArray objects = new JSONArray();
foreach(string name in occupancy[ix, iy, iz])
{
objects.Add(name);
}
coord["objects"] = objects;
grid.Add(coord);
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment