diff --git a/source/simulation/Entity.cpp b/source/simulation/Entity.cpp
index 5c38217ad7..ab9ae1b2a5 100755
--- a/source/simulation/Entity.cpp
+++ b/source/simulation/Entity.cpp
@@ -472,7 +472,12 @@ void CEntity::update( size_t timestep )
 				updateCollisionPatch();
 				return;
 			case CEntityOrder::ORDER_GOTO_WAYPOINT:
-				if ( processGotoWaypoint( current, timestep ) )
+				if ( processGotoWaypoint( current, timestep, false ) )
+					break;
+				updateCollisionPatch();
+				return;
+			case CEntityOrder::ORDER_GOTO_WAYPOINT_CONTACT:
+				if ( processGotoWaypoint( current, timestep, true ) )
 					break;
 				updateCollisionPatch();
 				return;
diff --git a/source/simulation/Entity.h b/source/simulation/Entity.h
index 2eee4c2f10..a4e9cfbf44 100755
--- a/source/simulation/Entity.h
+++ b/source/simulation/Entity.h
@@ -225,7 +225,7 @@ private:
 
 	bool processGotoNoPathing( CEntityOrder* current, size_t timestep_milli );
 	bool processGoto( CEntityOrder* current, size_t timestep_milli );
-	bool processGotoWaypoint( CEntityOrder* current, size_t timestep_milli );
+	bool processGotoWaypoint( CEntityOrder* current, size_t timestep_milli, bool contact );
 
 	bool processPatrol( CEntityOrder* current, size_t timestep_milli );
 
diff --git a/source/simulation/EntityOrders.h b/source/simulation/EntityOrders.h
index d0585154b8..79abd4d9a8 100755
--- a/source/simulation/EntityOrders.h
+++ b/source/simulation/EntityOrders.h
@@ -86,6 +86,7 @@ public:
 		ORDER_GOTO_SMOOTHED,
 		ORDER_GOTO_COLLISION,
 		ORDER_GOTO_WAYPOINT,
+		ORDER_GOTO_WAYPOINT_CONTACT,
 		ORDER_GOTO,
 		ORDER_RUN,
 		ORDER_PATROL,
diff --git a/source/simulation/EntityStateProcessing.cpp b/source/simulation/EntityStateProcessing.cpp
index 985d2bac11..efb5df8a31 100755
--- a/source/simulation/EntityStateProcessing.cpp
+++ b/source/simulation/EntityStateProcessing.cpp
@@ -592,7 +592,7 @@ bool CEntity::processGoto( CEntityOrder* current, size_t UNUSED(timestep_millis)
 	return( true );
 }
 
-bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep_milli) )
+bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep_milli), bool contact )
 {
 	CVector2D pos( m_position.X, m_position.Z );
 	CVector2D path_to = current->m_data[0].location;
@@ -607,7 +607,9 @@ bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep
 		return( false );
 	}
 
-	g_Pathfinder.requestLowLevelPath( me, path_to );
+	processChooseMovement( Distance );
+
+	g_Pathfinder.requestLowLevelPath( me, path_to, contact );
 
 	return( true );
 }
diff --git a/source/simulation/PathfindEngine.cpp b/source/simulation/PathfindEngine.cpp
index 16fda2c2d2..36e051f096 100755
--- a/source/simulation/PathfindEngine.cpp
+++ b/source/simulation/PathfindEngine.cpp
@@ -1,19 +1,157 @@
 #include "precompiled.h"
 
 #include "PathfindEngine.h"
-#include "PathfindSparse.h"
+//#include "PathfindSparse.h"
 #include "ConfigDB.h"
+#include "Terrain.h"
+#include "Collision.h"
+
+#include "ps/Game.h"
+#include "ps/World.h"
+
+#define MAXSLOPE 3000
+#define INITNODES 1000
+
+void processPath(HEntity, AStarNode*, bool);
+
+class AStarNode
+{
+public:
+	float f, g, h;
+	AStarNode* parent;
+	CVector2D coord;
+	bool operator <(const AStarNode& rhs) const { return f<rhs.f; }
+	bool equals(const AStarNode& rhs) const 
+	{ 
+		return ( coord.x==rhs.coord.x ) && ( coord.y==rhs.coord.y ); 
+	}
+};
+
+struct AStarNodeComp
+{
+  bool operator()(const AStarNode* n1, const AStarNode* n2) const
+  {
+    return (*n2) < (*n1);
+  }
+};
+
+CVector2D TilespaceToWorldspace( const CVector2D &ts )
+{
+	return CVector2D(ts.x*CELL_SIZE+CELL_SIZE/2, ts.y*CELL_SIZE+CELL_SIZE/2);
+}
+
+CVector2D WorldspaceToTilespace( const CVector2D &ws )
+{
+	return CVector2D(floor(ws.x/CELL_SIZE), floor(ws.y/CELL_SIZE));
+}
+
+bool isPassable( const CVector2D &wc )
+{
+	CTerrain* pTerrain = g_Game->GetWorld()->GetTerrain();
+	float slope = pTerrain->getSlope(wc.x, wc.y);
+	if (  slope < MAXSLOPE )
+	{
+		// If no entity blocking, return true
+		CBoundingBox bounds(wc.x, wc.y, 0, CELL_SIZE, CELL_SIZE, 3);
+		if ( getCollisionObject(&bounds) == NULL )
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+class PriQueue 
+ : public std::priority_queue<AStarNode*, std::vector<AStarNode*>, AStarNodeComp>
+{
+public:
+	// Promote a node in the PQ, or if it doesn't exist, add it
+	void promote(AStarNode* node)
+	{
+		if (node == NULL)
+			return;
+		std::vector<AStarNode*>::iterator ind, first;
+		for( ind = c.begin(); ind!=c.end() && !((*ind)->equals(*node)); ind++ );
+		if (ind == c.end())
+		{
+			push(node);
+			return;
+		}
+		if( (*ind)->f <= node->f ) return;
+
+		first = c.begin();
+		int index = ind-first;
+		int parent = (index - 1)/2;
+
+		while ( index>0 && (*(first+parent))->f > node->f )
+		{
+			*(first+index) = *(first+parent);
+			index = parent;
+			parent = (parent - 1)/2;
+		}
+		*(first+index) = node;
+	}
+};
+
+bool CPathfindEngine::isVisited( const CVector2D& coord )
+{
+	ASNodeHashMap::iterator it = visited.find(coord);
+	return ( it != visited.end() );
+}
+
+std::vector<AStarNode*> CPathfindEngine::getNeighbors( AStarNode* node )
+{
+	std::vector<AStarNode*> vec;
+
+	for( int xdiff = -1; xdiff <= 1; xdiff++ )
+	{
+		for( int ydiff = -1; ydiff <= 1; ydiff++ )
+		{
+			if ( xdiff!=0 || ydiff!=0 )
+			{
+				CVector2D coord = node->coord;
+				coord.x += xdiff;  coord.y += ydiff;
+				if ( isVisited(coord) || isPassable(TilespaceToWorldspace(coord)) )
+				{
+					AStarNode* n = getFreeASNode();
+					n->coord = coord;
+					n->f = n->g = n->h = 0;
+					n->parent = 0;
+					vec.push_back(n);
+				}
+			}
+		}
+	}
+
+	return vec;
+}
 
 CPathfindEngine::CPathfindEngine()
 {
-	CConfigValue* sparseDepth = g_ConfigDB.GetValue( CFG_USER, "pathfind.sparse.recursiondepth" );
+/*	CConfigValue* sparseDepth = g_ConfigDB.GetValue( CFG_USER, "pathfind.sparse.recursiondepth" );
 	if( sparseDepth )
-		sparseDepth->GetInt( SPF_RECURSION_DEPTH );
+		sparseDepth->GetInt( SPF_RECURSION_DEPTH ); */
+	for(int i=0; i<INITNODES; i++)
+	{
+		freeNodes.push_back(new AStarNode);
+	}
+}
+
+CPathfindEngine::~CPathfindEngine()
+{
+	std::vector<AStarNode*>::iterator it;
+	for( it = usedNodes.begin(); it != usedNodes.end(); it++)
+	{
+		delete (*it);
+	}
+	for( it = freeNodes.begin(); it != freeNodes.end(); it++)
+	{
+		delete (*it);
+	}
 }
 
 void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination )
 {
-//	pathSparse( entity, destination );
 	/* TODO: Add code to generate high level path
 	         For now, just the one high level waypoint to the final 
 			  destination is added
@@ -24,25 +162,182 @@ void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination
 	entity->m_orderQueue.push_front( waypoint );
 }
 
-void CPathfindEngine::requestLowLevelPath( HEntity entity, const CVector2D& destination )
+void CPathfindEngine::requestLowLevelPath( HEntity entity, const CVector2D& destination, bool contact )
 {
-	/* Temporary - will be replaced by low-level A* */
-	pathSparse( entity, destination );
+	/* TODO: Pull out code into AStarEngine */
+	/* TODO: Put a limit on the search space to prevent unreachable destinations from
+	         eating the CPU */
+	CVector2D source( entity->m_position.X, entity->m_position.Z );
+
+	// If the goal is unreachable, move it towards the start until it is reachable
+	CVector2D goalLoc = destination;
+	CVector2D unitVec = (entity->m_position - goalLoc);
+	unitVec= unitVec.normalize() * CELL_SIZE / 2;
+	while( !isPassable(goalLoc) )
+	{
+		goalLoc += unitVec;
+	}
+	
+	// Initialize priority queue (PQ) and visited list (V)
+	visited.clear();
+	PriQueue priQ;
+
+	// Construct a dummy node for the goal
+	AStarNode* goal = getFreeASNode();
+	goal->coord = WorldspaceToTilespace(goalLoc);
+	goal->parent = NULL;
+	goal->f = goal->g = goal->h = 0;
+
+	// Assign f,g,h to start location, add to PQ
+	AStarNode* start = getFreeASNode();
+	start->coord = WorldspaceToTilespace(source);
+	start->g = 0;
+	start->f = start->h = (goal->coord-start->coord).length();
+	start->parent = NULL;
+	priQ.push(start);
+	visited[start->coord] = start;
+
+	// Loop until PQ is empty
+	while(!priQ.empty())
+	{
+	    // Select best cost node, B, from PQ
+		AStarNode* best = priQ.top();
+		priQ.pop();
+		// If B is the goal, we are done, and found a path
+		if ( best->equals( *goal ) )
+		{
+			goal->parent = best;
+			goal->g = goal->f = best->g + 1;
+			break;
+		}
+		
+		std::vector<AStarNode*> neighbors = getNeighbors(best);
+		// For each neighbor, C, of B
+		std::vector<AStarNode*>::iterator it;
+		for( it = neighbors.begin(); it != neighbors.end(); it++ )
+		{
+			AStarNode* C = *it;
+			// Assign f,g,h to C
+			C->g = best->g + 1;
+			// Penalize for non-straight paths
+			if ( best->parent )
+			{
+				int dx1 = C->coord.x - best->coord.x;
+				int dy1 = C->coord.y - best->coord.y;
+				int dx2 = best->coord.x - best->parent->coord.x;
+				int dy2 = best->coord.y - best->parent->coord.y;
+				if ( ((dx1 - dx2) + (dy1 - dy2)) != 0 )
+				{
+					C->g += 0.1f;
+				}
+			}
+
+			C->h = ((goal->coord) - (C->coord)).length();
+			C->f = C->g + C->h;
+			C->parent = best;
+
+			// If C not in V, add C to V and PQ
+			// If the f of C is less than the f of C in the PQ, promote C in PQ and update V
+			ASNodeHashMap::iterator it2 = visited.find(C->coord);
+			if ( it2 != visited.end() && (C->f < it2->second->f) )
+			{
+				it2->second = C;
+				priQ.promote(C);
+			}
+			else if ( it2 == visited.end() )
+			{				
+				visited[C->coord] = C;
+				priQ.push(C);
+			}
+		}
+	}
+
+	if ( goal->parent )
+	{
+		processPath(entity, goal, contact);
+	}
+	else
+	{
+		// If no path was found, then unsolvable
+		// TODO: Figure out what to do in this case
+	}
+	
+	cleanup();
 }
 
 void CPathfindEngine::requestContactPath( HEntity entity, CEntityOrder* current )
 {
-	pathSparse( entity, current->m_data[0].entity->m_position );
-	// For attack orders, do some additional postprocessing (replace goto/nopathing 
-	// with attack/nopathing, up until the attack order marker)
-	std::deque<CEntityOrder>::iterator it;
-	for( it = entity->m_orderQueue.begin(); it != entity->m_orderQueue.end(); it++ )
-	{
-		if( it->m_type == CEntityOrder::ORDER_PATH_END_MARKER )
-			break;
-		if( it->m_type == CEntityOrder::ORDER_GOTO_NOPATHING )
-		{
-			*it = *current;
-		}
-	}
+	/* TODO: Same as non-contact: need high-level planner */
+	CEntityOrder waypoint;
+	waypoint.m_type = CEntityOrder::ORDER_GOTO_WAYPOINT_CONTACT;
+	waypoint.m_data[0].location = current->m_data[0].entity->m_position;
+	entity->m_orderQueue.push_front( waypoint );
+
+	//pathSparse( entity, current->m_data[0].entity->m_position );
+	//// For attack orders, do some additional postprocessing (replace goto/nopathing 
+	//// with attack/nopathing, up until the attack order marker)
+	//std::deque<CEntityOrder>::iterator it;
+	//for( it = entity->m_orderQueue.begin(); it != entity->m_orderQueue.end(); it++ )
+	//{
+	//	if( it->m_type == CEntityOrder::ORDER_PATH_END_MARKER )
+	//		break;
+	//	if( it->m_type == CEntityOrder::ORDER_GOTO_NOPATHING )
+	//	{
+	//		*it = *current;
+	//	}
+	//}
 }
+
+AStarNode* CPathfindEngine::getFreeASNode()
+{
+	AStarNode* ret;
+	if (!freeNodes.empty())
+	{
+		ret = freeNodes.back();
+		freeNodes.pop_back();
+	}
+	else
+	{
+		ret = new AStarNode;
+	}
+	usedNodes.push_back(ret);
+
+	return ret;
+}
+
+void CPathfindEngine::cleanup()
+{
+	std::vector<AStarNode*>::iterator it;
+	for( it = usedNodes.begin(); it != usedNodes.end(); it++)
+	{
+		freeNodes.push_back(*it);
+	}
+
+	usedNodes.clear();
+}
+
+void processPath(HEntity entity, AStarNode* goal, bool contact)
+{
+	AStarNode* current = goal;
+
+	CEntityOrder node;
+	node.m_type = CEntityOrder::ORDER_PATH_END_MARKER;
+	entity->m_orderQueue.push_front( node );
+
+	/* TODO: Smoothing for units with a turning radius */
+	while( current != NULL && current->g != 0 )
+	{
+		if ( !contact )
+		{
+			node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
+		}
+		else
+		{
+			// TODO: Is this right?
+			node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
+		}
+		node.m_data[0].location = TilespaceToWorldspace(current->coord);
+		entity->m_orderQueue.push_front( node );
+		current = current->parent;
+	}
+}
\ No newline at end of file
diff --git a/source/simulation/PathfindEngine.h b/source/simulation/PathfindEngine.h
index 53f46fc47f..451243d332 100755
--- a/source/simulation/PathfindEngine.h
+++ b/source/simulation/PathfindEngine.h
@@ -16,6 +16,7 @@
 #include "Vector2D.h"
 
 #define g_Pathfinder CPathfindEngine::GetSingleton()
+#define MAXSIZE 1024
 
 class CEntityOrder;
 
@@ -25,13 +26,40 @@ enum EPathType
 	PF_ATTACK_MELEE,
 };
 
+class AStarNode;
+class CVector2D_hash_compare
+{
+public:
+	static const size_t bucket_size = 4;
+	static const size_t min_buckets = 16;
+	size_t operator() (const CVector2D& Key) const
+	{
+		return Key.x + Key.y*MAXSIZE;
+	}
+	bool operator() (const CVector2D& _Key1, const CVector2D& _Key2) const
+	{
+		return (_Key1.x < _Key2.x) || (_Key1.x==_Key2.x && _Key1.y < _Key2.y);
+	}
+};
+typedef STL_HASH_MAP<CVector2D, AStarNode*, CVector2D_hash_compare> ASNodeHashMap;
+
 class CPathfindEngine : public Singleton<CPathfindEngine>
 {
 public:
 	CPathfindEngine();
+	~CPathfindEngine();
 	void requestPath( HEntity entity, const CVector2D& destination );
-	void requestLowLevelPath( HEntity entity, const CVector2D& destination );
+	void requestLowLevelPath( HEntity entity, const CVector2D& destination, bool contact );
 	void requestContactPath( HEntity entity, CEntityOrder* current );
+private:
+	std::vector<AStarNode*> getNeighbors( AStarNode* node );
+	bool isVisited( const CVector2D& coord );
+	AStarNode* getFreeASNode();
+	void cleanup();
+
+	std::vector<AStarNode*> freeNodes;
+	std::vector<AStarNode*> usedNodes;
+	ASNodeHashMap visited;
 };
 
 #endif