Unity3D Pathfinding Class

Node.cs

using UnityEngine;
using System.Collections;
using System;

public class Node : IComparable<Node> {

    public enum OPEN_STATE {
        BY_NONE,
        BY_START,
        BY_END
    };

    public Vector3 pos;
    public Node parent;

    public float g;
    public float f;
    public float h;

    public bool closed = false;
    public OPEN_STATE opened = OPEN_STATE.BY_NONE;

    public Node(Vector3 pos){
        this.pos = pos;
    }

    public int CompareTo(Node other){
        int value = (int)(other.f - this.f);

        return value;
    }

    public bool IsFirstNode(){
        return pos.x == -1;
    }

    public bool NodeMet(Node other){
        return opened != OPEN_STATE.BY_NONE && other.opened != OPEN_STATE.BY_NONE && opened != other.opened;
    }

    public bool StartNode(){
        return opened == OPEN_STATE.BY_START;
    }

    public bool EndNode(){
        return opened == OPEN_STATE.BY_END;
    }

}

PathFind.cs

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;
using Wintellect.PowerCollections;

public class PathFind {

    private Map _map;

    private OrderedBag<Node> _startList;
    private OrderedBag<Node> _endList;

    private Dictionary<Vector3, Node> _nodeCache;

    private PathFindProps _props;

    private Vector3[] _directions;

    private Stack<Vector3> _finishedPath;

    private int _nodeCount;

    public PathFind(Map map){
        _map = map;
        _startList = new OrderedBag<Node>();
        _endList = new OrderedBag<Node>();
        _nodeCache = new Dictionary<Vector3, Node>();
        _directions = new Vector3[4];

        _directions[0] = new Vector3(1, 0, 0);
        _directions[1] = new Vector3(-1, 0, 0);
        _directions[2] = new Vector3(0, 0, 1);
        _directions[3] = new Vector3(0, 0, -1);
    }

    public Stack<Vector3> Search(PathFindProps pathFindProps){
        _startList.Clear();
        _endList.Clear();
        _nodeCache.Clear();
        _nodeCount = 0;

        _props = pathFindProps;

        int count = 0;

        Node parent = new Node(new Vector3(-1, 0, -1));
        parent.opened = Node.OPEN_STATE.BY_START;
        AddNode(_props.startPos, parent);

        parent = new Node(new Vector3(-1, 0, -1));
        parent.opened = Node.OPEN_STATE.BY_END;
        AddNode(_props.endPos, parent);

        Node n;

        while(_startList.Count != 0 && _endList.Count != 0){
            n = _startList.RemoveLast();

            n.closed = true;

            for(int i=0; i<_directions.Length; i++){
                Vector3 dir = n.pos + _directions[i];
                if(AddNode(dir, n)){
                    return _finishedPath;
                }
            }

            n = _endList.RemoveLast();

            n.closed = true;

            for(int i=0; i<_directions.Length; i++){
                Vector3 dir = n.pos + _directions[i];
                if(AddNode(dir, n)){
                    return _finishedPath;
                }
            }

            count++;
            if(count == _props.maxLength){
                Debug.LogWarning("Pathfind max length hit");
                // max searchs met
                return null;
            }

        }

        // path not found
        return null;
    }

    private float Heuristic(float dx, float dy){
        // best first finder
        return dx + dy;
    }

    private bool AddNode(Vector3 pos, Node parentNode){
        Tile tile = _map.FindTile(pos);
        if(tile == null) return false;

        if(_props.checkWalk && !tile.walkable) return false;

        bool result = CheckTile(tile, pos, parentNode);

        if(!result) return false;

        Node n;

        if(!_nodeCache.TryGetValue(pos, out n)){
            n = new Node(pos);
            _nodeCache.Add(n.pos, n);
        }

        if(n.closed){
            return false;
        }

        // path is finished when both nodes meet
        if(n.NodeMet(parentNode)){
             CreatePath(n, parentNode);
             return true;
        }

        Vector3 nodePos = n.pos;

        // get distance between current node and parent node
        float ng = parentNode.g + ((nodePos.x - parentNode.pos.x == 0 || nodePos.z - parentNode.pos.z == 0) ? 1 : MathUtil.SQRT2);

        // check is node has not been inspected yet or 
        // can be reached with smaller cost from current node
        if(n.opened == Node.OPEN_STATE.BY_NONE || ng < n.g){
            n.g = ng;

            if(n.h == 0){
                Vector3 target = _props.endPos;
                if(parentNode.EndNode()){
                    target = _props.startPos;
                }

                n.h = _props.weight * Heuristic(Mathf.Abs(nodePos.x - target.x), Mathf.Abs(nodePos.z - target.z));
            }
            n.f = n.g + n.h;
            n.parent = parentNode;

            if(n.parent.IsFirstNode()){
                n.g = 0;
                n.f = 0;
            }

            if(n.opened == Node.OPEN_STATE.BY_NONE){
                n.opened = parentNode.opened;
                if(parentNode.opened == Node.OPEN_STATE.BY_END){
                    _endList.Add(n);
                } else {
                    _startList.Add(n);
                }

            }

        }

        return false;
    }

    private bool CheckTile(Tile tile, Vector3 pos, Node parentNode){

        // check tile height difference
        if(!parentNode.IsFirstNode()){
            Tile parentTile = _map.FindTile(parentNode.pos);

            if(parentNode.EndNode()){
                Tile temp = parentTile;
                parentTile = tile;
                tile = temp;
            }

            if(tile.GetHeightDiff(parentTile) > _props.maxJump){
                return false;
            }
            if(tile.GetHeightDiff(parentTile) < -_props.maxFall){
                return false;
            }
        }

        bool result = true;
        if(_props.OnCheck != null){
            if(parentNode.StartNode()){
                result = _props.OnCheck(pos, parentNode.pos);
            } else {
                // coming backwards
                result = _props.OnCheck(parentNode.pos, pos);
            }
        }

        return result;
    }

    private List<Vector3> CreateBacktrace(Node node){
        List<Vector3> path = new List<Vector3>();
        path.Add(node.pos);

        if(node.parent != null){
            while(!node.parent.IsFirstNode()){
                node = node.parent;
                path.Add(node.pos);
            }
        }

        return path;
    }

    private void CreatePath(Node node, Node secondNode){
        Node temp;
        if(node.EndNode()){
            temp = secondNode;
            secondNode = node;
            node = temp;
        }

        List<Vector3> firstPath = CreateBacktrace(node);
        List<Vector3> secondPath = CreateBacktrace(secondNode);

        // final path is always backwards
        secondPath.Reverse();
        foreach(Vector3 v in firstPath){
            secondPath.Add(v);
        }

        _finishedPath = new Stack<Vector3>(secondPath);
    }
}

PathFindProps.cs

using UnityEngine;
using System.Collections;
using System;

public class PathFindProps {

    public Vector3 startPos;
    public Vector3 endPos;

    public Action<Vector3> OnVisit;
    public Func<Vector3, Vector3, bool> OnCheck;

    public bool checkWalk = true;
    public float maxJump = 1.5f;
    public float maxFall = 2.0f;
    public int maxLength = 100;
    public float weight = 1.0f;
}