// 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 } }