//Expand one node
private void ExpandNode(Node currentNode)
{
//To be able to expand we need the simulated car's heading and position
float heading = currentNode.heading;
//Expand both forward and reverse
for (int j = 0; j < driveDistances.Length; j++)
{
float driveDistance = driveDistances[j];
//Expand by looping through all steering angles
for (int i = 0; i < steeringAngles.Count; i++)
{
//Steering angle
float alpha = steeringAngles[i];
//Turning angle
float beta = (driveDistance / carData.GetWheelBase()) * Mathf.Tan(alpha);
//Simulate the skeleton car
Vector3 newCarPos = SkeletonCar.CalculateNewPosition(heading, beta, driveDistance, currentNode.carPos);
float newHeading = SkeletonCar.CalculateNewHeading(heading, beta);
//Get the cell pos of the new position
IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(newCarPos);
//
//Check if the car is colliding with obstacle or is outside of map
//
if (!ObstaclesDetection.TargetPositionWithinTrack(newCarPos, newHeading, carData))
//if (ObstaclesDetection.HasCarInvalidPosition(newCarPos, newHeading, carData)) (MATT)
{
continue;
}
//
//Check if this node is closed
//
//Important this is after obstacle/outside of map detection or may be out of range
int roundedAngle = RoundAngle(newHeading * Mathf.Rad2Deg);
if (closedCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
{
continue;
}
//
//Check if this node is cheaper than any other node by calculating costs
//
//First create a new node with all data we need to calculate costs
Node nextNode = new Node();
nextNode.cellPos = cellPos;
nextNode.carPos = newCarPos;
nextNode.heading = newHeading;
nextNode.steeringAngle = steeringAngles[i];
nextNode.isReversing = driveDistance < 0f ? true : false;
nextNode.h = HeuristicsController.heuristics[cellPos.x, cellPos.z];
nextNode.previousNode = currentNode;
//Now we can calculate the cost to reach this node
nextNode.g = CalculateCosts(nextNode);
//Is this cost lower than it was or have we not expanded to this this cell with this angle?
if (
((lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle) && nextNode.g < lowestCostCells[cellPos.x, cellPos.z][roundedAngle])) ||
!lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
{
//We havent expanded to this node before with this angle
if (!lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
{
lowestCostCells[cellPos.x, cellPos.z].Add(roundedAngle, nextNode.g);
}
//The costs is lower than a previous expansion
else
{
lowestCostCells[cellPos.x, cellPos.z][roundedAngle] = nextNode.g;
//Now we should remove the old node from the heap
//Actually not needed because it's costly to remove nodes from the heap
//So its better to just skip the node when finding nodes with lowest cost
}
//Add the node to the list with open nodes
openNodes.Add(nextNode);
}
}
}
}