@@ -118,6 +118,30 @@ double EuclideanDistance((int Row, int Col) a, (int Row, int Col) b)
118118 return FindPath ( start , goal , GetNeighbors , heuristic ) ;
119119 }
120120
121+ /// <summary>
122+ /// Calculates the total cost of a path.
123+ /// </summary>
124+ /// <typeparam name="T">Type of node identifier.</typeparam>
125+ /// <param name="path">The path to calculate cost for.</param>
126+ /// <param name="getCost">Function to get the cost between two adjacent nodes.</param>
127+ /// <returns>Total cost of the path.</returns>
128+ public static double CalculatePathCost < T > ( List < T > path , Func < T , T , double > getCost )
129+ where T : notnull
130+ {
131+ if ( path == null || path . Count < 2 )
132+ {
133+ return 0 ;
134+ }
135+
136+ double totalCost = 0 ;
137+ for ( int i = 0 ; i < path . Count - 1 ; i ++ )
138+ {
139+ totalCost += getCost ( path [ i ] , path [ i + 1 ] ) ;
140+ }
141+
142+ return totalCost ;
143+ }
144+
121145 private static void ValidateGridPosition (
122146 bool [ , ] grid ,
123147 ( int Row , int Col ) position ,
@@ -139,7 +163,7 @@ private static void ValidateGridPosition(
139163 }
140164 }
141165
142- private static IEnumerable < ( ( int Row , int Col ) , double ) > GetGridNeighbors (
166+ private static IEnumerable < ( ( int Row , int Col ) Position , double Cost ) > GetGridNeighbors (
143167 ( int Row , int Col ) pos ,
144168 int rows ,
145169 int cols ,
@@ -149,61 +173,38 @@ private static void ValidateGridPosition(
149173 var neighbors = new List < ( ( int Row , int Col ) , double ) > ( ) ;
150174
151175 var directions = new [ ]
152- {
153- ( - 1 , 0 ) , ( 1 , 0 ) , ( 0 , - 1 ) , ( 0 , 1 ) ,
176+ {
177+ ( - 1 , 0 ) , ( 1 , 0 ) , ( 0 , - 1 ) , ( 0 , 1 ) ,
154178 } ;
155179
156180 if ( allowDiagonal )
157181 {
158182 directions = directions . Concat ( new [ ]
159- {
160- ( - 1 , - 1 ) , ( - 1 , 1 ) , ( 1 , - 1 ) , ( 1 , 1 ) ,
183+ {
184+ ( - 1 , - 1 ) , ( - 1 , 1 ) , ( 1 , - 1 ) , ( 1 , 1 ) ,
161185 } ) . ToArray ( ) ;
162186 }
163187
164188 foreach ( var ( dr , dc ) in directions )
165- {
166- int newRow = pos . Row + dr ;
167- int newCol = pos . Col + dc ;
189+ {
190+ int newRow = pos . Row + dr ;
191+ int newCol = pos . Col + dc ;
168192
169- bool isInBounds = newRow >= 0 && newRow < rows ;
170- isInBounds = isInBounds && newCol >= 0 && newCol < cols ;
193+ bool isInBounds = newRow >= 0 && newRow < rows ;
194+ isInBounds = isInBounds && newCol >= 0 && newCol < cols ;
171195
172- if ( isInBounds && grid [ newRow , newCol ] )
173- {
174- // Cost is sqrt(2) for diagonal, 1 for cardinal
175- bool isDiagonal = dr != 0 && dc != 0 ;
176- double cost = isDiagonal ? Math . Sqrt ( 2 ) : 1.0 ;
177- neighbors . Add ( ( ( newRow , newCol ) , cost ) ) ;
196+ if ( isInBounds && grid [ newRow , newCol ] )
197+ {
198+ // Cost is sqrt(2) for diagonal, 1 for cardinal
199+ bool isDiagonal = dr != 0 && dc != 0 ;
200+ double cost = isDiagonal ? Math . Sqrt ( 2 ) : 1.0 ;
201+ neighbors . Add ( ( ( newRow , newCol ) , cost ) ) ;
178202 }
179203 }
180204
181205 return neighbors ;
182206 }
183207
184- /// <summary>
185- /// Calculates the total cost of a path.
186- /// </summary>
187- /// <typeparam name="T">Type of node identifier.</typeparam>
188- /// <param name="path">The path to calculate cost for.</param>
189- /// <param name="getCost">Function to get the cost between two adjacent nodes.</param>
190- /// <returns>Total cost of the path.</returns>
191- public static double CalculatePathCost < T > ( List < T > path , Func < T , T , double > getCost ) where T : notnull
192- {
193- if ( path == null || path . Count < 2 )
194- {
195- return 0 ;
196- }
197-
198- double totalCost = 0 ;
199- for ( int i = 0 ; i < path . Count - 1 ; i ++ )
200- {
201- totalCost += getCost ( path [ i ] , path [ i + 1 ] ) ;
202- }
203-
204- return totalCost ;
205- }
206-
207208 private static List < T > ReconstructPath < T > ( Dictionary < T , T > cameFrom , T current ) where T : notnull
208209 {
209210 var path = new List < T > { current } ;
0 commit comments