void DrawBoundaryFencesOnMap(TerrainMap map)
{
// QQQ it would make more sense to do this with a "draw line
// QQQ on map" primitive, may need that for other things too
int cw = map.Cellwidth();
int ch = map.Cellheight();
int r = cw - 1;
int a = cw >> 3;
int b = cw - a;
int o = cw >> 4;
int p = (cw - o) >> 1;
int q = (cw + o) >> 1;
for (int i = 0; i < cw; i++)
{
for (int j = 0; j < ch; j++)
{
bool c = i > a && i < b && (i <p || i> q);
if (i == 0 || j == 0 || i == r || j == r || (c && (i == j || i + j == r)))
{
map.SetMapBit(i, j, true);
}
}
}
}
void DrawPathFencesOnMap(TerrainMap map, GCRoute path)
{
float xs = map.xSize / (float)map.resolution;
float zs = map.zSize / (float)map.resolution;
Vector3 alongRow = new Vector3(xs, 0, 0);
Vector3 nextRow = new Vector3(-map.xSize, 0, zs);
Vector3 g = new Vector3((map.xSize - xs) / -2, 0, (map.zSize - zs) / -2);
for (int j = 0; j < map.resolution; j++)
{
for (int i = 0; i < map.resolution; i++)
{
float outside = path.HowFarOutsidePath(g);
float wallThickness = 1.0f;
// set map cells adjacent to the outside edge of the path
if ((outside > 0) && (outside < wallThickness))
{
map.SetMapBit(i, j, true);
}
// clear all other off-path map cells
if (outside > wallThickness)
{
map.SetMapBit(i, j, false);
}
g += alongRow;
}
g += nextRow;
}
}
void ClearCenterOfMap(TerrainMap map)
{
int o = map.Cellwidth() >> 4;
int p = (map.Cellwidth() - o) >> 1;
int q = (map.Cellwidth() + o) >> 1;
for (int i = p; i <= q; i++)
{
for (int j = p; j <= q; j++)
{
map.SetMapBit(i, j, false);
}
}
}
void DrawRandomClumpsOfRocksOnMap(TerrainMap map)
{
if (useRandomRocks)
{
int spread = 4;
int r = map.Cellwidth();
int k = Random2(50, 150);
for (int p = 0; p < k; p++)
{
int i = Random2(0, r - spread);
int j = Random2(0, r - spread);
int c = Random2(0, 10);
for (int q = 0; q < c; q++)
{
int m = Random2(0, spread);
int n = Random2(0, spread);
map.SetMapBit(i + m, j + n, true);
}
}
}
}
void DrawPathFencesOnMap(TerrainMap map, GCRoute path)
{
float xs = map.xSize / (float)map.resolution;
float zs = map.zSize / (float)map.resolution;
Vector3 alongRow = new Vector3(xs, 0, 0);
Vector3 nextRow = new Vector3(-map.xSize, 0, zs);
Vector3 g = new Vector3((map.xSize - xs) / -2, 0, (map.zSize - zs) / -2);
for (int j = 0; j < map.resolution; j++)
{
for (int i = 0; i < map.resolution; i++)
{
float outside = path.HowFarOutsidePath(g);
float wallThickness = 1.0f;
// set map cells adjacent to the outside edge of the path
if ((outside > 0) && (outside < wallThickness))
map.SetMapBit(i, j, true);
// clear all other off-path map cells
if (outside > wallThickness) map.SetMapBit(i, j, false);
g += alongRow;
}
g += nextRow;
}
}
void DrawRandomClumpsOfRocksOnMap(TerrainMap map)
{
if (useRandomRocks)
{
int spread = 4;
int r = map.Cellwidth();
int k = Random2(50, 150);
for (int p = 0; p < k; p++)
{
int i = Random2(0, r - spread);
int j = Random2(0, r - spread);
int c = Random2(0, 10);
for (int q = 0; q < c; q++)
{
int m = Random2(0, spread);
int n = Random2(0, spread);
map.SetMapBit(i + m, j + n, true);
}
}
}
}
void DrawBoundaryFencesOnMap(TerrainMap map)
{
// QQQ it would make more sense to do this with a "draw line
// QQQ on map" primitive, may need that for other things too
int cw = map.Cellwidth();
int ch = map.Cellheight();
int r = cw - 1;
int a = cw >> 3;
int b = cw - a;
int o = cw >> 4;
int p = (cw - o) >> 1;
int q = (cw + o) >> 1;
for (int i = 0; i < cw; i++)
{
for (int j = 0; j < ch; j++)
{
bool c = i > a && i < b && (i < p || i > q);
if (i == 0 || j == 0 || i == r || j == r || (c && (i == j || i + j == r)))
map.SetMapBit(i, j, true);
}
}
}
void ClearCenterOfMap(TerrainMap map)
{
int o = map.Cellwidth() >> 4;
int p = (map.Cellwidth() - o) >> 1;
int q = (map.Cellwidth() + o) >> 1;
for (int i = p; i <= q; i++)
for (int j = p; j <= q; j++)
map.SetMapBit(i, j, false);
}
// constructor
public MapDriver()
{
map = MakeMap();
path = MakePath();
Reset();
// to compute mean time between collisions
sumOfCollisionFreeTimes = 0;
countOfCollisionFreeTimes = 0;
// keep track for reliability statistics
collisionLastTime = false;
timeOfLastCollision = Demo.Clock.TotalSimulationTime;
// keep track of average speed
totalDistance = 0;
totalTime = 0;
// keep track of path following failure rate
pathFollowTime = 0;
pathFollowOffTime = 0;
// innitialize counters for various performance data
stuckCount = 0;
stuckCycleCount = 0;
stuckOffPathCount = 0;
lapsStarted = 0;
lapsFinished = 0;
hintGivenCount = 0;
hintTakenCount = 0;
// follow the path "upstream or downstream" (+1/-1)
pathFollowDirection = 1;
// use curved prediction and incremental steering:
curvedSteering = true;
incrementalSteering = true;
}
// given a map of obstacles (currently a global, binary map) steer so as
// to avoid collisions within the next minTimeToCollision seconds.
//
public Vector3 SteerToAvoidObstaclesOnMap(float minTimeToCollision, TerrainMap map, Vector3 steerHint)
{
float spacing = map.MinSpacing() / 2;
float maxSide = Radius;
float maxForward = minTimeToCollision * Speed;
int maxSamples = (int)(maxForward / spacing);
Vector3 step = Forward * spacing;
Vector3 fOffset = Position;
Vector3 sOffset = Vector3.Zero;
float s = spacing / 2;
int infinity = 9999; // qqq
int nearestL = infinity;
int nearestR = infinity;
int nearestWL = infinity;
int nearestWR = infinity;
Vector3 nearestO = Vector3.Zero;
wingDrawFlagL = false;
wingDrawFlagR = false;
bool hintGiven = steerHint != Vector3.Zero;
if (hintGiven && !dtZero)
hintGivenCount++;
if (hintGiven)
annotation.CircleOrDisk(halfWidth * 0.9f, Up, Position + (Up * 0.2f), Color.White, 12, false, false);
// QQQ temporary global QQQoaJustScraping
QQQoaJustScraping = true;
float signedRadius = 1 / NonZeroCurvatureQQQ();
Vector3 localCenterOfCurvature = Side * signedRadius;
Vector3 center = Position + localCenterOfCurvature;
float sign = signedRadius < 0 ? 1.0f : -1.0f;
float arcRadius = signedRadius * -sign;
float twoPi = 2 * (float)Math.PI;
float circumference = twoPi * arcRadius;
float rawLength = Speed * minTimeToCollision * sign;
float fracLimit = 1.0f / 6.0f;
float distLimit = circumference * fracLimit;
float arcLength = ArcLengthLimit(rawLength, distLimit);
float arcAngle = twoPi * arcLength / circumference;
// XXX temp annotation to show limit on arc angle
if (curvedSteering)
{
if ((Speed * minTimeToCollision) > (circumference * fracLimit))
{
float q = twoPi * fracLimit;
Vector3 fooz = Position - center;
Vector3 booz = Vector3Helpers.RotateAboutGlobalY(fooz, sign * q);
annotation.Line(center, center + fooz, Color.Red);
annotation.Line(center, center + booz, Color.Red);
}
}
// assert loops will terminate
System.Diagnostics.Debug.Assert(spacing > 0);
// scan corridor straight ahead of vehicle,
// keep track of nearest obstacle on left and right sides
while (s < maxSide)
{
sOffset = Side * s;
s += spacing;
Vector3 lOffset = fOffset + sOffset;
Vector3 rOffset = fOffset - sOffset;
Vector3 lObsPos = Vector3.Zero, rObsPos = Vector3.Zero;
int L = (curvedSteering ?
(int)(ScanObstacleMap(lOffset,
center,
arcAngle,
maxSamples,
0,
Color.Yellow,
Color.Red,
out lObsPos)
/ spacing) :
map.ScanXZray(lOffset, step, maxSamples));
int R = (curvedSteering ?
(int)(ScanObstacleMap(rOffset,
center,
arcAngle,
maxSamples,
0,
Color.Yellow,
Color.Red,
out rObsPos)
/ spacing) :
map.ScanXZray(rOffset, step, maxSamples));
if ((L > 0) && (L < nearestL))
{
nearestL = L;
if (L < nearestR) nearestO = ((curvedSteering) ?
lObsPos :
lOffset + (step * (float)L));
}
if ((R > 0) && (R < nearestR))
{
nearestR = R;
if (R < nearestL) nearestO = ((curvedSteering) ?
rObsPos :
rOffset + (step * (float)R));
}
if (!curvedSteering)
{
AnnotateAvoidObstaclesOnMap(lOffset, L, step);
AnnotateAvoidObstaclesOnMap(rOffset, R, step);
}
if (curvedSteering)
{
// QQQ temporary global QQQoaJustScraping
bool outermost = s >= maxSide;
bool eitherSide = (L > 0) || (R > 0);
if (!outermost && eitherSide) QQQoaJustScraping = false;
}
}
qqqLastNearestObstacle = nearestO;
// scan "wings"
{
int wingScans = 4;
// see duplicated code at: QQQ draw sensing "wings"
// QQQ should be a parameter of this method
Vector3 wingWidth = Side * WingSlope() * maxForward;
Color beforeColor = new Color((byte)(255.0f * 0.75f), (byte)(255.0f * 0.9f), (byte)(255.0f * 0.0f)); // for annotation
Color afterColor = new Color((byte)(255.0f * 0.9f), (byte)(255.0f * 0.5f), (byte)(255.0f * 0.0f)); // for annotation
for (int i = 1; i <= wingScans; i++)
{
float fraction = (float)i / (float)wingScans;
Vector3 endside = sOffset + (wingWidth * fraction);
Vector3 corridorFront = Forward * maxForward;
// "loop" from -1 to 1
for (int j = -1; j < 2; j += 2)
{
float k = (float)j; // prevent VC7.1 warning
Vector3 start = fOffset + (sOffset * k);
Vector3 end = fOffset + corridorFront + (endside * k);
Vector3 ray = end - start;
float rayLength = ray.Length();
Vector3 step2 = ray * spacing / rayLength;
int raySamples = (int)(rayLength / spacing);
float endRadius =
WingSlope() * maxForward * fraction *
(signedRadius < 0 ? 1 : -1) * (j == 1 ? 1 : -1);
Vector3 ignore;
int scan = (curvedSteering ?
(int)(ScanObstacleMap(start,
center,
arcAngle,
raySamples,
endRadius,
beforeColor,
afterColor,
out ignore)
/ spacing) :
map.ScanXZray(start, step2, raySamples));
if (!curvedSteering)
AnnotateAvoidObstaclesOnMap(start, scan, step2);
if (j == 1)
{
if ((scan > 0) && (scan < nearestWL)) nearestWL = scan;
}
else
{
if ((scan > 0) && (scan < nearestWR)) nearestWR = scan;
}
}
}
wingDrawFlagL = nearestWL != infinity;
wingDrawFlagR = nearestWR != infinity;
}
// for annotation
savedNearestWR = (float)nearestWR;
savedNearestR = (float)nearestR;
savedNearestL = (float)nearestL;
savedNearestWL = (float)nearestWL;
// flags for compound conditions, used below
bool obstacleFreeC = nearestL == infinity && nearestR == infinity;
bool obstacleFreeL = nearestL == infinity && nearestWL == infinity;
bool obstacleFreeR = nearestR == infinity && nearestWR == infinity;
bool obstacleFreeWL = nearestWL == infinity;
bool obstacleFreeWR = nearestWR == infinity;
bool obstacleFreeW = obstacleFreeWL && obstacleFreeWR;
// when doing curved steering and we have already detected "just
// scarping" but neither wing is free, recind the "just scarping"
// QQQ temporary global QQQoaJustScraping
bool JS = curvedSteering && QQQoaJustScraping;
bool cancelJS = !obstacleFreeWL && !obstacleFreeWR;
if (JS && cancelJS) QQQoaJustScraping = false;
// ----------------------------------------------------------
// now we have measured everything, decide which way to steer
// ----------------------------------------------------------
// no obstacles found on path, return zero steering
if (obstacleFreeC)
{
qqqLastNearestObstacle = Vector3.Zero;
AnnotationNoteOAClauseName("obstacleFreeC");
// qqq this may be in the wrong place (what would be the right
// qqq place?!) but I'm trying to say "even if the path is
// qqq clear, don't go too fast when driving between obstacles
if (obstacleFreeWL || obstacleFreeWR || RelativeSpeed() < 0.7f)
return Vector3.Zero;
else
return -Forward;
}
// if the nearest obstacle is way out there, take hint if any
// if (hintGiven && (Math.Min (nearestL, nearestR) > (maxSamples * 0.8f)))
if (hintGiven && (Math.Min((float)nearestL, (float)nearestR) > (maxSamples * 0.8f)))
{
AnnotationNoteOAClauseName("nearest obstacle is way out there");
AnnotationHintWasTaken();
if (Vector3.Dot(steerHint, Side) > 0)
return Side;
else
return -Side;
}
// QQQ experiment 3-9-04
//
// since there are obstacles ahead, if we are already near
// maximum curvature, we MUST turn in opposite direction
//
// are we turning more sharply than the minimum turning radius?
// (code from adjustSteeringForMinimumTurningRadius)
float maxCurvature = 1 / (MinimumTurningRadius() * 1.2f);
if (Math.Abs(Curvature) > maxCurvature)
{
Color blue = new Color(0, 0, (byte)(255.0f * 0.8f));
AnnotationNoteOAClauseName("min turn radius");
annotation.CircleOrDisk(MinimumTurningRadius() * 1.2f, Up,
center, blue, 40, false, false);
return Side * sign;
}
// if either side is obstacle-free, turn in that direction
if (obstacleFreeL || obstacleFreeR)
AnnotationNoteOAClauseName("obstacle-free side");
if (obstacleFreeL) return Side;
if (obstacleFreeR) return -Side;
// if wings are clear, turn away from nearest obstacle straight ahead
if (obstacleFreeW)
{
AnnotationNoteOAClauseName("obstacleFreeW");
// distance to obs on L and R side of corridor roughtly the same
bool same = Math.Abs(nearestL - nearestR) < 5; // within 5
// if they are about the same and a hint is given, use hint
if (same && hintGiven)
{
AnnotationHintWasTaken();
if (Vector3.Dot(steerHint, Side) > 0)
return Side;
else
return -Side;
}
else
{
// otherwise steer toward the less cluttered side
if (nearestL > nearestR)
return Side;
else
return -Side;
}
}
// if the two wings are about equally clear and a steering hint is
// provided, use it
bool equallyClear = Math.Abs(nearestWL - nearestWR) < 2; // within 2
if (equallyClear && hintGiven)
{
AnnotationNoteOAClauseName("equallyClear");
AnnotationHintWasTaken();
if (Vector3.Dot(steerHint, Side) > 0) return Side; else return -Side;
}
// turn towards the side whose "wing" region is less cluttered
// (the wing whose nearest obstacle is furthest away)
AnnotationNoteOAClauseName("wing less cluttered");
if (nearestWL > nearestWR) return Side; else return -Side;
}
// like steerToAvoidObstacles, but based on a BinaryTerrainMap indicating
// the possitions of impassible regions
//
public Vector3 SteerToAvoidObstaclesOnMap(float minTimeToCollision, TerrainMap map)
{
return SteerToAvoidObstaclesOnMap(minTimeToCollision, map, Vector3.Zero); // no steer hint
}
