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 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 }