//Check if the car outside of map (MATT)
public static bool TargetPositionWithinTrack(Vector3 carRearWheelPos, float heading, CarData carData)
{
bool withinTrack = true;
//Make the car bigger than it is to be on the safe side
float marginOfSafety = 0.5f;
float carLength = carData.GetLength() + marginOfSafety;
float carWidth = carData.GetWidth() + marginOfSafety;
//Find the center pos of the car (carPos is at the rearWheels)
float distCenterToRearWheels = carData.GetDistanceToRearWheels();
float xCenter = carRearWheelPos.x + distCenterToRearWheels * Mathf.Sin(heading);
float zCenter = carRearWheelPos.z + distCenterToRearWheels * Mathf.Cos(heading);
Vector3 carPos = new Vector3(xCenter, carRearWheelPos.y, zCenter);
//Find all corners of the car
Rectangle carCornerPos = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);
//Detect if any of the corners is outside of the map
if (
!PathfindingController.IsPositionWithinGrid(carCornerPos.FL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.FR) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BR))
{
//At least one of the corners is outside of the map
withinTrack = false;
}
return(withinTrack);
}
//Update the circles to see if we can identify the coordinates from geometry
void UpdateCircles(Vector3 carPos, float carHeading)
{
circlePositions = SkeletonCar.GetCirclePositions(carPos, carHeading, circlePositions);
for (int i = 0; i < circleOBj.Length; i++)
{
AddCoordinates(circleOBj[i], circlePositions[i].x, circlePositions[i].z);
}
}
//Update the corners to see if we can identify the coordinates from geometry
void UpdateCorners(Vector3 carPos, float carHeading)
{
float carWidth = 0.95f * 2f;
float carLength = 2.44f * 2f;
Rectangle cornerPos = SkeletonCar.GetCornerPositions(carPos, carHeading, carWidth, carLength);
//Add the coordinates to the spheres
AddCoordinates(cornerFR, cornerPos.FR.x, cornerPos.FR.z);
AddCoordinates(cornerFL, cornerPos.FL.x, cornerPos.FL.z);
AddCoordinates(cornerBR, cornerPos.BR.x, cornerPos.BR.z);
AddCoordinates(cornerBL, cornerPos.BL.x, cornerPos.BL.z);
}
void TestDrive()
{
//Distance between the wheels (= wheelbase)
float L = 2.959f;
//Driving distance each update
float d = 10f;
//Steering angle in radians
float alpha = 20f * Mathf.Deg2Rad;
//Heading direction in radians
float theta = transform.eulerAngles.y * Mathf.Deg2Rad;
//Manual control
d = 0.1f * Input.GetAxis("Vertical");
alpha *= Input.GetAxis("Horizontal");
//Turning angle
float beta = (d / L) * Mathf.Tan(alpha);
//Get the new position
Vector3 newPos = SkeletonCar.CalculateNewPosition(theta, beta, d, transform.position);
//Get the new heading
float newHeading = SkeletonCar.CalculateNewHeading(theta, beta);
//Add the new position to the car
transform.position = newPos;
//Add the new heading to the car
Vector3 currentRot = transform.rotation.eulerAngles;
Vector3 newRotation = new Vector3(currentRot.x, newHeading * Mathf.Rad2Deg, currentRot.z);
transform.rotation = Quaternion.Euler(newRotation);
UpdateCorners(newPos, newHeading);
UpdateCircles(newPos, newHeading);
}
//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;
//Save which cells are expanded so we can close them after we have expanded all directions
expandedCellsForward.Clear();
expandedCellsReverse.Clear();
//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);
//Detect 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;
}
//Is this node closed? Important this is after obstacle/outside of map detection or may be out of range
else if (
(driveDistance < 0f && closedCellsReverse[cellPos.x, cellPos.z]) ||
(driveDistance > 0f && closedCellsForward[cellPos.x, cellPos.z]))
{
continue;
}
//We can create a new node because this is a valid position
else
{
//
//Calculate costs
//
//The cost it took to get to this node
float cost = Mathf.Abs(driveDistance);
//Add cost for turning if we are not having the same steering angle as previously
if (alpha != currentNode.steeringAngle)
{
cost += turningCost;
}
//Add a cost if we are close to an obstacle, its better to drive around them than close to them
//We can use the flow map to check this
/* (MATT)
* if (ObstaclesController.distanceToClosestObstacle[cellPos.x, cellPos.z] < 6)
* {
* cost += obstacleCost;
* }
*/
//Add cost for reversing
if (driveDistance < 0f)
{
cost += reverseCost;
}
//Add a cost if we are switching from reverse -> forward or the opposite
bool isReversing = driveDistance < 0f ? true : false;
if ((isReversing && !currentNode.isReversing) || (!isReversing && currentNode.isReversing))
{
cost += switchingDirectionOfMovementCost;
}
//The cost to reach this node
float g2 = currentNode.g + cost;
//Is this cost lower than it was?
if (
(driveDistance > 0f && g2 < lowestCostForward[cellPos.x, cellPos.z]) ||
(driveDistance < 0f && g2 < lowestCostReverse[cellPos.x, cellPos.z]))
{
//We have found a better path
if (driveDistance > 0f)
{
//lowestCostForward[cellPos.x, cellPos.z] = g2;
expandedCellsForward.Add(new ExpandedCellsStorage(cellPos, g2));
}
if (driveDistance < 0f)
{
//lowestCostReverse[cellPos.x, cellPos.z] = g2;
expandedCellsReverse.Add(new ExpandedCellsStorage(cellPos, g2));
}
//
//Create a new node
//
Node nextNode = new Node();
nextNode.g = g2;
nextNode.h = HeuristicsController.heuristics[cellPos.x, cellPos.z];
nextNode.cellPos = cellPos;
nextNode.previousNode = currentNode;
//Add the car data to the node
nextNode.carPos = newCarPos;
nextNode.heading = newHeading;
nextNode.steeringAngle = steeringAngles[i];
//Are we reversing?
nextNode.isReversing = driveDistance < 0f ? true : false;
//Add the node to the list with open nodes
openNodes.Add(nextNode);
}
}
}
}
//Close all cells we expanded to from this node so we cant reach them again from another node
if (expandedCellsForward.Count > 0)
{
for (int k = 0; k < expandedCellsForward.Count; k++)
{
//closedArrayForward[expandedCellsForward[k].x, expandedCellsForward[k].z] = true;
IntVector2 cellPos = expandedCellsForward[k].cellPos;
if (expandedCellsForward[k].cost < lowestCostForward[cellPos.x, cellPos.z])
{
lowestCostForward[cellPos.x, cellPos.z] = expandedCellsForward[k].cost;
}
}
}
if (expandedCellsReverse.Count > 0)
{
for (int k = 0; k < expandedCellsReverse.Count; k++)
{
//closedArrayReverse[expandedCellsReverse[k].x, expandedCellsReverse[k].z] = true;
IntVector2 cellPos = expandedCellsReverse[k].cellPos;
if (expandedCellsReverse[k].cost < lowestCostReverse[cellPos.x, cellPos.z])
{
lowestCostReverse[cellPos.x, cellPos.z] = expandedCellsReverse[k].cost;
}
}
}
}
//Check if the car is colliding with an obstacle or is outside of map
public static bool HasCarInvalidPosition(Vector3 carRearWheelPos, float heading, CarData carData)
{
bool hasInvalidPosition = false;
//Make the car bigger than it is to be on the safe side
float marginOfSafety = 0.5f;
float carLength = carData.GetLength() + marginOfSafety;
float carWidth = carData.GetWidth() + marginOfSafety;
//Find the center pos of the car (carPos is at the rearWheels)
float distCenterToRearWheels = carData.GetDistanceToRearWheels();
float xCenter = carRearWheelPos.x + distCenterToRearWheels * Mathf.Sin(heading);
float zCenter = carRearWheelPos.z + distCenterToRearWheels * Mathf.Cos(heading);
Vector3 carPos = new Vector3(xCenter, carRearWheelPos.y, zCenter);
//
// Step 1. Check if the car's corners is inside of the map
//
//Find all corners of the car
Rectangle carCornerPos = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);
//Detect if any of the corners is outside of the map
if (
!PathfindingController.IsPositionWithinGrid(carCornerPos.FL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.FR) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BR))
{
//At least one of the corners is outside of the map
hasInvalidPosition = true;
return(hasInvalidPosition);
}
//
// Step 2. Check if the car's center position is far away from an obstacle
//
//We dont need to check if the car is colliding with an obstacle if the distance to an obstacle is great
IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(carPos);
//The car is not colliding with anything if the steps to an obstacle is greater than the length of the car
if (ObstaclesController.distanceToClosestObstacle[cellPos.x, cellPos.z] > carData.GetLength())
{
//This is a valid position
hasInvalidPosition = false;
return(hasInvalidPosition);
}
//
// Step 3. Check if the car is hitting an obstacle
//
//Find all corners of the car
Rectangle carCornerPosFat = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);
//Method 1 - Use the car's corners and then rectangle-rectangle-intersection with the obstacles
hasInvalidPosition = ObstacleDetectionCorners(carPos, carCornerPosFat);
//Method 2 - Approximate the car with circles
//hasInvalidPosition = ObstacleDetectionCircles(carCenterPos, heading, carData, carCornerPosFat);
return(hasInvalidPosition);
}
//Approximate the car's area with circles to detect if there's an obstacle within the circle
private static bool ObstacleDetectionCircles(Vector3 carPos, float heading, CarData carData, Rectangle carCornerPos)
{
bool hasInvalidPosition = false;
Vector3[] circlePositions = new Vector3[3];
//Get the position of the 3 circles that approximates the size of the car
circlePositions = SkeletonCar.GetCirclePositions(carPos, heading, circlePositions);
//
//Find all obstacles close to the car
//
//The car's length is 5 m and each obstacle is 1 m, so add a little to be on the safe side
//float searchRadius = carData.GetLength() * 0.5f + 1.5f;
//List<ObstacleData> obstaclesThatAreClose = FindCloseObstaclesWithinRadius(carPos, searchRadius * searchRadius);
List <ObstacleData> obstaclesThatAreClose = FindCloseObstaclesCell(carPos, carCornerPos);
//If there are no obstacle close, then return
if (obstaclesThatAreClose.Count == 0)
{
return(hasInvalidPosition);
}
//
//If there are obstacles around the car, then we have to see if some of them intersect
//
//The radius of one circle that approximates the area of the car
//The width of the car is 2 m but the circle has to be larger to provide a margin of safety
float circleRadius = 1.40f;
//But we also need to add the radius of the box obstacle which has a width of 1 m
float criticalRadius = circleRadius + 0.5f;
//And square it to speed up
float criticalRadiusSqr = criticalRadius * criticalRadius;
//Loop through all circles and detect if there's an obstacle within the circle
for (int i = 0; i < circlePositions.Length; i++)
{
Vector3 currentCirclePos = circlePositions[i];
//Is there an obstacle within the radius of this circle
for (int j = 0; j < obstaclesThatAreClose.Count; j++)
{
float distSqr = (currentCirclePos - obstaclesThatAreClose[j].centerPos).sqrMagnitude;
if (distSqr < criticalRadiusSqr)
{
hasInvalidPosition = true;
return(hasInvalidPosition);
}
}
}
return(hasInvalidPosition);
}
//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);
}
}
}
}
