the whole game

src/world/level/pathfinder/BinaryHeap.h

@@ -0,0 +1,201 @@
+#ifndef NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__BinaryHeap_H__
+#define NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__BinaryHeap_H__
+
+//package net.minecraft.world.level->pathfinder;
+
+#include "Node.h"
+#include <vector>
+
+class BinaryHeap
+{
+public:
+    BinaryHeap()
+    :   _size(0),
+		_maxSize(1024)
+    {
+		_heap = new Node*[_maxSize];
+        //heap.reserve(1024);
+    }
+
+	~BinaryHeap() {
+		// @todo: figure out who's managing the memory
+		delete[] _heap;
+	}
+
+    Node* insert(Node* node)
+    {
+        if (node->heapIdx>=0) {
+            LOGE("BinaryHeap::insert. Node already added!\n");
+        }
+
+		if (_size == _maxSize)
+		{
+			Node** newHeap = new Node*[_maxSize = _size << 1];
+			for (int i = 0; i < _size; ++i)
+				newHeap[i] = _heap[i];
+
+			//System.arraycopy(heap, 0, newHeap, 0, size);
+			delete[] _heap;
+			_heap = newHeap;
+		}
+
+        // Insert at end and bubble up.
+        _heap[_size] = node;
+		//heap.push_back(node);
+        node->heapIdx = _size;
+        upHeap(_size++);
+
+        return node;
+    }
+    
+    void clear() {
+        _size = 0;
+    }
+
+    Node* pop()
+    {
+        Node* popped = _heap[0];
+        _heap[0] = _heap[--_size];
+        _heap[_size] = NULL;
+		//heap.pop_back(); //?
+        if (_size > 0) downHeap(0);
+        popped->heapIdx=-1;
+        return popped;
+    }
+
+    void remove(Node* node)
+    {
+        // This is what node->heapIdx is for.
+        _heap[node->heapIdx] = _heap[--_size];
+        _heap[_size] = NULL;
+		//heap.pop_back(); //?
+        if (_size > node->heapIdx)
+        {
+            if (_heap[node->heapIdx]->f < node->f)
+            {
+                upHeap(node->heapIdx);
+            }
+            else
+            {
+                downHeap(node->heapIdx);
+            }
+        }
+        // Just as a precaution: should make stuff blow up if the node is abused.
+        node->heapIdx = -1;
+    }
+
+    void changeCost(Node* node, float newCost)
+    {
+        float oldCost = node->f;
+        node->f = newCost;
+        if (newCost < oldCost)
+        {
+            upHeap(node->heapIdx);
+        }
+        else
+        {
+            downHeap(node->heapIdx);
+        }
+    }
+
+    int size()
+    {
+        return _size;
+    }
+
+    bool isEmpty() {
+        return _size==0;
+    }
+
+private:
+    void upHeap(int idx)
+    {
+        Node* node = _heap[idx];
+        float cost = node->f;
+        while (idx > 0)
+        {
+            int parentIdx = (idx - 1) >> 1;
+            Node* parent = _heap[parentIdx];
+            if (cost < parent->f)
+            {
+				//LOGI("idx1: %d\n", idx);
+                _heap[idx] = parent;
+                parent->heapIdx = idx;
+                idx = parentIdx;
+            }
+            else break;
+        }
+		//LOGI("idx2: %d\n", idx);
+        _heap[idx] = node;
+        node->heapIdx = idx;
+    }
+
+    void downHeap(int idx)
+    {
+        Node* node = _heap[idx];
+        float cost = node->f;
+
+        while (true)
+        {
+            int leftIdx = 1 + (idx << 1);
+            int rightIdx = leftIdx + 1;
+
+            if (leftIdx >= _size) break;
+
+            // We definitely have a left child.
+            Node* leftNode = _heap[leftIdx];
+            float leftCost = leftNode->f;
+            // We may have a right child.
+            Node* rightNode;
+            float rightCost;
+
+            if (rightIdx >= _size)
+            {
+                // Only need to compare with left.
+                rightNode = NULL;
+                rightCost = FLT_MAX;//.POSITIVE_INFINITY;
+            }
+            else
+            {
+                rightNode = _heap[rightIdx];
+                rightCost = rightNode->f;
+            }
+
+            // Find the smallest of the three costs: the corresponding node
+            // should be the parent.
+            if (leftCost < rightCost)
+            {
+                if (leftCost < cost)
+                {
+                    _heap[idx] = leftNode;
+                    leftNode->heapIdx = idx;
+					//LOGI("idx3: %d\n", idx);
+                    idx = leftIdx;
+                }
+                else break;
+            }
+            else
+            {
+                if (rightCost < cost)
+                {
+                    _heap[idx] = rightNode;
+                    rightNode->heapIdx = idx;
+					//LOGI("idx4: %d\n", idx);
+                    idx = rightIdx;
+                }
+                else break;
+            }
+        }
+
+		//LOGI("idx5: %d\n", idx);
+        _heap[idx] = node;
+        node->heapIdx = idx;
+    }
+
+	Node** _heap;
+
+    int _size;
+	int _maxSize;
+};
+
+#endif /*NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__BinaryHeap_H__*/

src/world/level/pathfinder/Node.h

@@ -0,0 +1,79 @@
+#ifndef NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__Node_H__
+#define NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__Node_H__
+
+//package net.minecraft.world.level.pathfinder;
+
+#include "../../../util/Mth.h"
+#include <string>
+
+class Node
+{
+public:
+    Node(int x = 0, int y = 0, int z = 0)
+	:	x(x),
+		y(y),
+		z(z),
+		heapIdx(-1),
+		g(0), h(0), f(0),
+		cameFrom(NULL),
+		closed(false),
+		hash(createHash(x,y,z))
+	{
+    }
+
+    static int createHash(const int x, const int y, const int z) {
+        return (y & 0xff) | ((x & 0x7fff) << 8) | ((z & 0x7fff) << 24) | ((x < 0) ? 0x0080000000 : 0) | ((z < 0) ? 0x0000008000 : 0);
+    }
+
+    float distanceTo(Node* to) const {
+        float xd = (float)(to->x - x);
+        float yd = (float)(to->y - y);
+        float zd = (float)(to->z - z);
+        return Mth::sqrt(xd * xd + yd * yd + zd * zd);
+    }
+
+	bool operator==(const Node& rhs) const {
+		return hash == rhs.hash && x == rhs.x && y == rhs.y && z == rhs.z;
+    }
+
+    int hashCode() const {
+        return hash;
+    }
+
+    bool inOpenSet() const {
+		return heapIdx >= 0;
+    }
+
+    std::string toString() const {
+        return "Node::toString not implemented";//x + ", " + y + ", " + z;
+    }
+
+public:
+    int heapIdx;
+    float g, h, f;
+    Node* cameFrom;
+
+	short x, y, z;
+	bool closed;
+private:
+	int hash;
+};
+
+class TNode {
+public:
+	TNode(Node* node)
+	:	node(node)
+	{}
+
+	bool operator==(const TNode& rhs) const {
+		return node->operator==(*rhs.node);
+	}
+	bool operator<(const TNode& rhs) const {
+		if (node->z != rhs.node->z) return node->z < rhs.node->z;
+		if (node->x != rhs.node->x) return node->x < rhs.node->x;
+		return node->y < rhs.node->y;
+	}
+	Node* node;
+};
+
+#endif /*NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__Node_H__*/

src/world/level/pathfinder/Path.cpp

@@ -0,0 +1,134 @@
+#include "Path.h"
+
+int Path::p = 0;
+
+Path::Path()
+:	nodes(NULL),
+	length(0),
+	index(0),
+	id(++p)
+{
+}
+
+Path::~Path()
+{
+	destroy();
+}
+
+bool Path::isEmpty() const
+{
+	return length == 0 || nodes == NULL;
+}
+
+void Path::copyNodes( Node** nodes, int length )
+{
+	destroy();
+
+	this->length = length;
+	this->nodes = new Node*[length];
+
+	for (int i = 0; i < length; ++i)
+		this->nodes[i] = new Node(*nodes[i]);
+}
+
+void Path::destroy()
+{
+	if (nodes) {
+		for (int i = 0; i < length; ++i)
+			delete nodes[i];
+		delete[] nodes;
+
+		nodes = NULL;
+		index = length = 0;
+	}
+}
+
+Node* Path::currentPos()
+{
+	return nodes[index];
+}
+
+Vec3 Path::currentPos( Entity* e ) const
+{
+	return getPos(e, index);
+}
+
+void Path::next()
+{
+	index++;
+}
+
+void Path::setSize( int size )
+{
+	length = size;
+}
+
+int Path::getSize() const
+{
+	return length;
+}
+
+bool Path::isDone() const
+{
+	return index >= length;
+}
+
+Node* Path::last() const
+{
+	if (length > 0) {
+		return nodes[length - 1];
+	}
+	return NULL;
+}
+
+Node* Path::get( int i ) const
+{
+	return nodes[i];
+}
+
+int Path::getIndex() const
+{
+	return index;
+}
+
+void Path::setIndex( int index )
+{
+	this->index = index;
+}
+
+Vec3 Path::getPos( Entity* e, int index ) const
+{
+	float x = nodes[index]->x + (int) (e->bbWidth + 1) * 0.5f;
+	float z = nodes[index]->z + (int) (e->bbWidth + 1) * 0.5f;
+	float y = nodes[index]->y;
+	return Vec3(x, y, z);
+}
+
+bool Path::sameAs( const Path* path ) const
+{
+	if (!path) return false;
+	if (path->length != length) return false;
+	for (int i = 0; i < length; ++i) {
+		Node& node = *path->nodes[i];
+		if (nodes[i]->x != node.x || nodes[i]->y != node.y || nodes[i]->z != node.z)
+			return false;
+	}
+	return true;
+}
+
+bool Path::endsIn( const Vec3& pos ) const
+{
+	const Node* end = last();
+	if (end == NULL) return false;
+	return end->x == Mth::floor(pos.x)
+		&& end->y == Mth::floor(pos.y)
+		&& end->z == Mth::floor(pos.z);
+}
+
+bool Path::endsInXZ( const Vec3& pos ) const
+{
+	const Node* end = last();
+	if (end == NULL) return false;
+	return end->x == Mth::floor(pos.x)
+		&& end->z == Mth::floor(pos.z);
+}

src/world/level/pathfinder/Path.h

@@ -0,0 +1,53 @@
+#ifndef NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__Path_H__
+#define NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__Path_H__
+
+//package net.minecraft.world.level.pathfinder;
+
+#include "Node.h"
+#include "../../phys/Vec3.h"
+#include "../../entity/Entity.h"
+
+class Path
+{
+public:
+	Path();
+	~Path();
+
+	void  copyNodes(Node** nodes, int length);
+
+	void  destroy();
+
+    void  next();
+
+	void  setSize(int size);
+	int   getSize() const;
+
+	bool  isEmpty() const;
+    bool  isDone() const;
+
+    Node* last() const;
+    Node* get(int i) const;
+
+    int   getIndex() const;
+    void  setIndex(int index);
+
+    Vec3  currentPos(Entity* e) const;
+	Node* currentPos();
+
+    Vec3  getPos(Entity* e, int index) const;
+
+    bool  sameAs(const Path* path) const;
+
+    bool  endsIn(const Vec3& pos) const;
+    bool  endsInXZ(const Vec3& pos) const;
+
+	int id;
+private:
+	Node** nodes;
+	int length;
+    int index;
+
+	static int p;
+};
+
+#endif /*NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__Path_H__*/

src/world/level/pathfinder/PathFinder.h

@@ -0,0 +1,394 @@
+#ifndef NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__PathFinder_H__
+#define NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__PathFinder_H__
+
+//package net.minecraft.world.level.pathfinder;
+
+#include "../LevelSource.h"
+#include "../material/Material.h"
+#include "../tile/DoorTile.h"
+#include "../../entity/Entity.h"
+#include "../../../util/Mth.h"
+#include <map>
+
+#include "BinaryHeap.h"
+#include "Node.h"
+#include "Path.h"
+
+static int __created;
+static int __maxCreated = 0;
+static const int MAX_NODES = 2048;
+
+class FreeCache {
+public:
+	FreeCache()
+	:	cache(w * w * h)
+	{}
+	void setCenterPos(int x, int y, int z) {
+		bx = x - w/2;
+		by = y - h/2;
+		bz = z - w/2;
+	}
+	int getValue(int x, int y, int z) {
+		return cache.get(_index(x, y, z));
+	}
+	void setValue(int x, int y, int z, int value) {
+		cache.set(_index(x, y, z), value);
+	}
+	void clear() {
+		cache.setAll(0);
+	}
+	__inline int _index(int x, int y, int z) {
+		return (x-bz) | ((z-bz) << ShiftZ) | ((y-by) << ShiftY);
+	}
+private:
+	int bx, by, bz;
+	DataLayer cache;
+
+	static const int w = 64, h = 32;
+	static const int ShiftY = 10, ShiftZ = 5;
+};
+
+class PathFinder
+{
+	typedef std::map<int, TNode> NodeMap;
+public:
+	PathFinder()
+	:	level(NULL),
+		canOpenDoors(false),
+		avoidWater(false)
+	{
+	}
+    PathFinder(LevelSource* level)
+	:	canOpenDoors(false),
+		avoidWater(false)
+	{
+        setLevelSource(level);
+    }
+
+	void setLevelSource(LevelSource* level) {
+		this->level = level;
+	}
+
+    bool findPath(Path* path, Entity* from, Entity* to, float maxDist) {
+        return findPath(*path, from, to->x, to->bb.y0, to->z, maxDist);
+    }
+
+    bool findPath(Path* path, Entity* from, int x, int y, int z, float maxDist) {
+        return findPath(*path, from, x + 0.5f, y + 0.5f, z + 0.5f, maxDist);
+    }
+
+private:
+    bool findPath(Path& path, Entity* e, float xt, float yt, float zt, float maxDist) {
+		//openSet.clear();
+		//LOGI("<--------------------->\n");
+		static Stopwatch w;
+		w.start();
+
+		// @attn @fix: this is danger!
+		nodes.clear();
+		_nodeIndex = 0;
+
+		// Calculate the From node
+		bool resetAvoidWater = avoidWater;
+		int startY;
+		if (e->isInWater()) {
+			startY = (int) (e->bb.y0);
+			int tileId = level->getTile(Mth::floor(e->x), startY, Mth::floor(e->z));
+			while (tileId == Tile::water->id || tileId == Tile::calmWater->id) {
+				++startY;
+				tileId = level->getTile(Mth::floor(e->x), startY, Mth::floor(e->z));
+			}
+			resetAvoidWater = avoidWater;
+			avoidWater = false;
+		} else startY = Mth::floor(e->bb.y0 + 0.5f);
+
+		Node* from = getNode(Mth::floor(e->bb.x0), startY, Mth::floor(e->bb.z0));
+
+		// Try finding a To node that doesn't have air below
+		const int xx0 = Mth::floor(xt - e->bbWidth / 2);
+		const int yy0 = Mth::floor(yt);
+		const int zz0 = Mth::floor(zt - e->bbWidth / 2);
+
+		Node* to = NULL;
+		if (level->getTile(xx0, yy0-1, zz0)) {
+			to = getNode(xx0, yy0, zz0);
+		} else {
+			const int xx1 = Mth::floor(xt + e->bbWidth /2);
+			const int zz1 = Mth::floor(zt + e->bbWidth /2);
+			for (int xx = xx0; xx <= xx1; ++xx)
+			for (int zz = zz0; zz <= zz1; ++zz) {
+				if (level->getTile(xx, yy0-1, zz) != 0) {
+					to = getNode(xx, yy0, zz);
+					break;
+				}
+			}
+			if (!to) { // Find the first non-air tile below
+				int yy = yy0;
+				while(!level->getTile(xx0, yy-1, zz0) && yy > 0)
+					--yy;
+				to = getNode(xx0, yy, zz0);
+			}
+		}
+
+		Node size(Mth::floor((e->bbWidth + 1)), Mth::floor((e->bbHeight + 1)), Mth::floor((e->bbWidth + 1)));
+        bool out = findPath(path, e, from, to, &size, maxDist);
+		w.stop();
+		//w.printEvery(1, "Pathfinder");
+
+		// Clear excessive Nodes that was created this round
+		if (_nodeIndex >= MAX_NODES) {
+			for (unsigned int i = 0; i < _pending.size(); ++i)
+				delete _pending[i];
+			_pending.clear();
+		}
+
+		return out;
+    }
+
+    // function A*(start,goal)
+    bool findPath(Path& path, Entity* e, Node* from, Node* to, const Node* size, float maxDist) {
+		//static int _x = 0;
+		__created = 0;
+
+        from->g = 0;
+        from->h = from->distanceTo(to);
+        from->f = from->h;
+		
+        openSet.clear();
+        openSet.insert(from);
+
+        Node* closest = from;
+
+        while (!openSet.isEmpty()) {
+			//LOGI("size1: %d\n", openSet.size());
+            Node* x = openSet.pop();
+			//LOGI("size2: %d\n", openSet.size());
+
+			//if (x->x == to->x && x->y == to->y && x->z == to->z) {
+            if (*x == *to) {
+				//LOGI(">>> %p, %p : %d, %d\n", x, to, x->hashCode(), to->hashCode());
+				if (__created > __maxCreated) {
+					__maxCreated = __created;
+					for (int i = 0; i < 1; ++i) LOGI("\tNEW MAX: Created %d nodes\n", __created);
+				}
+				reconstruct_path(path, from, to); //@fix?
+                return true;
+            }
+
+            if (x->distanceTo(to) < closest->distanceTo(to)) {
+				//LOGI("closer!\n");
+                closest = x;
+            }
+            x->closed = true;
+
+            int neighborCount = getNeighbors(e, x, size, to, maxDist);
+            for (int i = 0; i < neighborCount; i++) {
+                Node* y = neighbors[i];
+				if (y->closed) continue;
+
+                float tentative_g_score = x->g + x->distanceTo(y);
+                if (!y->inOpenSet() || tentative_g_score < y->g) {
+				//if (!openSet.has(y) || tentative_g_score < y->g) {
+                    y->cameFrom = x;
+                    y->g = tentative_g_score;
+                    y->h = y->distanceTo(to);
+                    if (y->inOpenSet()) {
+					//if (openSet.has(y)) {
+                        openSet.changeCost(y, y->g + y->h);
+						//delete y;
+                    } else {
+                        y->f = y->g + y->h;
+                        openSet.insert(y);
+                    }
+                } //else delete y;
+				
+				//bool isBetter = false;
+				//float tentative_g_score = x->g + x->distanceTo(y);
+				//if (!y->inOpenSet()) {
+				//	openSet.insert(y);
+				//	y->h = y->distanceTo(to);
+				//	isBetter = true;
+				//} else if (tentative_g_score < y->g) {
+				//	isBetter = true;
+				//}
+				//if (isBetter) {
+				//	//y->f = y->g + y->h;
+				//	y->cameFrom = x;
+				//	y->g = tentative_g_score;
+				//	openSet.changeCost(y, y->g + y->h);
+				//}
+            }
+        }
+
+		if (__created > __maxCreated) {
+			__maxCreated = __created;
+			for (int i = 0; i < 1; ++i) LOGI("\tNEW MAX: Created %d nodes\n", __created);
+		}
+
+        if (closest == from)
+			return false;
+		reconstruct_path(path, from, closest); //@fix?
+		return true;
+    }
+
+    int getNeighbors(Entity* entity, Node* pos, const Node* size, Node* target, float maxDist) {
+        int p = 0;
+
+		//LOGI("Getting neighbours for: (%d, %d, %d)\n", pos->x, pos->y, pos->z);
+
+        int jumpSize = 0;
+        if (isFree(entity, pos->x, pos->y + 1, pos->z, size) == TYPE_OPEN) jumpSize = 1;
+
+        Node* n = getNode(entity, pos->x, pos->y, pos->z + 1, size, jumpSize);
+        Node* w = getNode(entity, pos->x - 1, pos->y, pos->z, size, jumpSize);
+        Node* e = getNode(entity, pos->x + 1, pos->y, pos->z, size, jumpSize);
+        Node* s = getNode(entity, pos->x, pos->y, pos->z - 1, size, jumpSize);
+
+        if (n != NULL && !n->closed && n->distanceTo(target) < maxDist) neighbors[p++] = n;
+        if (w != NULL && !w->closed && w->distanceTo(target) < maxDist) neighbors[p++] = w;
+        if (e != NULL && !e->closed && e->distanceTo(target) < maxDist) neighbors[p++] = e;
+        if (s != NULL && !s->closed && s->distanceTo(target) < maxDist) neighbors[p++] = s;
+        return p;
+    }
+
+    Node* getNode(Entity* entity, int x, int y, int z, const Node* size, int jumpSize) {
+        Node* best = NULL;
+		int pathType = isFree(entity, x, y, z, size);
+		if (pathType == TYPE_WALKABLE) return getNode(x, y, z);
+        if (pathType == TYPE_OPEN) best = getNode(x, y, z);
+		if (best == NULL && jumpSize > 0 && pathType != TYPE_FENCE && isFree(entity, x, y + jumpSize, z, size) == TYPE_OPEN) {
+			best = getNode(x, y + jumpSize, z);
+            y += jumpSize;
+        }
+
+        if (best != NULL) {
+            int drop = 0;
+            int cost = 0;
+			while (y > 0) {
+				cost = isFree(entity, x, y - 1, z, size);
+				if (avoidWater && cost == TYPE_WATER) return NULL;
+				if (cost != TYPE_OPEN) break;
+
+                // fell too far?
+                if (++drop >= 4) return NULL;
+                if (--y > 0) best = getNode(x, y, z);
+            }
+            // fell into lava?
+            if (cost == TYPE_LAVA) return NULL;
+        }
+
+        return best;
+    }
+
+    Node* getNode(int x, int y, int z) {
+        int i = Node::createHash(x, y, z);
+		NodeMap::iterator it = nodes.find(i);
+
+        if (it == nodes.end()){
+            Node* node = new_Node(x, y, z);
+			++__created;
+            nodes.insert(std::make_pair(i, TNode(node)));
+			return node;
+        }
+        return it->second.node;
+    }
+
+	static const int TYPE_FENCE    = 1;
+    static const int TYPE_LAVA     = 2;
+    static const int TYPE_WATER    = 3;
+    static const int TYPE_BLOCKED  = 4;
+    static const int TYPE_OPEN     = 5;
+	static const int TYPE_WALKABLE = 6;
+
+    int isFree(Entity* entity, int x, int y, int z, const Node* size) {
+		bool walkable = false;
+		//LOGI("isfree: [%d, %d, %d]\n", x, y, z);
+        for (int xx = x; xx < x + size->x; xx++) {
+            for (int yy = y; yy < y + size->y; yy++) {
+                for (int zz = z; zz < z + size->z; zz++) {
+                    int tileId = level->getTile(xx, yy, zz);
+					if (tileId <= 0) continue;
+                    if (tileId == Tile::door_iron->id || tileId == Tile::door_wood->id) {
+						//LOGI("canOpenDoors? %d : %d\n", canOpenDoors, DoorTile::isOpen(level->getData(xx, yy, zz)));
+						if (tileId == Tile::door_wood->id && canOpenDoors)
+							continue;
+                        int data = level->getData(xx, yy, zz);
+						if (!DoorTile::isOpen(entity->level, xx, yy, zz)) {
+							return TYPE_BLOCKED;
+						}
+						else
+							continue;
+                    }
+					else if (tileId == Tile::water->id || tileId == Tile::calmWater->id) {
+						if (avoidWater) {
+							return TYPE_WATER;
+						}
+						walkable = true;
+					}
+					else if (tileId == Tile::fence->id || tileId == Tile::fenceGate->id) {
+						return TYPE_FENCE;
+					}
+
+					const Material* m = Tile::tiles[tileId]->material;
+                    if (m->blocksMotion()) {
+						return TYPE_BLOCKED;
+					} else walkable = true;
+                    if (m == Material::lava) {
+                        return TYPE_LAVA;
+                    }
+                }
+            }
+        }
+        return TYPE_OPEN;
+    }
+
+    // function reconstruct_path(came_from,current_node)
+    void reconstruct_path(Path& path, Node* from, Node* to) {
+        int count = 1;
+        Node* n = to;
+        while (n->cameFrom != NULL) {
+            count++;
+            n = n->cameFrom;
+        }
+
+		int size = count;
+        Node** nodes = new Node*[size]; //@todo: have one long static array for this
+        n = to;
+        nodes[--count] = n;
+        while (n->cameFrom != NULL) {
+            n = n->cameFrom;
+            nodes[--count] = n;
+        }
+		//LOGI("Setting %p nodes to path %p\n", nodes, &path);
+		path.copyNodes(nodes, size);
+		delete[] nodes;
+    }
+
+	Node* new_Node(int x, int y, int z) {
+		//return new Node(x, y, z);
+		if (++_nodeIndex >= MAX_NODES) {
+			Node* out = new Node(x, y, z);
+			_pending.push_back( out );
+			return out;
+		}
+		Node& n = _nodes[_nodeIndex-1];
+		n = Node(x, y, z);
+		return &n;
+	}
+
+    LevelSource* level;
+
+    BinaryHeap	openSet;
+	NodeMap		nodes;
+
+	Node _nodes[MAX_NODES];
+	std::vector<Node*> _pending;
+	int _nodeIndex;
+
+    Node* neighbors[32];
+public:
+	bool canOpenDoors;
+	bool avoidWater;
+};
+
+#endif /*NET_MINECRAFT_WORLD_LEVEL_PATHFINDER__PathFinder_H__*/