1 files changed, 107 insertions, 0 deletions
diff --git a/labb8/src/costs.cpp b/labb8/src/costs.cpp
new file mode 100755
index 0000000..1b663b1
--- /dev/null
+++ b/labb8/src/costs.cpp
@@ -0,0 +1,107 @@
+/*
+ * TDDD86 Trailblazer
+ * This file contains functions for computing the costs of navigating the
+ * terrain and maze worlds, as well as heuristics for predicting costs.
+ * See costs.h for declarations of each function and related constants.
+ *
+ * Please do not modify this provided file. Your turned-in files should work
+ * with an unmodified version of all provided code files.
+ *
+ * Author: Marty Stepp, Keith Schwarz, et al
+ * Slight modifications by Tommy Farnqvist
+ */
+
+#include "costs.h"
+#include <cmath>
+#include <limits>
+using namespace std;
+
+/*
+ * The cost of moving from one location to another in the world is computed as
+ *
+ * distance(loc1, loc2) * k * |Delta h|
+ *
+ * This means that the cost of moving from one point to another depends on two
+ * factors: the change in elevation and the absolute distance between those
+ * points.	Moving in a cardinal direction has base cost 1, while moving
+ * diagonally has base cost sqrt(2).
+ *
+ * The secondary cost associated with moving in the terrain is based on the
+ * change in the height between the two points.	 This implementation makes the
+ * cost linear in the difference in height.
+ */
+double terrainCost(TBLoc from, TBLoc to, const Grid<double>& world) {
+    // The cost of moving from a location to itself is 0.
+	if (from == to) return 0.0;
+
+    // Confirm that the locations are adjacent to one another.
+	int drow = abs(to.row - from.row);
+	int dcol = abs(to.col - from.col);
+	if (drow > 1 || dcol > 1) {
+		error("Non-adjacent locations passed into cost function.");
+	}
+
+    // Determine the absolute distance between the points.
+	double distance = sqrt(double(drow * drow + dcol * dcol));
+    double dheight = fabs(world.get(to.row, to.col) - world.get(from.row, from.col));
+	return distance + kAltitudePenalty * dheight;
+}
+
+/*
+ * Our terrain heuristic simply returns the straight-line distance between
+ * the two points, plus the height differential scaled appropriately.
+ */
+double terrainHeuristic(TBLoc from, TBLoc to, const Grid<double>& world) {
+	int drow = to.row - from.row;
+	int dcol = to.col - from.col;
+    double dheight = fabs(world.get(to.row, to.col) - world.get(from.row, from.col));
+	return sqrt((double) (drow * drow + dcol * dcol)) + kAltitudePenalty * dheight;
+}
+
+/*
+ * The cost of moving in a maze is 1.0 when moving in cardinal directions from
+ * floors to floors and is infinite otherwise.	This prevents any motion across
+ * walls or diagonally.
+ */
+double mazeCost(TBLoc from, TBLoc to, const Grid<double>& world) {
+    // The cost of moving from a location to itself is 0.
+	if (from == to) return 0.0;
+
+    // Confirm that the locations are adjacent to one another.
+	int drow = abs(to.row - from.row);
+	int dcol = abs(to.col - from.col);
+	if (drow > 1 || dcol > 1) {
+		error("Non-adjacent locations passed into cost function.");
+	}
+	
+    // Moving diagonally costs infinitely much.
+    if (drow == 1 && dcol == 1) {
+        // return numeric_limits<double>::infinity();
+        return POSITIVE_INFINITY;
+    }
+
+    // See if we're moving to or from a wall.
+    if (world.get(from.row, from.col) == kMazeWall
+            || world.get(to.row, to.col) == kMazeWall) {
+        // return numeric_limits<double>::infinity();
+        return POSITIVE_INFINITY;
+    }
+
+	return 1.0;
+}
+
+/*
+ * The maze heuristic is the rectangular "Manhattan" distance for moving around
+ * in a grid world.
+ */
+double mazeHeuristic(TBLoc from, TBLoc to, const Grid<double>& /*world*/) {
+	return abs(from.row - to.row) + abs(from.col - to.col);
+}
+
+/*
+ * The zero heuristic, unsurprisingly, returns zero and ignores its
+ * arguments.
+ */
+double zeroHeuristic(TBLoc, TBLoc, const Grid<double>&) {
+	return 0.0;
+}