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