/// <summary>
/// Updates the vehicle state as well as other values
/// </summary>
/// <param name="vehicleUpdate"></param>
public void Update(SceneEstimatorTrackedCluster update)
{
// update state
this.Observed = update;
// calculate other values such as involved areas
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="vehicleId"></param>
public VehicleAgent(SceneEstimatorTrackedCluster ov)
{
this.VehicleId = ov.id;
this.StateMonitor = new StateMonitor();
this.QueuingState = new QueuingMonitor();
this.UpdateState(ov);
this.BirthTimer();
}
public VehicleAgent(bool temp, bool temp2)
{
this.VehicleId = 5000;
SceneEstimatorTrackedCluster setc = new SceneEstimatorTrackedCluster();
setc.isStopped = true;
setc.relativePoints = new Coordinates[0];
this.StateMonitor = new StateMonitor();
this.QueuingState = new QueuingMonitor();
this.BirthTimer();
this.StateMonitor.Observed = setc;
}
/// <summary>
/// Updates the vehicle's monitor
/// </summary>
/// <param name="vehicleUpdate"></param>
/// <param name="speed"></param>
public void UpdateState(SceneEstimatorTrackedCluster update)
{
this.StateMonitor.Update(update);
}
/// <summary>
/// Get the vehicles from the world and put them into sensors form
/// </summary>
/// <param name="ours"></param>
/// <returns></returns>
public SceneEstimatorTrackedClusterCollection VehiclesFromWorld(SimVehicleId ours, double ts)
{
// vehicle list
List<SceneEstimatorTrackedCluster> vehicles = new List<SceneEstimatorTrackedCluster>();
// get our vehicle
SimVehicleState ourVS = null;
foreach (SimVehicleState svs in this.worldState.Vehicles.Values)
{
if (svs.VehicleID.Equals(ours))
{
ourVS = svs;
}
}
// generate "tracked" clusters
foreach (SimVehicleState svs in this.worldState.Vehicles.Values)
{
// don't inclue our vehicle
if (!svs.VehicleID.Equals(ours))
{
// construct cluster
SceneEstimatorTrackedCluster setc = new SceneEstimatorTrackedCluster();
// set heading valid
setc.headingValid = true;
// closest point
Coordinates closestPoint = this.ClosestToPolygon(this.VehiclePolygon(svs), ourVS.Position);
setc.closestPoint = closestPoint;
// stopped
bool isStopped = Math.Abs(svs.Speed) < 0.01;
setc.isStopped = isStopped;
// speed
float speed = (float)Math.Abs(svs.Speed);
setc.speed = speed;
setc.speedValid = svs.SpeedValid;
// absolute heading
float absHeading = (float)(svs.Heading.ArcTan);
setc.absoluteHeading = absHeading;
// relative heading
float relHeading = absHeading - (float)(ourVS.Heading.ArcTan);
setc.relativeheading = relHeading;
// set target class
setc.targetClass = SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE;
// set id
setc.id = svs.VehicleID.Number;
// cluster partitions
SceneEstimatorClusterPartition[] partitions = this.GetClusterPartitions(svs, ourVS);
setc.closestPartitions = partitions;
// state
setc.statusFlag = SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;
// raw points
Coordinates[] points = this.VehiclePointsRelative(svs, ourVS.Position, ourVS.Heading);
setc.relativePoints = points;
// add
vehicles.Add(setc);
}
}
// array of clusters
SceneEstimatorTrackedClusterCollection setcc = new SceneEstimatorTrackedClusterCollection();
setcc.clusters = vehicles.ToArray();
setcc.timestamp = ts;
// return the clusters
return setcc;
}
public SensedVehicleDisplay(SceneEstimatorTrackedCluster trackedCluster)
{
this.trackedCluster = trackedCluster;
}