2020-05-03 13:05:05 +02:00
using System ;
using System.Collections.Generic ;
using System.Linq ;
using System.Text ;
using System.Threading.Tasks ;
using Microsoft.Xna.Framework ;
2020-05-09 15:54:53 +02:00
using C5 ;
using System.Diagnostics ;
2020-05-03 13:05:05 +02:00
class Astar
{
private Vector2 tractorPos ;
private Vector2 housePos ;
2020-05-07 22:09:45 +02:00
private static Crops [ , ] crops ;
2020-05-03 13:05:05 +02:00
private Vector2 Size ;
2020-05-03 16:35:46 +02:00
private Vector2 targetPos ;
2020-05-04 01:17:10 +02:00
private int Rotation ;
2020-05-03 13:05:05 +02:00
2020-05-07 22:09:45 +02:00
public void update ( Crops [ , ] newCrops , Vector2 newSize , Vector2 newTractorPos , Vector2 newHousePos , int rotation )
2020-05-03 13:05:05 +02:00
{
tractorPos = new Vector2 ( ( int ) newTractorPos . X , ( int ) newTractorPos . Y ) ;
housePos = new Vector2 ( ( int ) newHousePos . X , ( int ) newHousePos . Y ) ;
crops = newCrops ;
Size = newSize ;
2020-05-04 01:17:10 +02:00
Rotation = rotation ;
2020-05-03 13:05:05 +02:00
}
2020-05-07 22:09:45 +02:00
public void update ( Crops [ , ] newCrops , Vector2 newSize , Vector2 newTractorPos , Vector2 newHousePos , Vector2 newTargetPos , int rotation )
{
tractorPos = new Vector2 ( ( int ) newTractorPos . X , ( int ) newTractorPos . Y ) ;
housePos = new Vector2 ( ( int ) newHousePos . X , ( int ) newHousePos . Y ) ;
crops = newCrops ;
Size = newSize ;
Rotation = rotation ;
targetPos = newTargetPos ;
}
2020-05-04 01:50:43 +02:00
// Get all adjacent nodes
2020-05-08 00:35:53 +02:00
public List < Nodes > GetAdjacentNodes ( Vector2 currentPos )
2020-05-03 13:05:05 +02:00
{
2020-05-03 16:35:46 +02:00
var adjacentNodes = new List < Nodes > ( )
{
2020-05-10 21:16:24 +02:00
new Nodes ( new Vector2 ( currentPos . X , currentPos . Y - 1 ) , 0 ) ,
2020-05-03 23:40:40 +02:00
new Nodes ( new Vector2 ( currentPos . X + 1 , currentPos . Y ) , 1 ) ,
2020-05-10 21:16:24 +02:00
new Nodes ( new Vector2 ( currentPos . X , currentPos . Y + 1 ) , 2 ) ,
2020-05-03 23:40:40 +02:00
new Nodes ( new Vector2 ( currentPos . X - 1 , currentPos . Y ) , - 1 ) ,
2020-05-03 16:35:46 +02:00
} ;
2020-05-03 23:40:40 +02:00
//check if out of range
2020-05-04 01:17:10 +02:00
for ( int i = 3 ; i > = 0 ; i - - )
2020-05-03 17:06:03 +02:00
{
if ( adjacentNodes [ i ] . getCords ( ) . X < 0 | | adjacentNodes [ i ] . getCords ( ) . Y < 0 )
adjacentNodes . Remove ( adjacentNodes [ i ] ) ;
else
{
2020-05-03 19:30:41 +02:00
if ( adjacentNodes [ i ] . getCords ( ) . X > Size . X - 1 | | adjacentNodes [ i ] . getCords ( ) . Y > Size . Y - 1 )
2020-05-03 17:06:03 +02:00
adjacentNodes . Remove ( adjacentNodes [ i ] ) ;
}
}
2020-05-04 01:50:43 +02:00
// return if not an obstacle
2020-05-03 16:35:46 +02:00
return adjacentNodes . Where (
2020-05-05 16:27:45 +02:00
item = > ( crops [ ( int ) item . getCords ( ) . X , ( int ) item . getCords ( ) . Y ] . getStatus ( ) ) ! = 0 ) . ToList ( ) ;
2020-05-03 16:35:46 +02:00
}
2020-05-03 23:40:40 +02:00
// Heuristic function, Manhattan method.
2020-05-03 16:35:46 +02:00
public int ComputeHScore ( Vector2 currentNode , Vector2 endNote )
{
return ( int ) ( Math . Abs ( endNote . X - currentNode . X ) + Math . Abs ( endNote . Y - currentNode . Y ) ) ;
}
2020-05-04 01:50:43 +02:00
// Rotation Cost
2020-05-03 23:40:40 +02:00
public int CalculateRotationCost ( int currDir , int newDir )
{
if ( currDir = = newDir )
return 0 ;
else if ( Math . Abs ( currDir - newDir ) = = 1 | | Math . Abs ( currDir - newDir ) = = 3 )
2020-05-10 13:51:19 +02:00
return 3 ;
2020-05-04 01:50:43 +02:00
else if ( Math . Abs ( currDir - newDir ) = = 0 | | Math . Abs ( currDir - newDir ) = = 2 )
2020-05-10 13:51:19 +02:00
return 18 ;
2020-05-03 23:40:40 +02:00
return 0 ;
}
2020-05-04 01:50:43 +02:00
// Convert rotation used by sprite, to get direction of first node in next path
2020-05-04 01:17:10 +02:00
public int ConvertRotation ( )
{
int rotation = 0 ;
2020-05-09 15:54:53 +02:00
if ( Rotation > 135 & & Rotation < 225 )
2020-05-04 01:17:10 +02:00
rotation = 0 ;
2020-05-09 15:54:53 +02:00
else if ( Rotation > 225 & & Rotation < 315 )
2020-05-04 01:17:10 +02:00
rotation = 1 ;
2020-05-09 15:54:53 +02:00
else if ( Rotation > 315 & & Rotation < 45 )
2020-05-04 01:17:10 +02:00
rotation = 2 ;
2020-05-09 15:54:53 +02:00
else if ( Rotation > 45 & & Rotation < 135 )
2020-05-04 01:17:10 +02:00
rotation = - 1 ;
return rotation ;
}
2020-05-04 01:50:43 +02:00
// Main function of A* algorithm
2020-05-07 22:09:45 +02:00
public Path FindPath ( bool flipArray )
2020-05-03 16:35:46 +02:00
{
2020-05-04 01:50:43 +02:00
int g = 0 ;
int direction = ConvertRotation ( ) ;
2020-05-03 17:06:03 +02:00
Path path = new Path ( ) ;
2020-05-03 22:28:52 +02:00
MinHeap openList = new MinHeap ( ) ;
MinHeap closedList = new MinHeap ( ) ;
2020-05-03 17:52:35 +02:00
Nodes target = new Nodes ( targetPos ) ;
2020-05-04 01:17:10 +02:00
Nodes startPos = new Nodes ( tractorPos , direction ) ;
2020-05-03 16:35:46 +02:00
Nodes current = null ;
2020-05-03 22:28:52 +02:00
openList . Insert ( startPos ) ;
2020-05-03 16:35:46 +02:00
2020-05-03 22:28:52 +02:00
while ( openList . GetSize ( ) > 0 )
2020-05-03 16:35:46 +02:00
{
2020-05-03 22:28:52 +02:00
current = openList . getMin ( ) ;
closedList . Insert ( current ) ;
openList . removeMin ( ) ;
2020-05-03 23:40:40 +02:00
direction = current . getDirection ( ) ;
2020-05-03 16:35:46 +02:00
2020-05-03 22:28:52 +02:00
if ( current . getCords ( ) = = target . getCords ( ) )
2020-05-04 01:17:10 +02:00
break ;
2020-05-03 16:35:46 +02:00
var adjacentNodes = GetAdjacentNodes ( current . getCords ( ) ) ;
2020-05-04 01:17:10 +02:00
foreach ( var adjacentNode in adjacentNodes )
2020-05-03 16:35:46 +02:00
{
2020-05-04 01:50:43 +02:00
if ( closedList . Exists ( adjacentNode . getCords ( ) ) ) // check if adjacent node is on closed list, if it is, skip it
2020-05-03 16:35:46 +02:00
continue ;
2020-05-04 01:50:43 +02:00
g = current . getG ( ) + crops [ ( int ) adjacentNode . getCords ( ) . X , ( int ) adjacentNode . getCords ( ) . Y ] . getCostOnMovement ( ) + CalculateRotationCost ( direction , adjacentNode . getDirection ( ) ) ; // calculate g - cost from start point
if ( ! ( openList . Exists ( adjacentNode . getCords ( ) ) ) ) // if adjacent node is not on open list, add it
2020-05-03 16:35:46 +02:00
{
adjacentNode . setG ( g ) ;
2020-05-03 17:52:35 +02:00
adjacentNode . setH ( ComputeHScore ( adjacentNode . getCords ( ) , target . getCords ( ) ) ) ;
2020-05-03 16:35:46 +02:00
adjacentNode . calculateF ( ) ;
adjacentNode . setParent ( current ) ;
2020-05-03 22:28:52 +02:00
openList . Insert ( adjacentNode ) ;
2020-05-03 16:35:46 +02:00
}
else
{
2020-05-04 01:50:43 +02:00
if ( g + adjacentNode . getH ( ) < adjacentNode . getF ( ) ) // check if adjacent node is a better path than the current one
2020-05-03 16:35:46 +02:00
{
adjacentNode . setG ( g ) ;
adjacentNode . calculateF ( ) ;
adjacentNode . setParent ( current ) ;
}
}
2020-05-03 23:40:40 +02:00
2020-05-03 16:35:46 +02:00
}
}
2020-05-04 01:50:43 +02:00
// backtrack to create path
2020-05-03 17:52:35 +02:00
while ( current ! = null )
2020-05-03 16:35:46 +02:00
{
path . AddNode ( current ) ;
current = current . getParent ( ) ;
}
2020-05-07 22:09:45 +02:00
if ( flipArray )
path = path . FlipArray ( ) ;
2020-05-04 01:50:43 +02:00
2020-05-03 22:28:52 +02:00
openList . deleteHeap ( ) ;
closedList . deleteHeap ( ) ;
2020-05-09 15:54:53 +02:00
2020-05-03 16:35:46 +02:00
return path ;
2020-05-03 13:05:05 +02:00
}
2020-05-03 16:35:46 +02:00
2020-05-10 21:16:24 +02:00
public bool isReachable ( Crops [ , ] crops , Vector2 targetPos , Vector2 start )
{
Rotation = 0 ;
int g = 0 ;
int direction = ConvertRotation ( ) ;
Path path = new Path ( ) ;
MinHeap openList = new MinHeap ( ) ;
MinHeap closedList = new MinHeap ( ) ;
Nodes target = new Nodes ( targetPos ) ;
Nodes startPos = new Nodes ( tractorPos , direction ) ;
Nodes current = null ;
openList . Insert ( startPos ) ;
while ( openList . GetSize ( ) > 0 )
{
current = openList . getMin ( ) ;
closedList . Insert ( current ) ;
openList . removeMin ( ) ;
direction = current . getDirection ( ) ;
if ( current . getCords ( ) = = target . getCords ( ) )
break ;
var adjacentNodes = GetAdjacentNodes ( current . getCords ( ) ) ;
foreach ( var adjacentNode in adjacentNodes )
{
if ( closedList . Exists ( adjacentNode . getCords ( ) ) ) // check if adjacent node is on closed list, if it is, skip it
continue ;
g = current . getG ( ) + crops [ ( int ) adjacentNode . getCords ( ) . X , ( int ) adjacentNode . getCords ( ) . Y ] . getCostOnMovement ( ) + CalculateRotationCost ( direction , adjacentNode . getDirection ( ) ) ; // calculate g - cost from start point
if ( ! ( openList . Exists ( adjacentNode . getCords ( ) ) ) ) // if adjacent node is not on open list, add it
{
adjacentNode . setG ( g ) ;
adjacentNode . setH ( ComputeHScore ( adjacentNode . getCords ( ) , target . getCords ( ) ) ) ;
adjacentNode . calculateF ( ) ;
adjacentNode . setParent ( current ) ;
openList . Insert ( adjacentNode ) ;
}
else
{
if ( g + adjacentNode . getH ( ) < adjacentNode . getF ( ) ) // check if adjacent node is a better path than the current one
{
adjacentNode . setG ( g ) ;
adjacentNode . calculateF ( ) ;
adjacentNode . setParent ( current ) ;
}
}
}
}
openList . deleteHeap ( ) ;
closedList . deleteHeap ( ) ;
if ( current . getCords ( ) ! = targetPos )
return false ;
else
return true ;
}
2020-05-04 01:17:10 +02:00
}