void MakeGrid() { // create grid with voids var colliders = _voids .GetComponentsInChildren <MeshCollider>() .ToArray(); var voxelSize = float.Parse(_voxelSize); _grid = Grid3d.MakeGridWithVoids(colliders, voxelSize); // select edges of boundary faces var edges = _grid.GetEdges().Where(e => e.ClimbableFaces.Length == 2); // create graph from edges var graphEdges = edges.Select(e => new TaggedEdge <Face, Edge>(e.ClimbableFaces[0], e.ClimbableFaces[1], e)); var graph = graphEdges.ToUndirectedGraph <Face, TaggedEdge <Face, Edge> >(); // start face for shortest path var start = _grid.GetFaces().Where(f => f.IsSkin).Skip(10).First(); // calculate shortest path from start face to all boundary faces var shortest = graph.ShortestPathsDijkstra(e => 1.0, start); // select an end face to draw one specific path var end = _grid.GetFaces().Where(f => f.IsSkin).Skip(200).First(); shortest(end, out var endPath); // unsorted distinct faces of the path from start to end faces var endPathFaces = new HashSet <Face>(endPath.SelectMany(e => new[] { e.Source, e.Target })); // create a mesh face for every outer face colored based on the path length (except a solid yellow path to end face) var faceMeshes = new List <CombineInstance>(); foreach (var face in _grid.GetFaces().Where(f => f.IsSkin)) { float t = 1; if (shortest(face, out var path)) { t = path.Count() * 0.04f; t = Mathf.Clamp01(t); } Mesh faceMesh; // paint face yellow if its part of the start-end path, gradient color for other faces if (endPathFaces.Contains(face)) { faceMesh = Drawing.MakeFace(face.Center, face.Direction, _grid.VoxelSize, 0, 1); } else { faceMesh = Drawing.MakeFace(face.Center, face.Direction, _grid.VoxelSize, t); } faceMeshes.Add(new CombineInstance() { mesh = faceMesh }); } var mesh = new Mesh() { indexFormat = UnityEngine.Rendering.IndexFormat.UInt32 }; mesh.CombineMeshes(faceMeshes.ToArray(), true, false, false); Mesh pathMesh = new Mesh(); // draw a polyline for the start-end path { if (shortest(end, out var path)) { float offset = 0.1f; var vertices = new List <Vector3>(); var current = start; vertices.Add(current.Center + current.Normal * offset); foreach (var edge in path) { vertices.Add(edge.Tag.Center + edge.Tag.Normal * offset); current = edge.GetOtherVertex(current); vertices.Add(current.Center + current.Normal * offset); } pathMesh.SetVertices(vertices); pathMesh.subMeshCount = 2; pathMesh.SetIndices(Enumerable.Range(0, vertices.Count).ToArray(), MeshTopology.LineStrip, 1); } } _meshes = new[] { mesh, pathMesh }; }
public IEnumerator MoveToRaycastVoxel() { allPaths.Clear(); foreach (var voxel in previewVoxels) { var linkFaces = voxel.Faces.Where(f => f.IsClimbable && f.Normal != Vector3.left); foreach (var end in linkFaces) { Vector3 faceOffset = new Vector3(); if (end.Normal == Vector3.up) { faceOffset = new Vector3(0, halfSize.y * 2, -halfSize.y * 2); } else if (end.Normal == Vector3.down) { faceOffset = new Vector3(0, -halfSize.y * 2, halfSize.y * 2); } else if (end.Normal == Vector3.forward) { faceOffset = new Vector3(0, halfSize.y * 2, halfSize.y * 2); } else if (end.Normal == Vector3.back) { faceOffset = new Vector3(0, -halfSize.y * 2, -halfSize.y * 2); } else if (end.Normal == Vector3.right) { faceOffset = new Vector3(halfSize.y * 2, 0, 0); } else { throw new Exception("normal not found"); } if (startFaces.All(f => f.IsUsed)) { foreach (var sf in startFaces) { sf.IsUsed = false; } } var start = startFaces.OrderBy(sf => Vector3.Distance(sf.Center, end.Center)).First(f => !f.IsUsed); var stack = _stackList.First(st => st.transform.position == start.Center); var tileStackList = stack.GetComponent <CreateStack>().tileList; foreach (Transform tile in stack.transform) { if (tile.gameObject.name == "robot1") { tile.GetComponent <Renderer>().enabled = false; } } Destroy(tileStackList.First()); tileStackList.Remove(tileStackList.First()); StartCoroutine(MoveRobot(start, end, graphFinal, true, 0, faceOffset)); } MakeGraph(); } yield return(new WaitForSeconds(tumblingTime * 30)); foreach (var voxel in previewVoxels) { voxel.IsActive = true; // Shortest Path to each voxel var edges = _grid.GetEdges().Where(e => e.ClimbableFaces.Length == 2); var graphEdges = edges.Select(e => new TaggedEdge <Face, Edge>(e.ClimbableFaces[0], e.ClimbableFaces[1], e)); var graph = graphEdges.ToUndirectedGraph <Face, TaggedEdge <Face, Edge> >(); var endFaces = voxel.Faces.Where(f => f.IsClimbable).ToList(); foreach (var end in endFaces) { if (startFaces.All(f => f.IsUsed)) { foreach (var sf in startFaces) { sf.IsUsed = false; } } var start = startFaces.OrderBy(sf => Vector3.Distance(sf.Center, end.Center)).First(f => !f.IsUsed); var stack = _stackList.First(st => st.transform.position == start.Center); var tileStackList = stack.GetComponent <CreateStack>().tileList; stack.GetComponent <CreateStack>().robot1.GetComponent <Renderer>().enabled = false; Destroy(tileStackList.First()); tileStackList.Remove(tileStackList.First()); StartCoroutine(MoveRobot(start, end, graph, true, 0, Vector3.zero)); } } yield return(new WaitForSeconds(tumblingTime * 10)); previewVoxels.Clear(); }