136 lines
4.8 KiB
C#
136 lines
4.8 KiB
C#
using System;
|
|
using UnityEngine;
|
|
|
|
namespace Obi
|
|
{
|
|
public class VoxelPathFinder
|
|
{
|
|
private MeshVoxelizer voxelizer = null;
|
|
private bool[,,] closed;
|
|
private PriorityQueue<TargetVoxel> open;
|
|
|
|
public struct TargetVoxel : IEquatable<TargetVoxel>, IComparable<TargetVoxel>
|
|
{
|
|
public Vector3Int coordinates;
|
|
public float distance;
|
|
public float heuristic;
|
|
//public TargetVoxel parent;
|
|
|
|
public float cost
|
|
{
|
|
get { return distance + heuristic; }
|
|
}
|
|
|
|
public TargetVoxel(Vector3Int coordinates, float distance, float heuristic)
|
|
{
|
|
this.coordinates = coordinates;
|
|
this.distance = distance;
|
|
this.heuristic = heuristic;
|
|
}
|
|
|
|
public bool Equals(TargetVoxel other)
|
|
{
|
|
return this.coordinates.Equals(other.coordinates);
|
|
}
|
|
|
|
public int CompareTo(TargetVoxel other)
|
|
{
|
|
return this.cost.CompareTo(other.cost);
|
|
}
|
|
}
|
|
|
|
public VoxelPathFinder(MeshVoxelizer voxelizer)
|
|
{
|
|
this.voxelizer = voxelizer;
|
|
closed = new bool[voxelizer.resolution.x, voxelizer.resolution.y, voxelizer.resolution.z];
|
|
open = new PriorityQueue<TargetVoxel>();
|
|
}
|
|
|
|
private TargetVoxel AStar(in Vector3Int start, Func<TargetVoxel,bool> termination, Func<Vector3Int, float> heuristic)
|
|
{
|
|
Array.Clear(closed, 0, closed.Length);
|
|
|
|
// A* algorithm:
|
|
open.Clear();
|
|
open.Enqueue(new TargetVoxel(start, 0, 0));
|
|
|
|
while (open.Count() != 0)
|
|
{
|
|
var current = open.Dequeue();
|
|
|
|
if (termination(current))
|
|
return current;
|
|
|
|
closed[current.coordinates.x, current.coordinates.y, current.coordinates.z] = true;
|
|
|
|
for (int i = 0; i < MeshVoxelizer.fullNeighborhood.Length; ++i)
|
|
{
|
|
var successorCoords = current.coordinates + MeshVoxelizer.fullNeighborhood[i];
|
|
|
|
if (voxelizer.VoxelExists(successorCoords) &&
|
|
voxelizer[successorCoords.x, successorCoords.y, successorCoords.z] != MeshVoxelizer.Voxel.Outside &&
|
|
!closed[successorCoords.x, successorCoords.y, successorCoords.z])
|
|
{
|
|
var successor = new TargetVoxel(successorCoords, current.distance + voxelizer.GetDistanceToNeighbor(i),
|
|
heuristic(successorCoords));
|
|
//successor.parent = current;
|
|
|
|
int index = -1;
|
|
for (int j = 0; j < open.Count(); ++j)
|
|
if (open.data[j].coordinates == successorCoords)
|
|
{ index = j; break; }
|
|
|
|
if (index < 0)
|
|
open.Enqueue(successor);
|
|
else if (successor.distance < open.data[index].distance)
|
|
open.data[index] = successor;
|
|
}
|
|
}
|
|
}
|
|
|
|
return new TargetVoxel(Vector3Int.zero, -1, -1);
|
|
}
|
|
|
|
public TargetVoxel FindClosestNonEmptyVoxel(in Vector3Int start)
|
|
{
|
|
if (voxelizer == null) return new TargetVoxel(Vector3Int.zero, -1, -1);
|
|
|
|
if (!voxelizer.VoxelExists(start))
|
|
return new TargetVoxel(Vector3Int.zero, -1, -1);
|
|
|
|
if (voxelizer[start.x, start.y, start.z] != MeshVoxelizer.Voxel.Outside)
|
|
return new TargetVoxel(start, 0, 0);
|
|
|
|
Array.Clear(closed, 0, closed.Length);
|
|
|
|
return AStar(start,
|
|
(TargetVoxel v) => {
|
|
return voxelizer[v.coordinates.x, v.coordinates.y, v.coordinates.z] != MeshVoxelizer.Voxel.Outside;
|
|
},
|
|
(Vector3Int c) => {
|
|
return 0;
|
|
});
|
|
}
|
|
|
|
public TargetVoxel FindPath(in Vector3Int start, Vector3Int end)
|
|
{
|
|
if (voxelizer == null) return new TargetVoxel(Vector3Int.zero,-1, -1);
|
|
|
|
if (!voxelizer.VoxelExists(start) || !voxelizer.VoxelExists(end))
|
|
return new TargetVoxel(Vector3Int.zero, -1, -1);
|
|
|
|
if (voxelizer[start.x, start.y, start.z] == MeshVoxelizer.Voxel.Outside ||
|
|
voxelizer[end.x, end.y, end.z] == MeshVoxelizer.Voxel.Outside)
|
|
return new TargetVoxel(Vector3Int.zero, -1, -1);
|
|
|
|
return AStar(start,
|
|
(TargetVoxel v) => {
|
|
return v.coordinates == end;
|
|
},
|
|
(Vector3Int c) => {
|
|
return Vector3.Distance(c, end) * voxelizer.voxelSize;
|
|
});
|
|
}
|
|
}
|
|
}
|