Navigation systems

Navigation towards points

Precomputed nav data
A set of waypoints(3D coordinates, radius, flags) reachable by the bot and a set of connections(WptFrom, WptTo, Flags) between waypoints describing paths free from static obstacles.
The waypoint radius, as I have seen it in several implementations, may have different meaning
- the maximum error allowed to consider that the waypoint is reached,
- the radius of the largest free-move ( no obstacles) circle around the waypoint,
- the radius of the largest circle around the waypoint that can be reached from any other connected waypoint circle without bumping into obstacles.
I consider here that the first definition applies.

Applicable guidance control law:
Moving towards a destination point

Switching intermediate destinations
The current destination is discarded for the next one whenever the distance between the mobile and its current destination is less than a pre-determined radius ( the destination waypoint radius or a generic value).

Comments
- this mode does not guarantee that the mobile will follow a path defined by a sequence of destinations. If for any reason ( player avoidance, sidewind, side blow, gust...) a mobile is pushed sideways from the path, it will ignore the resulting lateral deviation from the reference ( ideal) path, moving straight to its destination. It may then bump into obstacles the reference path was meant to help avoiding.
- for mobiles that cannot turn at null speed ( cars, airplanes), having a minimum turn radius creates problem when there is no circle of radius>= minimum radius tangential to the starting yaw and passing through the starting position and the destination point. A first phase to reach such a configuration must be implemented ( eg: rallying a circle) before switching to a move towards waypoint.

Note:The "PodBot" bot introduces random variations at switching time on the actual destination waypoint based on its radius to have less predictable trajectories.

The following 2 screenshots shows the basic implementation
The bot is only allowed to switch segments when its center is within the segments "corridors": the blue corridor is the one of the previous segment, the green one the current. Most corridors widths are extremely small (+- 2 "units") just for the purpose of the demonstrations.
It is obvious that the bot, running at full speed, is not able at all to stay in the corridors, overshoots a lot during turns, which makes it bumpheavily into obstacles.



Implementing speed control ( to put it simply: speed proportional to the distance to the next segment), turns are better but the bot still cannot follow the paths, staying away from their centerline.




I did not try to improve speed direction control by introducing some prediction, as "Navigation along segments" as follows was what I was looking for.

Navigation along segments

Precomputed nav data
Type1: A set of waypoints(3D coordinates, flags) reachable by the bot and a set of Path segments(WptFrom, WptTo, Width, Flags) between waypoints describing corridors free from static obstacles.
Type2: A set of segments(Start, End, Width, Flags) describing corridors free from static obstacles and a set of segments pairings telling those which cross.
A radius attached to a waypoint might be needed in some cases, but it is the Width of the corridors that is here the basic key parameter.
Applicable guidance control law:
Rallying & moving along a directed line, defined by the source and destination waypoints of the current path segment.
InterceptAngMax can be computed as InterceptAngMax= 0.5*abs( wrap180(nextpathsegmentCourse- currentpathsegemntCourse));
Switching intermediate destinations
The current destination/path segment is discarded for the next one whenever the distance between the mobile and the next path segment supporting line is less than this next path segment Width.
For a bot to be able to patrol to and fro between 2 waypoints, a supplementary condition on the distance to the current destination waypoint must obviously be introduced.

With the Half-life engine model
The pseudo_code used for the Half-life engine model is:

// current segment = PQ; next segment = QS;
// max allowed velocity = UE_mVelMax;
// at init: IcptAngMax= INTERCEPTANGMAX

PredCoeff= ( -T+ 1/ Friction);/*Half-life specific computation*/
vDeltaPredh= PredCoeff* vVel;
PredPos= M+ vDeltaPredh;

XTD= crossproduct( uvPQ, vQM);
//
// switching condition check
bDoSwitch = TRUE;
while( bDoSwitch AND (next segment exists) )
{
XTDNext= crossproduct( uvQS, vSM);
bDoSwitch= abs( XTDNext)< Width( nextsegment);
if( bDoSwitch== TRUE)
{
IcptAngMax= 0.5*abs( wrap180(nextpathsegmentCourse- currentpathsegmentCourse));
currsegment= nextsegment;//P= Q; Q= S
nextsegment= next of currsegment;//S= ..
XTD=XTDNext;
}
}
//
if( no next segment)
{
moveTowardsFinalDest();//not detailed here
}
else
{
XTDPred= XTD+ crossproduct( uvPQ, vDeltaPredh);
if(mVel!=0)
{
aVel_2= aVel;
mVel_2= mVel;
}
else
{
aVel_2= Yaw;
mVel_2= accelerate * T* 2;
}

//distance control based on next segment parameters
/* DistToIsec = distance along velocity direction to intersection with next segment*/
if( aVel_2== CourseNext)
{
DistToIsec= INFINITE;/**/
}
else
{
SinNext=sin( (aVel_2- CourseNext)* K_DegToRad );
vSM= PredPos- S;
DistToIsec= abs( XTDNext/ SinNext);
}

UE_mVel= k_D* DistToIsec;// k_D must be less than 1/T
UE_mVel= max( stopspeed * friction/ accelerate, UE_mVel);/*to work around Half-life engine behavior*/
UE_mVel= min( UE_mVelMax, UE_mVel);

//crosstrack deviation control on current segment
if( abs(XTDPred)< (Width( currentsegment)+K_margin) )//K_Margin= safeguard ( eg: bot width)
{
/* no control on crosstrack deviation whenever no risk of crossing one bound*/
U_IcptAng= 0.0f;/*can be any value in [0,90[; to be smart, it should maintain the bot within bounds if possible for a "long" time ( several seconds)*/
IcptAngMax= INTERCEPTANGMAX;
}
else
{
U_IcptAng= K_RadToDeg* asin( min(max(-k_Y * XTDPred/ mVel_2, -1),1));
if( U_IcptAng > IcptAngMax) U_IcptAng= IcptAngMax;
else if ( U_IcptAng < -IcptAngMax) U_IcptAng= -IcptAngMax;
else IcptAngMax= INTERCEPTANGMAX;
}

UE_yaw= wrap360(Course+ U_IcptAng);
}

Under "comfortable" conditions ( corridor width big enough for given velocity ), simple switching based on corridor bounds would give trajectories like the following.
Note that InterceptAngMax effect shows on the left curve.


But a small width ( eg:bot walking on a beam ) results in unacceptable overshoot:

This shows
1. that if overshoots can occur, one must provide a way to "walk back" into the corridor while staying on the current segment; switching should not be based on crossing the first bound only, but on being within the corridor; but then one cannot be sure that any computed position will fall within bounds, and something ( adjust speed down) has to be done to be sure that this happens on the first bound crossing.
2. that bot velocity at the time of switching must be such that the ensuing trajectory remains within bounds, and if possible, smoothly rallies the next segment ( adjust speed), which would avoid the first issue.

Half-life game-engine model provides several functions that can be used to do that "open-loop".

Comments
This provides better control upon the trajectory than "Navigation towards points", and the trajectories tends to be more realistic because less zig-zagging on the average: in cases where the path has almost aligned waypoints and high Widths, the system will switch to the last path segment right from the start, whereas the bot may still be far from the source waypoint of this last path segment.
A side effect is that if the bot quits navigation mode for whatever reason, none of the 2 path segments waypoints can then be automatically considered as the closest waypoint for localization purposes.
This can be avoided by adding a switching condition based on distance to waypoints, either to effectively switch to the next path segment, or to keep track as a separate data of the closest destination waypoint just for localization purposes (The true smart solution is to not use waypoints+radius for localization purposes).
In the solution I use, I avoid this problem and some others ( jumps,..) by allowing switching to the next path segment based on reaching the closest bound of the next corridor AND crossing a plane perpendicular to the current path segment close to its destination point.

The following 2 screenshots shows a basic implementation, with prediction, ie not using the bot position, but its position + (-Timestep+1/friction)* 2Dvelocity. This reinjects dampening in the engine speed model and allows for higher K_y ( K_y = 4 ). The bot decently rallies the center line of the segment but overshoots on turns.




Adding speed control, the trajectory is perfect in the sense that the bot stays within bounds. The unavoidable "cost" is that velocity drops a lot on tight turns, which is logical.




The following code is a simplified version of my own actual implementation of a bot for Counter-strike( no jumps, no obstacle avoidance,...) which would require pages of explanations
Ladder moves are special as the input velocity is in the horizontal plane, but the half-line engine transfoms it in a vertical and lateral speed, with a preset velocity modulus ( no friction, no bound on acceleration).

// current segment = PQ; next segment = QS;
// max allowed velocity = UE_mVelMax;
// IcptAngMax= INTERCEPTANGMAX
// bAllowSwitchXTD initialized to FALSE at segment switch; carried over between frames;
// DeltaNextAng initialized at segment switch
// DeltaNextAng= NextSegmentCourse- CurrentSegmentCourse in [-180, 180[
// UE_mVelMax is the max speed wished by the bot, which can be lower then the engine bound (eg: walk to be silent)
// switching condition check, assuming a next segment exists

XTDNextSeg= dotproduct( uvPQNormalNextSeg, vQM);//signed distance to the next segment
if( bAllowSwitchXTD== FALSE)
{
//testing if the next segment corridor is reached, ie a bound crossed
if( DeltaNextAng> 0)
bAllowSwitchXTD=( XTDNextSeg< NextSegmentWidth);
else
bAllowSwitchXTD=( XTDNextSeg> -NextSegmentWidth);
if( bAllowSwitchXTD);
{
// testing if the bot has moved close enough to the current destination waypoint
/*CurrentSegmentDestReachedDist is typically the destination waypoint radius or something equivalent */
bDoSwitch= dotProduct( uvPQ, MQ)< max( NextSegmentWidth, CurrentSegmentDestReachedDist);
}
if( bDoSwitch== TRUE)
{
//... switch segment: initialize data...
bAllowSwitchXTD= FALSE;
DeltaNextAng=...
...
}
}
//
if( no next segment)
{
moveTowardsFinalDest();/*not detailed here; it could be the same code as below with some more code to be sure to stop on the final dest, or a Nav Towards Point code*/
}
else
{
XTD= dotproduct( uvPQNormal, vQM);
if( onLadder)
{
U_IcptAng= -K_RadToDeg* k_Y * XTD/ UE_mVelMax; /*half-life specific: k_Y= 2 */
if( U_IcptAng > 90) U_IcptAng= IcptAngMax;
else if ( U_IcptAng < -90) U_IcptAng= -IcptAngMax;
UE_yaw= wrap360(Course+ U_IcptAng);
}
else
{
PredCoeff= ( -T+ 1/ Friction);/*Half-life specific computation*/
vDeltaPredh= PredCoeff* vVelh;
PredPos= M+ vDeltaPredh;
//uvPQNormal is the normal to uvPQ to its left ( it is horizontal)

//crosstrack deviation control on current segment
XTDPred= XTD+ dotproduct( uvPQNormal, vDeltaPredh);// crosstrack dev + a proportion of speed
// get a safe velocity modulus ( ie non null)
mVel_2= mVel;//velocity modulus in the horizontal plane, ie consistent with XTD
mVelSafe= accelerate * T* EngineSpeedMax;//half-life specific
if( mVel_2< mVelSafe) mVel_2= mVelSafe;
// compute the intercept angle command
U_IcptAng= -K_RadToDeg* k_YPred * XTDPred/ mVel_2; /*half-life specific: k_YPred= 4 is OK because using XTDPred, ie re-injecting XTDSpeed along with XTD*/
// bound -to at least 90 degrees- the intercept angle command
IcptAngMax= 60;
if( U_IcptAng > IcptAngMax) U_IcptAng= IcptAngMax;
else if ( U_IcptAng < -IcptAngMax) U_IcptAng= -IcptAngMax;
// compute the yaw command
UE_yaw= wrap360(Course+ U_IcptAng);

// safety: in case the bot is deviated ( by engine collision management...)
// or overshoots on a final dest point
// (in particular: case where AngDevCmd_a=0 with the destination right in the back)
Bearing= aBearMQ;
BearingRelToCourse= wrap180(Bearing- Course);/*in [-180,+180[ range*/
if( abs(BearingRelToCourse)>= 90.0f
|| ( U_IcptAng< BearingRelToCourse && 0<= U_IcptAng)
|| ( U_IcptAng> BearingRelToCourse && 0>= U_IcptAng))
{
UE_yaw= Bearing;
}
}

//distance control through velocity modulus control
/* BotDistToIsec = distance to current segment destination*/
if( onLadder)
{
UE_mVel= BotDistToIsec/ T;
UE_mVel= min( UE_mVel, UE_mVelMax);
}
else
{
SinNext= sin( (aVel_2- CourseNext)* K_DegToRad );
if( SinNext==0)
{
UE_mVel= UE_mVelMax;
}
else
{
//* in practice, the following code can be simplified to UE_mVel=UE_mVelMax if the minimum pathwidth is greater or equal to the bot half-with*/
Dist= BotDistToIsec+ NextSegmentWidth/abs(SinNext);/*max value, in order to prevent excessive slow-down on turns*/
UE_mVel= k_D* Dist;/* say 2< k_D< 4 for Half-life*/
UE_mVel= max( sv_stopspeed * sv_friction/ sv_accelerate, UE_mVel);/*to work around Half-life engine behavior*/
UE_mVel= min( Dist/T, UE_mVel);
UE_mVel= min( UE_mVelMax, UE_mVel);
}
}
}

A necessary improvement is to get smoother transient trajectories by initiating a turn before entering the next (narrower) segment, while insuring the bot always stays in at least one corridor. This can be achieved using trajectory control fromReaching a point in a given direction
One could also use "Navigation around points", with the point being the intersection of the 2 corridors lying "inside" the turn, and a radius computed accordingly, or at least a point and a radius such that the circle includes the "inside point", excludes the bot current position, and is tangential to the next segment .

To have a less "rigid" trajectory, one could add a "blind-zone" on the crosstrack deviation, defined as Corridor_Width minus a constant. Within this "blind-zone", the crosstrack deviation is considered as being zero ( ie k_Y=0), ie the mobile does not react on small crosstrack deviations across the path segment. This is acceptable as the control law will anyway ensure that the bot direction is the same as the path segment. This must nevertheless adapted to switching conditions, as - for almost aligned segments with the next being narrower than the current - these conditions might never be met. Actually, using a "blind zone" creates problems with my obstacle avoidance code ( jumps in yaw values) and a threshold might be more appropriate.
Something different I did not test yet would be to let the bot move freely in an appropriate direction ( no crosstrack deviation control per se) while ensuring that it never crosses the bounds of the corridor, using crosstrack deviation control code for each bound to prevent their crossing. The resulting leeway within the corridor would be used for possible players-collision-avoidance moves or combat strafing or a different control law.

Navigation around points

Precomputed nav data
A set of points(3D coordinates, flags) or (close-to-)vertical wedges/"poles"( segment, turning sign) around which the bot will turn ( "slalomming" around).
Applicable guidance control law:
Moving passed & around a point.

Switching intermediate destinations:
The bot moves towards the current point, then around. It switches to the next point when it is in a position to move tangentially to the next point circle. Tangency can be checked on the actual yaw ( better but may cause locks , ie continuous circling, on the current point by missing switch conditions in some cases, eg when the 2 points are too close and their radius too small compared to the actual bot turn radius generated by the control law) or on the yaw command based on the next point. The sign of turn tells on which side the next point must lie ( ie the sign of the next turn).

Comments: the basic idea is to have more realistic trajectories, but above all to avoid the need of waypoints data files, using data from the world ( .bsp files in Half-life) to extract acute edges between faces for move, and using the bsptree for localization ( under development) .
The following is a 2D simulation using Half-life model, with switching (safely) based on the next point yaw command. We face the same issue as for navigation towards point, ie the bot overshoots on turns, but this should cause less problems as the bot turns close to the edge. This can be reduced by using a predicted position instead of the position alone.