//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); } } } }