public TreeNode(TreeSettings aSettings, List <TreePoint> aPoints, int aDepth)
{
int axis = aSettings.GetAxis(aDepth);
if (axis == 0)
{
aPoints.Sort(sortX);
}
else if (axis == 1)
{
aPoints.Sort(sortY);
}
else if (axis == 2)
{
aPoints.Sort(sortZ);
}
int median = aPoints.Count / 2;
point = aPoints[median];
List <TreePoint> leftList = aPoints.GetRange(0, median);
List <TreePoint> rightList = aPoints.GetRange(median + 1, aPoints.Count - (median + 1));
if (leftList.Count > 0)
{
left = new TreeNode(aSettings, leftList, aDepth + 1);
}
if (rightList.Count > 0)
{
right = new TreeNode(aSettings, rightList, aDepth + 1);
}
}
public TreePoint Get(Vector3 aAt)
{
TreePoint pt = null;
float dist = 0;
root.GetNearest(settings, 0, aAt, ref pt, ref dist);
return(pt);
}
public void GetNearest(TreeSettings aSettings, int aDepth, Vector3 aPt, ref TreePoint aClosest, ref float aClosestDist)
{
if (IsLeaf)
{
float dist = (point.point - aPt).sqrMagnitude;
if (aClosest == null || dist < aClosestDist)
{
aClosest = point;
aClosestDist = dist;
}
return;
}
int axis = aSettings.GetAxis(aDepth);
bool goLeft = false;
if (axis == 0)
{
goLeft = aPt.x <= point.point.x ? true : false;
}
else if (axis == 1)
{
goLeft = aPt.y <= point.point.y ? true : false;
}
else if (axis == 2)
{
goLeft = aPt.z <= point.point.z ? true : false;
}
TreeNode first = goLeft ? left : right;
TreeNode other = goLeft ? right: left;
if (first == null)
{
first = other;
other = null;
}
first.GetNearest(aSettings, aDepth + 1, aPt, ref aClosest, ref aClosestDist);
float thisDist = (point.point - aPt).sqrMagnitude;
if (thisDist < aClosestDist)
{
aClosest = point;
aClosestDist = thisDist;
}
if (other != null)
{
float axisDist = aSettings.AxisDist(axis, point.point, aPt);
if (axisDist * axisDist <= aClosestDist)
{
other.GetNearest(aSettings, aDepth + 1, aPt, ref aClosest, ref aClosestDist);
}
}
}
public TreePoint(Vector3 position, float theta, float velocity, float cost, TreePoint parent)
{
this.position = position;
this.theta = theta;
this.velocity = velocity;
this.cost = cost;
this.parent = parent;
children = new List <TreePoint>();
}
/// <summary>
/// Generates the heightmap thread.
/// </summary>
private void GenerateHeightmapThread()
{
lock (HeightmapThread)
{
Heightmap = NoiseProvider.GetHeightmapData(Setting.HeightmapResolution, Setting.HeightmapResolution, Position.X, Position.Z);
if (Setting.Trees.Count > 0)
{
for (var zRes = 0; zRes < Setting.HeightmapResolution; zRes++)
{
for (var xRes = 0; xRes < Setting.HeightmapResolution; xRes++)
{
int height = Mathf.FloorToInt(Heightmap[zRes, xRes] * 100);
if (height > 30 && zRes % 20 == 0 && xRes % 20 == 0)
{
var xCoordinate = (float)xRes / (Setting.HeightmapResolution - 1);
var zCoordinate = (float)zRes / (Setting.HeightmapResolution - 1);
Vector3 point = new Vector3(xCoordinate, zCoordinate, height);
TreePoint.Add(point);
}
else
{
if (height == 0 && zRes % 10 == 0 && xRes % 10 == 0)
{
var xCoordinate = (float)xRes / (Setting.HeightmapResolution - 1);
var zCoordinate = (float)zRes / (Setting.HeightmapResolution - 1);
Vector3 point = new Vector3(xCoordinate, zCoordinate, height);
ChopPoint.Add(point);
}
}
}
}
}
}
}
private void Awake()
{
Time.timeScale = 1;
// get the car controller
m_Car = GetComponent <CarController>();
terrain_manager = terrain_manager_game_object.GetComponent <TerrainManager>();
maxVelocity = 150;
acceleration = 1f;
InitializeCSpace();
RRTIterations = 500000; // Limits the number of iterations allowed in the RRT before running the car.
maxTimesFoundGoal = 10; // Limits the times that we find goal before running the car.
maxShortcutAngle = 5; // Angle deviance accepted for points that can give a straight line shortcut.
maxShortcutTheta = 15;
maxDistTarget = 5; // Maximum accepted distance from a target point.
timeStep = 0.05f;
numberOfSteps = 5;
time = 0;
steerDirection = 0;
accelerationDirection = 0;
brake = 0;
handBrake = 0;
carLength = FindCarLength();
world_width = (terrain_manager.myInfo.x_high - terrain_manager.myInfo.x_low);
world_height = (terrain_manager.myInfo.z_high - terrain_manager.myInfo.z_low);
gridSize = Mathf.Min(
world_width / terrain_manager.myInfo.x_N,
world_height / terrain_manager.myInfo.z_N
);
gridAmountX = (int)(world_width / gridSize);
gridAmountZ = (int)(world_height / gridSize);
start = new TreePoint(terrain_manager.myInfo.start_pos, m_Car.transform.eulerAngles.y, 0, 0);
treePoints = new List <TreePoint> [gridAmountX, gridAmountZ];
for (int i = 0; i < gridAmountX; ++i)
{
for (int j = 0; j < gridAmountZ; ++j)
{
if (NearestNeighbourTraversable(i, j))
{
treePoints[i, j] = new List <TreePoint>();
}
}
}
treePoints[NearestNeighbourGetIndexI(start.position.x), NearestNeighbourGetIndexJ(start.position.z)].Add(start);
treeSize = 1;
shortestPointToGoalDistance = Vector3.Distance(start.position, terrain_manager.myInfo.goal_pos);
path = new List <TreePoint>();
pathIndex = 1;
goalFoundAmount = 0;
path = RRT();
if (path == null)
{
Debug.Log("No path found!");
}
nextPoint = path[pathIndex];
crashed = false;
crashTime = 0;
crashCheckTime = 0.5f;
crashDirection = 0;
previousPosition = Vector3.up;
}
//Simulate movement from a point in the tree to a point in the plane.
public TreePoint SimulateMovement(TreePoint from, Vector3 to, int timeFactor)
{
Vector3 position = from.position;
float theta = from.theta;
float velocity = from.velocity;
float cost = from.cost;
for (int step = 0; step < timeFactor * numberOfSteps; step++)
{
//Get steering angle
float delta = m_Car.m_MaximumSteerAngle * SteerInput(position, theta, to);
float braking = 1;
if (AccelerationInput(position, theta, to) < 0)
{
delta = -delta;
}
if (Mathf.Abs(delta) <= 0.4f * m_Car.m_MaximumSteerAngle)
{
delta = 0; //Do we need this?
}
else if (Mathf.Abs(delta) > 0.8f * m_Car.m_MaximumSteerAngle)
{
if (Mathf.Abs(velocity) > maxVelocity / 10)
{
braking = -1;
}
}
/*
* //Calculate motion model values according to kinematic car model
* float xDiff = velocity * Mathf.Sin(Mathf.Deg2Rad * theta);
* float zDiff = velocity * Mathf.Cos(Mathf.Deg2Rad * theta);
* float thetaDiff = velocity / carLength * Mathf.Tan(Mathf.Deg2Rad * delta) * Mathf.Rad2Deg;
*
* //Get new position and orientation using Euler's method
* Vector3 newPosition = new Vector3(Euler(position.x, xDiff, timeStep), 0, Euler(position.z, zDiff, timeStep));
* cost += Vector3.Distance(position, newPosition);
* position = newPosition;
* theta = Euler(theta, thetaDiff, timeStep);
*/
//Get new position and orientation using RK4
float[] nextState = RK4Step(position.x, position.z, theta, delta, carLength, velocity, timeStep);
position.x = nextState[0];
position.z = nextState[1];
theta = nextState[2];
//If collision, this point is not traversable
if (configurationSpace.Collision(position.x, position.z, theta))
{
return(null);
}
//If close enough to end position, stop iterating
if (Vector3.Distance(position, to) <= 0.1f)
{
break;
}
velocity += Mathf.Clamp(AccelerationInput(position, theta, to) * acceleration * timeStep * braking, -maxVelocity, maxVelocity);
}
TreePoint result = new TreePoint(position, theta, velocity, cost);
return(result);
}
public List <TreePoint> RRT()
{
bool foundGoal = false;
TreePoint pathPoint = null;
for (int i = 0; i < RRTIterations; i++)
{
Vector3 randomPoint;
do
{
float probability = foundGoal ? 1 : UnityEngine.Random.Range(0f, 1f);
if (probability < 0.1f)
{
Vector2 p = UnityEngine.Random.insideUnitCircle * shortestPointToGoalDistance;
randomPoint = new Vector3(
p.x,
0,
p.y
);
}
else
{
randomPoint = new Vector3(
UnityEngine.Random.Range(terrain_manager.myInfo.x_low, terrain_manager.myInfo.x_high),
0,
UnityEngine.Random.Range(terrain_manager.myInfo.z_low, terrain_manager.myInfo.z_high)
);
}
}while (configurationSpace.Collision(randomPoint.x, randomPoint.z, 0));
List <TreePoint> nearestPoints = kNearestNeighbours(randomPoint, 1);
TreePoint nearPoint = nearestPoints.Count > 0 ? nearestPoints[0] : null;
if (nearPoint == null)
{
continue;
}
TreePoint newPoint = SimulateMovement(nearPoint, randomPoint, 1);
if (newPoint != null)
{
//Check traversability
int newIIndex = NearestNeighbourGetIndexI(newPoint.position.x);
int newJIndex = NearestNeighbourGetIndexJ(newPoint.position.z);
if (!NearestNeighbourTraversable(newIIndex, newJIndex))
{
continue;
}
nearestPoints = kNearestNeighbours(newPoint.position, Mathf.Min(10 + treeSize / 1000, treeSize));
treePoints[newIIndex, newJIndex].Add(newPoint);
treeSize++;
//RRT*
TreePoint minPoint = nearPoint;
float minCost = newPoint.cost;
foreach (TreePoint point in nearestPoints)
{
TreePoint p = SimulateMovement(point, newPoint.position, 3);
if (p != null)
{
if (Vector3.Distance(p.position, newPoint.position) <= 0.1f && p.cost < minCost)
{
minCost = p.cost;
minPoint = point;
}
}
}
newPoint.parent = minPoint;
minPoint.children.Add(newPoint);
//Update value for sampling heuristic
if (Vector3.Distance(newPoint.position, terrain_manager.myInfo.goal_pos) < shortestPointToGoalDistance)
{
shortestPointToGoalDistance = Vector3.Distance(newPoint.position, terrain_manager.myInfo.goal_pos);
}
//Found goal
if (Vector3.Distance(newPoint.position, terrain_manager.myInfo.goal_pos) <= 5)
{
foundGoal = true;
if (pathPoint == null)
{
pathPoint = newPoint;
}
else if (newPoint.cost < pathPoint.cost)
{
pathPoint = newPoint;
}
goalFoundAmount++;
if (goalFoundAmount >= maxTimesFoundGoal)
{
Debug.Log("Found goal " + maxTimesFoundGoal + " times, stopping.");
break;
}
}
}
}
if (foundGoal)
{
List <TreePoint> goalPath = new List <TreePoint> {
pathPoint
};
while (pathPoint.parent != null)
{
goalPath.Insert(0, pathPoint.parent);
pathPoint = pathPoint.parent;
}
Debug.Log("Reached " + RRTIterations + " RRT-iterations with at least one goal, stopping.");
return(goalPath);
}
Debug.Log("Reached " + RRTIterations + " RRT-iterations but found no goal, stopping.");
return(null);
}
private void FixedUpdate()
{
if (!crashed)
{
time += Time.deltaTime;
if (time >= crashCheckTime && Vector3.Distance(m_Car.transform.position, terrain_manager.myInfo.start_pos) > 5)
{
time = 0;
if (Vector3.Distance(previousPosition, m_Car.transform.position) < 0.1f)
{
crashed = true;
if (Physics.BoxCast(
m_Car.transform.position,
new Vector3(configurationSpace.BoxSize.x / 2, configurationSpace.BoxSize.y / 2, 0.5f),
m_Car.transform.forward,
Quaternion.LookRotation(m_Car.transform.forward),
configurationSpace.BoxSize.z / 2
))
{
crashDirection = -1;
}
else
{
crashDirection = 1;
}
}
else
{
previousPosition = m_Car.transform.position;
}
}
//Improve path by straightening unnecessary curves
if (pathIndex < path.Count - 1)
{
for (int i = path.Count - 1; i >= pathIndex + 1; --i)
{
Vector3 position = m_Car.transform.position;
Vector3 direction = path[i].position - position;
float angle = Vector3.Angle(m_Car.transform.forward, direction);
//If somewhat straight to point on path, check if it is possible to go there
if (angle <= maxShortcutAngle && Quaternion.Angle(Quaternion.Euler(0, path[i].theta, 0), Quaternion.LookRotation(direction)) <= maxShortcutTheta)
{
bool collision = false;
for (float h = 0; h <= 1; h += 0.005f)
{
float x = (position + direction * h).x;
float z = (position + direction * h).z;
if (configurationSpace.Collision(x, z, Quaternion.LookRotation(direction).y))
{
collision = true;
}
}
//No collision, go to this point instead
if (!collision)
{
nextPoint = path[pathIndex];
pathIndex = i;
break;
}
}
}
}
steerDirection = SteerInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, nextPoint.position);
if (Mathf.Abs(steerDirection) < 0.2f)
{
steerDirection = 0;
}
brake = 0;
if (Mathf.Abs(steerDirection) > 0.8f && m_Car.CurrentSpeed > maxVelocity / 10)
{
accelerationDirection = 0;
if (m_Car.CurrentSpeed > maxVelocity / 5)
{
handBrake = 1;
}
else
{
handBrake = 0;
}
}
else
{
accelerationDirection = AccelerationInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, nextPoint.position);
handBrake = 0;
}
if (pathIndex < path.Count - 1)
{
int stepsToCheck = Mathf.Min(3 + (int)(m_Car.CurrentSpeed * 1.6f * 1.6f * m_Car.CurrentSpeed / 500), path.Count - 1 - pathIndex);
for (int i = 1; i <= stepsToCheck; ++i)
{
float steerCheck = SteerInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, path[pathIndex + i].position);
if (Mathf.Abs(steerCheck) > 0.8f && (m_Car.CurrentSpeed * 1.6f * 1.6f * m_Car.CurrentSpeed) >= Vector3.Distance(m_Car.transform.position, path[pathIndex + i].position) * 250 * 0.8f)
{
accelerationDirection = 0;
brake = 1;
break;
}
}
}
if (m_Car.CurrentSpeed >= maxVelocity)
{
accelerationDirection = 0;
}
if (accelerationDirection < 0)
{
m_Car.Move(-steerDirection, brake, accelerationDirection * acceleration, handBrake);
}
else
{
m_Car.Move(steerDirection, accelerationDirection * acceleration, -brake, handBrake);
}
}
else
{
crashTime += Time.deltaTime;
if (crashTime <= 1f)
{
steerDirection = SteerInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, nextPoint.position);
if (crashDirection > 0)
{
m_Car.Move(steerDirection, acceleration, 0, 0);
}
else
{
m_Car.Move(-steerDirection, 0, -acceleration, 0);
}
}
else
{
crashTime = 0;
crashed = false;
}
}
//Update point if close enough to current one
if (Vector3.Distance(m_Car.transform.position, nextPoint.position) <= maxDistTarget + m_Car.CurrentSpeed / 40)
{
pathIndex = Mathf.Min(pathIndex + 1, path.Count - 1);
nextPoint = path[pathIndex];
}
}
