diff --git a/map.cpp b/map.cpp
index 4716983..0a55164 100755
--- a/map.cpp
+++ b/map.cpp
@@ -139,6 +139,10 @@ namespace battleship{
 		Camera *cam = root->getCamera();
 		cam->setPosition(Vector3(1, 1, 1) * 40);
 		cam->lookAt(Vector3(-1, -1, -1).norm(), Vector3(-1, 1, -1).norm());
+
+		//TODO replace hardcoded values
+		size = Vector3(100, 33, 100);
+		bottom = 33;
     }
 
     void Map::unload() {}
diff --git a/map.h b/map.h
index e91bf43..800349d 100755
--- a/map.h
+++ b/map.h
@@ -46,9 +46,8 @@ namespace battleship{
 		vb01::Model *terrainModel = nullptr;
 		std::string mapName;
 		std::vector<WaterBody> waterBodies;
-		//TODO replace hardcoded values
-		vb01::Vector3 size = vb01::Vector3(100, 30, 100);
-		float bottom = 30;
+		vb01::Vector3 size;
+		float bottom;
 
         Map(){}
 		void loadSkybox(gameBase::LuaManager*);
diff --git a/unit.cpp b/unit.cpp
index 0fbf645..b84b7f1 100755
--- a/unit.cpp
+++ b/unit.cpp
@@ -154,7 +154,7 @@ namespace battleship{
 		Vector3 linDest = Vector3(pathPoints[0].x, pos.y, pathPoints[0].z);
 		Vector3 destDir = (linDest - pos).norm();
 
-		float angle = dirVec.getAngleBetween(destDir);
+		float angle = (destDir != Vector3::VEC_ZERO ? dirVec.getAngleBetween(destDir) : -1);
 		UnitDataManager *udm = UnitDataManager::getSingleton();
 
 		if(angle > udm->getAnglePrecision()[id]){
@@ -171,8 +171,6 @@ namespace battleship{
 				float movementAmmount = (speed > dist ? dist : speed);
 				advance(movementAmmount);
 			}
-			else
-				pathPoints.erase(pathPoints.begin());
 
 			if(fabs(pos.y - pathPoints[0].y) > destOffset){
 				float dist = pos.y - pathPoints[0].y;
@@ -183,6 +181,9 @@ namespace battleship{
 
 				advance(movementAmmount, MoveDir::UP);
 			}
+
+			if(pos.getDistanceFrom(pathPoints[0]) <= destOffset)
+				pathPoints.erase(pathPoints.begin());
 		}
 
 		if(pathPoints.empty())
@@ -348,7 +349,7 @@ namespace battleship{
 			if(map->getWaterBody(i).isPointWithin(pos)){
 				if(type == UnitType::SEA_LEVEL || type == UnitType::UNDERWATER){
 					Vector2 s = map->getWaterBody(i).size;
-					size = Vector3(s.x, 0, s.y);
+					size = Vector3(s.x, size.y, s.y);
 				}
 
 				waterbodyId = i;
@@ -372,12 +373,12 @@ namespace battleship{
 
 		generateWeights(weights, cellsByDim, cellSize, waterbodyId);
 
-		Pathfinder *pathfinder = Pathfinder::getSingleton();
 		int source = map->getCellId(pos, cellSize, waterbodyId);
 		int dest = map->getCellId(order.targets[0].pos, cellSize, waterbodyId);
+		Pathfinder *pathfinder = Pathfinder::getSingleton();
 		vector<int> path = pathfinder->findPath(weights, numCells, source, dest);
-		bool impassibleNodePresent = false;
 
+		bool impassibleNodePresent = false;
 		pathPoints.clear();
 
 		for(int i = 1; i < path.size(); i++)