/// <summary> /// Calculates vehicle's rectangular center due to separation requirements and back axel position /// </summary> /// <param name="svs">Vehicle</param> /// <param name="extraForwardSpace">Extra space needed in the front</param> /// <returns></returns> public static Coordinates recalculateCenter(ObservedVehicle svs, double extraForwardSpace) { //double addedLength = (2 * svs.Length + extraForwardSpace + 1) / 2 - svs.PositionOffsetFromRear - 1; //return (svs.AbsolutePosition + new Coordinates(addedLength * Math.Cos(svs.Heading.ToDegrees() * Math.PI / 180), // addedLength * Math.Sin(svs.Heading.ToDegrees() * Math.PI / 180))); return(new Coordinates()); }
public ObservedVehicleDisplay(ObservedVehicle observedVehicle, Color c) { // set the vehicle this.observedVehicle = observedVehicle; color = c; bodyRect = new RectangleF(-Width / 2, -RearOffset, Width, Length); wheelRectL = RectangleF.FromLTRB(-tireWidth, -tireDiameter / 2, 0, tireDiameter / 2); wheelRectR = RectangleF.FromLTRB(0, -tireDiameter / 2, tireWidth, tireDiameter / 2); }
/// <summary> /// Full Constructor /// </summary> /// <param name="Position"></param> /// <param name="velocity"></param> public ObservedVehicleDisplay(ObservedVehicle observedVehicle) { // set the vehicle this.observedVehicle = observedVehicle; // set the vehicle's color if (observedVehicle.ObservationState == ObservedVehicleState.Normal) { color = Color.Green; } else if (observedVehicle.ObservationState == ObservedVehicleState.Occluded) { color = Color.Red; } else { color = Color.Black; } bodyRect = new RectangleF(-Width / 2, -RearOffset, Width, Length); wheelRectL = RectangleF.FromLTRB(-tireWidth, -tireDiameter / 2, 0, tireDiameter / 2); wheelRectR = RectangleF.FromLTRB(0, -tireDiameter / 2, tireWidth, tireDiameter / 2); }
/// <summary> /// Constructor /// </summary> /// <param name="observed"></param> /// <param name="vehicleLocation"></param> public VehicleInformation(ObservedVehicle observed, RndfLocation vehicleLocation) { this.Observed = observed; this.VehicleLocation = vehicleLocation; }
/// <summary> /// Checks if a specific vehicle is within generous vehicle-like area of an exit /// </summary> /// <param name="observedVehicle"></param> /// <param name="stop"></param> /// <returns></returns> public static bool CheckVehicleAtExit(ObservedVehicle observedVehicle, RndfWayPoint exit) { throw new Exception("This method is not yet implemented"); }
/// <summary> /// Checks if a specific vehicle is within the intersection itself /// </summary> /// <param name="stops"></param> /// <param name="intersectionPolygon"></param> /// <returns></returns> public static bool CheckVehicleWithinIntersection(ObservedVehicle observedVehicle, List <BoundaryLine> intersectionPolygon) { return(CheckVehicleInPolygon(observedVehicle, 1, intersectionPolygon, 0)); }
/// <summary> /// Determines if a vehicle is contained inside a polygon /// With a distance epsilon allowed for error in our polygon bounds (i.e. shrink polygon by epsilon) /// With a factor portion of the vehicle's total area needed to be inside bounds to return true /// </summary> /// <param name="vehicle">Vehicle in question</param> /// <param name="polygon">Polygon to check the vehicle against</param> /// <param name="portion">Portion of the area (greater than 0 less than or equal to 1) that needs to be inside polygon to return true</param> /// <param name="epsilon">Epsilon in meters to shrink the polygon by</param> /// <returns></returns> /// <remarks>Careful of vehicle with 0 width of length. Just use position in that case</remarks> /// <remarks>Assuming that a given boundary line shares one coordinate with the next boundary line in the list</remarks> public static bool CheckVehicleInPolygon(ObservedVehicle vehicle, double portion, List <BoundaryLine> polygon, double epsilon) { /*//resize polygon * //find center * Coordinates center = new Coordinates(0f, 0f); * foreach (BoundaryLine bl in polygon) * { * center += bl.p1 + bl.p2; * } * * //since each point is present exactly twice in the boundary line representation * center.X = center.X / 2 * polygon.Count; * center.Y = center.Y / 2 * polygon.Count; * * //adjust polygon and check for line segment intersections with the vehicle, maintaining a list for such intersections * Coordinates vehicleCenter = recalculateCenter(vehicle, 0); * Rectangle vehicleRect = calculateRectangle(vehicleCenter, vehicle.Width, vehicle.Length, vehicle.Heading.ToDegrees() * Math.PI / 180); * List<Coordinates> vehicleHits = new List<Coordinates>(); * * foreach (BoundaryLine bl in polygon) * { * bl.p1 = resizeCoordinate(center, bl.p1, epsilon); * bl.p2 = resizeCoordinate(center, bl.p2, epsilon); * Coordinates? up = LineIntersectsLine(vehicleRect.upperLeft, vehicleRect.upperRight, bl.p1, bl.p2); * Coordinates? down = LineIntersectsLine(vehicleRect.lowerLeft, vehicleRect.lowerRight, bl.p1, bl.p2); * Coordinates? left = LineIntersectsLine(vehicleRect.upperLeft, vehicleRect.lowerLeft, bl.p1, bl.p2); * Coordinates? right = LineIntersectsLine(vehicleRect.upperRight, vehicleRect.lowerRight, bl.p1, bl.p2); * if (up != null) vehicleHits.Add((Coordinates)up); * if (down != null) vehicleHits.Add((Coordinates)down); * if (left != null) vehicleHits.Add((Coordinates)left); * if (right != null) vehicleHits.Add((Coordinates)right); * } * * if (vehicleHits.Count == 0) * { * //vehicle is either entirely inside or entirely outside the polygon * foreach (BoundaryLine bl in polygon) * { * if (LineIntersectsLine(center, vehicleRect.lowerLeft, bl.p1, bl.p2) != null) return false; * } * return true; * } * * bool bUpperLeft = false; bool bUpperRight = false; bool bLowerLeft = false; bool bLowerRight = false; * foreach (BoundaryLine bl in polygon) * { * if (LineIntersectsLine(center, vehicleRect.lowerLeft, bl.p1, bl.p2) != null) bLowerLeft = true; * if (LineIntersectsLine(center, vehicleRect.lowerRight, bl.p1, bl.p2) != null) bLowerRight = true; * if (LineIntersectsLine(center, vehicleRect.upperLeft, bl.p1, bl.p2) != null) bUpperLeft = true; * if (LineIntersectsLine(center, vehicleRect.upperRight, bl.p1, bl.p2) != null) bUpperRight = true; * } * * if (bLowerLeft) vehicleHits.Add(vehicleRect.lowerLeft); * if (bLowerRight) vehicleHits.Add(vehicleRect.lowerRight); * if (bUpperLeft) vehicleHits.Add(vehicleRect.upperLeft); * if (bUpperRight) vehicleHits.Add(vehicleRect.upperRight); * * List<BoundaryLine> vehiclePolygon = JarvisMarch(vehicleHits); * * //need to calculate the area of the vehicle inside the polygon * double area = 0; * * foreach (BoundaryLine bl in vehiclePolygon) * { * area += TriangleArea(vehicleCenter, bl.p1, bl.p2); * } * * if ((area / (vehicle.Length * vehicle.Width)) > portion) return true; * return false;*/ return(false); }