// Computes heuristics from this state to goal state
// Uses simple manhattan distance
// It looks a bit nasty with "as", but since heuristic function
// is an expert knowledge, it makes a lot of sense
public static float HDisc(IState curr, IState goal)
{
DiscreteState a = curr as DiscreteState;
DiscreteState b = goal as DiscreteState;
return(Vector2.Distance(new Vector2(a.x, a.y), new Vector2(b.x, b.y)));
}
// Update is called once per frame
void Update()
{
uint DiscreteState;
InputDevice device = InputDevices.GetDeviceAtXRNode(node);
if (InputDevices.GetDeviceAtXRNode(node).TryGetFeatureValue(new InputFeatureUsage <uint>(usageName), out DiscreteState))
{
valueText.text = "0x" + DiscreteState.ToString("X8");
}
}
// Equality
public override bool Equals(object other)
{
if (!(other is DiscreteState))
{
return(false);
}
DiscreteState o = (DiscreteState)other;
return(this.x == o.x && this.y == o.y);
}
// Construct discrete graph from file
// It is returning 3 values thus out modifiers
// Parameter neigh tells which neighborhood to use
// Parameter obstacles is filled with vectors
public static void CreateDiscreteFromFile(string filename, int neigh,
out GraphState g, out IState start, out IState goal,
List <Vector3> obstacles)
{
StreamReader sr = new StreamReader(filename);
try {
// Read start coordinates
string[] sxy = sr.ReadLine().Split(' ');
start = new DiscreteState(sxy[0], sxy[1]);
// Read goal coordinates
string[] gxy = sr.ReadLine().Split(' ');
goal = new DiscreteState(gxy[0], gxy[1]);
// Read dimensions of obstacle matrix
string[] dim = sr.ReadLine().Split(' ');
int ydim = int.Parse(dim[0]);
int xdim = int.Parse(dim[1]);
// Read the obstacle matrix
int[,] A = new int[ydim, xdim];
for (int y = 0; y < ydim; y++)
{
string[] bits = sr.ReadLine().Split(' ');
for (int x = 0; x < xdim; x++)
{
A[y, x] = int.Parse(bits[x]);
}
}
// Create all vertices, it creates full ydim by xdim
// vertex space even though some vertices are obstacles
// and have no connections
List <IState> vertices = new List <IState>();
for (int y = 0; y < ydim; y++)
{
for (int x = 0; x < xdim; x++)
{
vertices.Add(new DiscreteState(x, y));
if (A[y, x] == 1) // If it is obstacle, add to list
{
obstacles.Add(new Vector3(x, 0.0f, y));
}
}
}
// Picking the right neighborhood list
List <Vector2> moves = null;
if (neigh == 4)
{
moves = movesN4;
}
else if (neigh == 8)
{
moves = movesN8;
}
else if (neigh == 16)
{
moves = movesN16;
}
else
{
throw new ArgumentException("Neighborhood does not exist.");
}
// Adding edges
g = new GraphState(vertices);
foreach (IState vertex in vertices)
{
DiscreteState state = vertex as DiscreteState;
int x = state.x;
int y = state.y;
Vector2 pos = new Vector2(x, y);
if (A[y, x] == 1) // Vertex is obstacle
{
continue;
}
// Iterate over all neighbors (moves)
foreach (Vector2 move in moves)
{
Vector2 newPos = pos + move;
x = (int)newPos.x;
y = (int)newPos.y;
// Check if edge should exist
// Add edge with current vertex, neighbor vertex
// and magnitude (norm) as weight
if (x >= 0 && x < xdim && y >= 0 && y < ydim &&
A[y, x] == 0)
{
g.AddEdge(
state,
new DiscreteState(x, y),
move.magnitude
);
}
}
}
} finally {
sr.Close(); // Closing stream
}
}
