public PathLaneModel(CarTimestamp timestamp, PathRoadModel.LaneEstimate lane) { this.path = lane.path; int dotIndex = lane.paritionID.LastIndexOf('.'); if (dotIndex == -1) { this.laneID = lane.paritionID + ".not a lane"; } else { this.laneID = lane.paritionID.Substring(0, dotIndex); } this.width = lane.width; if (this.width < TahoeParams.T + 2) { this.width = TahoeParams.T + 2; } this.timestamp = timestamp; }
public PathRoadModel GetPathRoadEstimate(SimVehicleState state) { PathRoadModel ret = new PathRoadModel(new List <PathRoadModel.LaneEstimate>(), (double)SimulatorClient.GetCurrentTimestamp); ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position); LinePath path; if (IsPartitionSameDirection(closestPartition, state.Heading)) { path = BuildPath(closestPartition, true, 10, 40); } else { path = BuildPath(closestPartition, false, 10, 40); } Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan) * Matrix3.Translation(-state.Position.X, -state.Position.Y); path.TransformInPlace(absTransform); PathRoadModel.LaneEstimate center = new PathRoadModel.LaneEstimate(path, closestPartition.Lane.Width, closestPartition.PartitionId.ToString()); ret.laneEstimates.Add(center); // get the lane to the left if (closestPartition.Lane.LaneOnLeft != null) { ArbiterLanePartition partition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position); if (closestPartition.NonLaneAdjacentPartitions.Contains(partition)) { if (IsPartitionSameDirection(partition, state.Heading)) { path = BuildPath(partition, true, 10, 25); } else { path = BuildPath(partition, false, 10, 25); } path.TransformInPlace(absTransform); PathRoadModel.LaneEstimate left = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString()); ret.laneEstimates.Add(left); } } if (closestPartition.Lane.LaneOnRight != null) { ArbiterLanePartition partition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position); if (closestPartition.NonLaneAdjacentPartitions.Contains(partition)) { if (IsPartitionSameDirection(partition, state.Heading)) { path = BuildPath(partition, true, 10, 25); } else { path = BuildPath(partition, false, 10, 25); } path.TransformInPlace(absTransform); PathRoadModel.LaneEstimate right = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString()); ret.laneEstimates.Add(right); } } return(ret); }