void TurnToNode(TrafficNode node)
{
Quaternion myRotation = transform.rotation;
Vector3 direction = (node.transform.position - transform.position).normalized;
transform.rotation.SetFromToRotation(transform.position, direction);
}
TrafficNode chooseDestination(TrafficNode node)
{
int index = Random.Range(0, node.nextNodes.Count);
Debug.Log(gameObject + " is going to " + node.nextNodes[index] + " next");
return(node.nextNodes[index]);
}
// Merge the two nodes together, provided that their positions are
// close enough together
public static TrafficNode merge(TrafficNode node1, TrafficNode node2,
HashSet <TrafficNode> node_set,
float eps = 1.0e-9f)
{
if ((node1.position - node2.position).magnitude > eps)
{
throw new System.ArgumentException(
"The provided nodes are not close enough " +
"to each other to be merged");
}
TrafficNode merged = TrafficNode.from_pos(
(node1.position + node2.position) * 0.5f);
merged.connected_to.UnionWith(node1.connected_to);
merged.connected_to.UnionWith(node2.connected_to);
// Remove the original nodes
merged.connected_to.Remove(node1);
merged.connected_to.Remove(node2);
// Run through the adjacency list and replace all references to the
// original nodes with a reference to the new merged node
foreach (TrafficNode neighbor in merged.connected_to)
{
neighbor.connected_to.Remove(node1);
neighbor.connected_to.Remove(node2);
neighbor.connected_to.Add(merged);
}
node_set.Remove(node1);
node_set.Remove(node2);
node_set.Add(merged);
return(merged);
}
public static TrafficNode from_pos(Vector3 pos)
{
TrafficNode to_return = new TrafficNode();
to_return.position = pos;
to_return.connected_to = new HashSet <TrafficNode>();
return(to_return);
}
// Update is called once per frame
void Update()
{
if (DistanceToNode(currentNode) > minDist)
{
transform.LookAt(currentNode.transform.position);
DriveToNode(currentNode);
}
else
{
currentNode = GetNextNode(currentNode);
}
}
public static GLOSAResult ProjectedLaneForManeuver(MapData map, List <GPSLocation> gpsHistory, double?deviceHeading, ulong maneuver)
{
List <TrafficNode> trafficNodePositions = ExtractTrafficNodesFromMAP(map, maneuver);
List <GPSLocation> gpsLocations = gpsHistory;
GPSLocation location = gpsLocations.First();
var sortedList = trafficNodePositions.
OrderBy(m => Distance.CalculateDistanceBetween2PointsKMs(
m.GPSLocation.Latitude,
m.GPSLocation.Longitude,
location.Latitude, location.Longitude)).ToList();
TrafficNode nearestNode = sortedList[0];
MapDataIntersectionsIntersectionGeometryGenericLane lane = null;
MapDataIntersectionsIntersectionGeometryGenericLane[] lanes = map.intersections.IntersectionGeometry.laneSet;
var possibleLanes = lanes.Where(genricLane => genricLane.laneID.ToString() == nearestNode.ID);
if (possibleLanes.Count() > 0)
{
lane = possibleLanes.First();
}
// Verify the lane is in the same direction as the vehicle
var nodes = ExtractTrafficNodesFromLane(lane);
// Let's sort all lane nodes from MAP Ref Point
var refPoint = map.intersections.IntersectionGeometry.refPoint;
var mapLocation = new GPSLocation()
{
Latitude = refPoint.lat / Constants.MAPCoordinateIntConverterUnit,
Longitude = refPoint.@long / Constants.MAPCoordinateIntConverterUnit,
};
// Sort the nodes by distance ascending
var sortedNodes = nodes.OrderBy(node => Distance.CalculateDistanceBetween2PointsKMs(node.GPSLocation.Latitude, node.GPSLocation.Longitude, mapLocation.Latitude, mapLocation.Longitude)).ToList();
GLOSAResult result = IsDirectionOfVehicleInSameDirectionAsLane(map.intersections.IntersectionGeometry.id.id, sortedNodes, deviceHeading, gpsHistory, 50, maneuver);
result.Description = $"LaneId: {lane.laneID}, {result.Description}";
result.Object = lane;
return(result);
}
void OnCollisionEnter(Collision collision)
{
if (collision.gameObject.CompareTag("Car"))
{
TrafficNode trafficNode = TrafficSystem.GetTrafficNode(trafficNodeId);
CarController car = collision.gameObject.GetComponent <CarController>();
if (!trafficNode.HasThreeNodesConnected())
{
car.DecideWith3StreetsConnected();
}
else if (TrafficSystem.GetTrafficNode(trafficNodeId).HasTwoNodesConnected())
{
car.DecideWith2StreetsConnected(trafficNode.left != TrafficNode.NO_ROAD_CONNECTED ||
trafficNode.front != TrafficNode.NO_ROAD_CONNECTED);
}
countOfCarsInLane++;
}
}
public static List <TrafficNode> ExtractTrafficNodesFromLane(MapDataIntersectionsIntersectionGeometryGenericLane lane)
{
List <TrafficNode> trafficNodePositions = new List <TrafficNode>();
foreach (var node in lane.nodeList.nodes)
{
TrafficNode trafficNode = new TrafficNode()
{
GPSLocation = new GPSLocation()
{
Latitude = node.delta.nodeLatLon.lat / Constants.MAPCoordinateIntConverterUnit,
Longitude = node.delta.nodeLatLon.lon / Constants.MAPCoordinateIntConverterUnit,
},
ID = lane.laneID.ToString(),
};
trafficNodePositions.Add(trafficNode);
}
return(trafficNodePositions);
}
private List <MapNode> GetPath(Point source, Point destination)
{
var start = _trafficNodes.OrderBy(p => GetDistance(p.Location, source)).First();
var end = _trafficNodes.OrderBy(p => GetDistance(p.Location, destination)).First();
// breath first search of potential nodes
var searched = new List <TrafficNode>();
var potentials = new Queue <TrafficNode>(new[] { start });
TrafficNode current = null;
do
{
current = potentials.Dequeue();
var children = current.Connections.Select(i => _trafficNodes[i]);
foreach (var c in children)
{
if (searched.Contains(c))
{
continue;
}
c.Previous = current;
searched.Add(c);
potentials.Enqueue(c);
}
}while (potentials.Any() && current != end);
// backwards iterate to find the shortest path
var path = new List <MapNode>(new[] { current });
while (current.Previous != null && !path.Contains(current.Previous))
{
path.Insert(0, current.Previous);
current = current.Previous;
}
path.OrderBy(p => GetDistance(p.Location, destination));
path.Add(new TrafficNode(destination.X, destination.Y));
return(path);
}
TrafficNode GetNextNode(TrafficNode node)
{
List <Transform> nextPossibleNodes = new List <Transform>(node.connectedNodes);
nextPossibleNodes.Remove(previousNode.transform);
int randomNodeIndex = Random.Range(0, nextPossibleNodes.ToArray().Length);
Debug.Log(randomNodeIndex);
TrafficNode nextNode = nextPossibleNodes[randomNodeIndex].GetComponent <TrafficNode>();
previousNode = node;
if (nextNode != null)
{
return(nextNode);
}
else
{
Debug.LogWarning("No next node found continuing on old node");
return(previousNode);
}
}
public void decideWhatNext()
{
previousNode = nextNode;
nextNode = chooseDestination(nextNode);
StartCoroutine("driveToNextNode");
}
float DistanceToNode(TrafficNode node)
{
float distance = Vector3.Distance(new Vector3(transform.position.x, 0, transform.position.z), new Vector3(node.transform.position.x, 0, node.transform.position.z));
return(distance);
}
void DriveToNode(TrafficNode node)
{
transform.Translate(Vector3.forward * CurrentSpeed * Time.deltaTime);
//rigidBody.AddForce(transform.forward * CurrentSpeed);
//rigidBody.velocity += (transform.forward * CurrentSpeed);
}
public static void link(TrafficNode node1, TrafficNode node2)
{
node1.connected_to.Add(node2);
node2.connected_to.Add(node1);
}
