@@ -214,6 +214,7 @@ template <class UserState> class AStarSearch
 			// The user is going to use the Goal Node he passed in 
 			// so copy the parent pointer of n 
 			m_Goal->parent = n->parent;
+			m_Goal->g = n->g;
 
 			// A special case is that the goal was passed in as the start state
 			// so handle that here
@@ -513,6 +514,20 @@ template <class UserState> class AStarSearch
 		return NULL;
 	}
 
+	// Get final cost of solution
+	// Returns FLT_MAX if goal is not defined or there is no solution
+	float GetSolutionCost()
+	{
+		if( m_Goal && m_State == SEARCH_STATE_SUCCEEDED )
+		{
+			return m_Goal->g;
+		}
+		else
+		{
+			return FLT_MAX;
+		}
+	}
+
 	// For educational use and debugging it is useful to be able to view
 	// the open and closed list at each step, here are two functions to allow that.