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