Ejemplo n.º 1
0
        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);
                    }
                }
            }
        }
Ejemplo n.º 2
0
        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;
            }
        }
Ejemplo n.º 3
0
        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);
                }
            }
        }
Ejemplo n.º 4
0
        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);
                    }
                }
            }
        }
Ejemplo n.º 5
0
        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;
            }
        }
Ejemplo n.º 6
0
        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);
                    }
                }
            }
        }
Ejemplo n.º 7
0
        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);
                }
            }
        }
Ejemplo n.º 8
0
 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);
 }
Ejemplo n.º 9
0
        // 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;
        }
Ejemplo n.º 10
0
        // 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;
        }
Ejemplo n.º 11
0
 // 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
 }