internal virtual void Accelerate(DriveCtx dctx)
{
if (dctx.iFrontHeadWay >= 2 && dctx.Params.iSpeed < this.iDesiredSpeed)
{
dctx.Params.iSpeed += dctx.iAcceleration;
}
}
/// <summary>
/// Prudent .neither aggressive nor slow
/// </summary>
/// <param name="crc"></param>
internal virtual void NormalRun(DriveCtx crc)
{
//if (crc.iEntityGap > crc.Out.iSpeed)
//{
crc.Params.iMoveY = crc.Params.iSpeed;
//}
//else
//{
// crc.Out.iMoveStepY
//}
}
/// <summary>
/// 加速行驶
/// </summary>
/// <param name="crx"></param>
internal virtual void Accelerate(DriveCtx crx)
{
if (crx.iFrontHeadWay > 2 * crx.Params.iSpeed) //保证了加速
{
if (crx.Params.iSpeed < this.iDesiredSpeed)
{
//crx.iAcceleration += 1;
crx.Params.iSpeed += crx.iAcceleration;
//crx.iAcceleration = 1;
}
} //否则不加速
}
internal virtual void Decelerate(DriveCtx dctx)
{
//交通规则限制减速
if (dctx.Params.iSpeed > this.iDesiredSpeed)
{
dctx.Params.iSpeed -= dctx.iAcceleration;
}
//空间限制而减速
if (dctx.iFrontHeadWay < dctx.Params.iSpeed)
{
dctx.Params.iSpeed = dctx.iFrontHeadWay; //-crc.iSaftySpace
}
}
internal virtual void DriveMobile(MobileOBJ mobile, DriveCtx dctx)
{
var currLane = mobile.Container as Lane;
var currWay = currLane.Container as Way;
if (dctx.IsReachEnd == true)
{
currLane.Mobiles.RemoveFirst();
return;
}
//Before updating mobiles status,update that on roadNode
//the following four funtions are in charge of making decisions,not executing
this.LaneChanging(dctx); //换道
this.Accelerate(dctx); //符合条件就加速
this.Decelerate(dctx); //否则减速
this.NormalRun(dctx); //更新位置
//To execute params setted hereinbefore
// if a mobile stops ,it must be blocked by traffic light or a mobile ahead
if (dctx.Params.iMoveY == 0)
{
return;
}
//还在路段内部
if (dctx.Params.iMoveY <= dctx.iLaneGap)
{
mobile.Move(dctx.Params.iMoveY);
mobile.iSpeed = dctx.Params.iSpeed;
mobile.iAcceleration = dctx.Params.iAcceleration;
}
else //进入了交叉口
{
//原有的车道删除该车辆
currLane.Mobiles.RemoveFirst();
//进入交叉口
mobile.Container = currWay.To;
//calculate steps to move in a xnode
int iXNodeMoveStep = dctx.Params.iMoveY - dctx.iLaneGap;
//recalculate moblie shape position
mobile.Move(iXNodeMoveStep);
//进入交叉口的等待队列
currWay.To.MobilesInn.Enqueue(mobile);
}
mobile.iAcceleration = Math.Max(dctx.Params.iAcceleration, 1);
mobile.iSpeed = dctx.Params.iSpeed;
}
/// <summary>
/// 匀速行驶
/// </summary>
/// <param name="crx"></param>
internal virtual void NormalRun(DriveCtx crx)
{
if (crx.Params.iSpeed + crx.iSafeHeadWay <= crx.iFrontHeadWay)
{
crx.Params.iMoveY = crx.Params.iSpeed;
}
else //小于速度个车头时距
{
crx.Params.iMoveY = crx.Params.iSpeed - crx.iSafeHeadWay;
}
//确保前进的距离不是负值
crx.Params.iMoveY = Math.Max(0, crx.Params.iMoveY);
}
internal virtual void DriveMobile(MobileOBJ mobile, DriveCtx dctx)
{
var currNode = mobile.Container as XNode;
//控制权转移出去了
this.LaneChanging(dctx); //换道
this.Accelerate(dctx); //符合条件就加速
this.Decelerate(dctx); //否则减速
this.NormalRun(dctx); //更新位置
if (mobile.ID == 2)
{
;
}
//still runing within a xnode
if (dctx.Params.iMoveY <= dctx.iXNodeGap && dctx.iXNodeGap > 0)
{
//modify a mobiles's position
mobile.Move(dctx.Params.iMoveY);
}
else //enter a lane from its container xnode
{
if (dctx.Params.iMoveY > dctx.iFrontHeadWay)
{
ThrowHelper.ThrowArgumentException("前进距离大于车头时距会导致撞车");
}
currNode.Mobiles.Remove(mobile);
int iXNodeStep = dctx.Params.iMoveY - dctx.iXNodeGap;
var toLane = mobile.Track.ToLane;
if (toLane != null)
{
toLane.MobilesInn.Enqueue(mobile);
//tempraryly modify mobile to get prepareed for moving
//mobile.Shape.Start = toLane.Shape.Start;//bug here to modified in the future
mobile.Container = toLane; //a moible cross a lane and a xnode since it has a ilength
mobile.Move(iXNodeStep);
}
}
mobile.iAcceleration = Math.Max(dctx.Params.iAcceleration, 1);
mobile.iSpeed = dctx.Params.iSpeed;
}
/// <summary>
/// 减速行驶
/// </summary>
/// <param name="crx"></param>
internal virtual void Decelerate(DriveCtx crx)
{
//大于期望车速减速
if (crx.Params.iSpeed > this.iDesiredSpeed)
{
crx.Params.iSpeed -= crx.iAcceleration;
}
///危险减速或者是前进距离过大减速
if (crx.Params.iSpeed > crx.iFrontHeadWay + crx.iSafeHeadWay) //可能有bug
{
crx.Params.iSpeed = crx.iFrontHeadWay - crx.iSafeHeadWay;
}
if (crx.dRandom < crx.dModerationRatio && crx.Params.iSpeed > 1) //随机漫化
{
crx.Params.iSpeed -= crx.iAcceleration;
}
//确保车速不小于零
crx.Params.iSpeed = Math.Max(crx.Params.iSpeed, 0);
}
/// <summary>
/// To make a mobile on a crossing chang its drive track
/// </summary>
/// <param name="dctx"></param>
internal virtual void LaneChanging(DriveCtx dctx)
{
}
///////////////////////////////////////////////////
internal DriveCtx Observe(StaticOBJ driveEn, MobileOBJ mobile)
{
DriveCtx dctx = new DriveCtx(driveEn);
dctx.iAcceleration = mobile.iAcceleration;
dctx.iSpeed = mobile.iSpeed;
switch (driveEn.EntityType)
{
// calculate headway on the left/right/current lane of current mobile
case EntityType.Lane:
var currLane = mobile.Container as Lane;
var currWay = currLane.Container as Way;
//current mobile
int iCurrStart = currLane.Shape.GetIndex(mobile.Shape.Start);
//current mobile
int iCurrEnd = currLane.Shape.GetIndex(mobile.Shape.End);
// int iLaneGap = currLane.iLength-iCurrentStart;
if (mobile.Front != null)
{
//behiand 14 .front 15. iLaneGap need to reduce 1
dctx.iLaneGap = currLane.Shape.GetIndex(mobile.Front.Shape.End) - iCurrStart - 1;
dctx.iFrontSpeed = mobile.Front.iSpeed;
dctx.iXNodeGap = 0;
dctx.iFrontHeadWay = dctx.iLaneGap + dctx.iXNodeGap;
}
//front mobile is null, current mobile is the first one on this lane
//the first mobile needs to deal with a traffic light or/and a crossing(XNode)
else //this.FrontMobile==null)
//front mobile,there's a signal light playing on the lane
//deal with that signal light
{
if (currLane.IsBlocked == true)
{
dctx.iFrontHeadWay = currLane.Length - iCurrStart;
}
//deal with that crossing
else //the current mobile is the first one to deal with a crossing
//先计算车辆的轨迹，where a mobile is heading for. right .left or straight forward
{
int iXNodeGap = 0;
int iLaneGap = currLane.Length - 1 - iCurrStart;
if (iLaneGap <= 3 * mobile.iSpeed) //space si more than triple car speed.
{
mobile.Track.Update();
if (mobile.Track.ToLane != null)
{
//class acts as parameters will pass its address to functions,so clone is used here
var temp = currLane.Shape.End;
// if (this.ID == 2) {
// ;
// }
//再计算剩余轨迹
GetXNodeGap(currWay.To, temp, out iXNodeGap, mobile);
}
else //toLane == null. reach destination
{
dctx.IsReachEnd = true;
iXNodeGap = 10; //to let the first car go away
iLaneGap = 0;
}
}
dctx.iXNodeGap = iXNodeGap;
dctx.iLaneGap = iLaneGap;
dctx.iFrontHeadWay = iXNodeGap + iLaneGap;
dctx.iFrontSpeed = -1;
}
}
//rear mobile
dctx.iRearHeadWay = iCurrStart;
if (mobile.Rear != null)
{
dctx.iRearHeadWay = iCurrEnd - currLane.Shape.GetIndex(mobile.Rear.Shape.Start);
dctx.iRearSpeed = mobile.Rear.iSpeed;
}
else //rear mobile is empty
{
dctx.iRearHeadWay = iCurrStart; //rear mobiel
dctx.iRightRearSpeed = -1;
}
//get dirving context on the left lane
this.GetSidesContext(currLane.Left, LaneType.Left, iCurrStart, iCurrEnd, ref dctx);
//get dirving context on the right lane
this.GetSidesContext(currLane.Right, LaneType.Right, iCurrStart, iCurrEnd, ref dctx);
break;
case EntityType.XNode:
var xnode = driveEn as XNode;
int iLaneEnGap = 0;
int iXNodeEnGap = 0;
//计算剩余轨迹数量//如果pcurrPos没到头，iXnodeGap等于零
//when a mobile is on a xnode .its headway is xnodeGap for a secend mobile
//the frist mobile bIsBlocked is never true;while its following may be true
var bIsBlocked = this.GetXNodeGap(xnode, mobile.Track.Current, out iXNodeEnGap, mobile);
if (bIsBlocked == false) //the first mobile
{
var toLane = mobile.Track.ToLane;
//计算车道上的长度
if (toLane != null) //no destination lane means a mobile has reach its destnation.
{
if (toLane.MobilesInn.Count > 0) //theres already mobiles waiting to enter tolane.
{
iLaneEnGap = 0;
}
else
{
var lastMobile = toLane.Mobiles.Last;
if (lastMobile != null)
{
iLaneEnGap = toLane.Shape.GetIndex(lastMobile.Value.Shape.End);
}
else //no mobiles running at tolane
{
iLaneEnGap = toLane.Length;
}
}
}
}
else //the a mobile blocked by its previous mobile on a xnode
{
iLaneEnGap = 0;
}
dctx.iLaneGap = iLaneEnGap;
dctx.iXNodeGap = iXNodeEnGap;
dctx.iFrontHeadWay = iXNodeEnGap + iLaneEnGap;
break;
case EntityType.Way:
throw new NotImplementedException("不应该传入这个参数，应在在车道上，或者是交叉口上");
break;
default: break;
}
return(dctx);
}
/// <summary>
/// get left and right driving context
/// </summary>
/// <param name="lane">lane</param>
/// <param name="lanetype">lanetype of current lane</param>
/// <param name="iCurrentStart">headway of current mobile index</param>
/// <param name="iCurrentEnd">rear of current mobile index</param>
/// <param name="dc">out parameters</param>
private void GetSidesContext(Lane lane, LaneType lanetype, int iCurrentStart, int iCurrentEnd, ref DriveCtx dc)
{
//to make sure current lane got a lefe lane
if (lane == null)
{
return;
}
//headway on the lane
int iFrontHeadWay = -1;
int iRearHeadWay = -1;
int iFrontSpeed = -1;
int iRearSpeed = -1;
//there's no mobile on lane
if (lane.Mobiles.Count > 0)
{
//there's mobiles on lane
int iLeastGap = lane.Length;
int iTempGap = iLeastGap;
MobileOBJ mobile = null;
//loop to find two adjacent mobiles on the lane.one rear,one ahead of the current mobile
foreach (var element in lane.Mobiles)
{
iTempGap = lane.Shape.GetIndex(element.Shape.End) - iCurrentStart;
//make it positive,to find the nearest mobile on the left lane
if (Math.Abs(iTempGap) < Math.Abs(iLeastGap))
{
iLeastGap = iTempGap;
mobile = element;
}
}
//nearest mobile on the left is at the front
if (iLeastGap >= 0)
{
iFrontHeadWay = iLeastGap;
iFrontSpeed = mobile.iSpeed;
var rearMobile = mobile.Rear;
if (rearMobile != null)
{
iRearHeadWay = iCurrentEnd - lane.Shape.GetIndex(rearMobile.Shape.Start);
iRearSpeed = rearMobile.iSpeed;
}
} //nearest mobile on the left is at the behind
else
{
//make it postive
iRearHeadWay = Math.Abs(iLeastGap);
iRearSpeed = mobile.iSpeed;
var frontMobile = mobile.Front;
if (frontMobile != null)
{
iFrontHeadWay = lane.Shape.GetIndex(frontMobile.Shape.End) - iCurrentStart;
iFrontSpeed = mobile.Front.iSpeed;
}
}
}
else
{
iFrontHeadWay = lane.Length - iCurrentStart;
iRearHeadWay = iCurrentEnd;
}
//make driving observation true
switch (lanetype)
{
case LaneType.Right:
dc.iRightFrontHeadWay = iFrontHeadWay;
dc.iRightFrontSpeed = iFrontSpeed;
dc.iRightRearHeadWay = iRearHeadWay;
dc.iRightRearSpeed = iRearSpeed;
break;
case LaneType.Left:
dc.iLeftFrontHeadWay = iFrontHeadWay;
dc.iLeftFrontSpeed = iFrontSpeed;
dc.iLeftRearHeadWay = iRearHeadWay;
dc.iLeftRearSpeed = iRearSpeed;
break;
default:
throw new ArgumentException("parameter lanetype error!");
break;
}
}
