public bool DoesIntersect(LinePath path)
        {
            foreach (LineSegment ls in path.GetSegmentEnumerator())
            {
                if (DoesIntersect(ls))
                {
                    return(true);
                }
            }

            return(false);
        }
        private LinePath FindBoundary(double offset)
        {
            // create a list of shift line segments
            List <LineSegment> segs = new List <LineSegment>();

            foreach (LineSegment ls in basePath.GetSegmentEnumerator())
            {
                segs.Add(ls.ShiftLateral(offset));
            }

            // find the intersection points between all of the segments
            LinePath boundPoints = new LinePath();

            // add the first point
            boundPoints.Add(segs[0].P0);

            // loop through the stuff
            for (int i = 0; i < segs.Count - 1; i++)
            {
                // find the intersection
                Line l0 = (Line)segs[i];
                Line l1 = (Line)segs[i + 1];

                Coordinates pt;
                if (l0.Intersect(l1, out pt))
                {
                    // figure out the angle of the intersection
                    double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));

                    // calculate the length factor
                    // 90 deg, 3x width
                    // 0 deg, 2x width
                    double widthFactor = Math.Pow(angle / (Math.PI / 2.0), 2) * 2 + 1;

                    // get the line formed by pt and the corresponding path point
                    Coordinates boundLine = pt - basePath[i + 1];

                    boundPoints.Add(widthFactor * Math.Abs(offset) * boundLine.Normalize() + basePath[i + 1]);
                }
                else
                {
                    boundPoints.Add(segs[i].P1);
                }
            }

            // add the last point
            boundPoints.Add(segs[segs.Count - 1].P1);

            return(boundPoints);
        }
Esempio n. 3
0
        public override void Process(object param)
        {
            if (!base.BeginProcess())
            {
                return;
            }

            if (param is ZoneTravelingBehavior)
            {
                HandleBaseBehavior((ZoneTravelingBehavior)param);
            }

            extraObstacles = GetPerimeterObstacles();

            if (reverseGear)
            {
                ProcessReverse();
                return;
            }

            AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);

            // get the vehicle relative path
            LinePath relRecommendedPath = recommendedPath.Transform(absTransform);

            LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint;

            // get the distance to the end point
            double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint);

            // get the planning distance
            double planningDist = GetPlanningDistance();

            planningDist  = Math.Max(planningDist, 20);
            planningDist -= zeroPoint.Location.Length;
            if (planningDist < 2.5)
            {
                planningDist = 2.5;
            }

            if (distToEnd < planningDist)
            {
                // make the speed command at stop speed command
                behaviorTimestamp = curTimestamp;
                speedCommand      = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL);
                planningDist      = distToEnd;
                approachSpeed     = recommendedSpeed.Speed;

                settings.endingHeading       = relRecommendedPath.EndSegment.UnitVector.ArcTan;
                settings.endingPositionFixed = true;
                settings.endingPositionMax   = 2;
                settings.endingPositionMin   = -2;
            }
            else
            {
                speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed);
            }

            // get the distance of the path segment we care about
            LinePath pathSegment   = relRecommendedPath.SubPath(zeroPoint, planningDist);
            double   avoidanceDist = planningDist + 5;

            avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist);
            if (avoidanceDist > 0)
            {
                avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist));
            }

            // test if we should clear out of arc mode
            if (arcMode)
            {
                if (TestNormalModeClear(relRecommendedPath, zeroPoint))
                {
                    prevCurvature = double.NaN;
                    arcMode       = false;
                }
            }

            if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1)
            {
                pathSegment.Insert(0, Coordinates.Zero);
            }
            else
            {
                if (pathSegment[0].DistanceTo(pathSegment[1]) < 1)
                {
                    pathSegment.RemoveAt(0);
                }
                pathSegment[0] = Coordinates.Zero;
            }

            if (arcMode)
            {
                Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location;
                ArcVoteZone(relativeGoalPoint, extraObstacles);
                return;
            }

            double pathLength = pathSegment.PathLength;

            if (pathLength < 6)
            {
                double additionalDist = 6.25 - pathLength;
                pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist));
            }

            // determine if polygons are to the left or right of the path
            for (int i = 0; i < zoneBadRegions.Length; i++)
            {
                Polygon poly = zoneBadRegions[i].Transform(absTransform);

                int numLeft  = 0;
                int numRight = 0;
                foreach (LineSegment ls in pathSegment.GetSegmentEnumerator())
                {
                    for (int j = 0; j < poly.Count; j++)
                    {
                        if (ls.IsToLeft(poly[j]))
                        {
                            numLeft++;
                        }
                        else
                        {
                            numRight++;
                        }
                    }
                }

                if (numLeft > numRight)
                {
                    // we'll consider this polygon on the left of the path
                    //additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
                }
                else
                {
                    //additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
                }
            }

            // TODO: add zone perimeter
            disablePathAngleCheck   = false;
            laneWidthAtPathEnd      = 7;
            settings.Options.w_diff = 3;
            smootherBasePath        = new LinePath();
            smootherBasePath.Add(Coordinates.Zero);
            smootherBasePath.Add(pathSegment.EndPoint.Location);
            AddTargetPath(pathSegment, 0.005);

            settings.maxSpeed = recommendedSpeed.Speed;
            useAvoidancePath  = false;

            SmoothAndTrack(command_label, true);
        }
        public static LinePath FindBoundary(double offset, LinePath path)
        {
            // create a list of shift line segments
            List<LineSegment> segs = new List<LineSegment>();

            foreach (LineSegment ls in path.GetSegmentEnumerator()) {
                segs.Add(ls.ShiftLateral(offset));
            }

            // find the intersection points between all of the segments
            LinePath boundPoints = new LinePath();
            // add the first point
            boundPoints.Add(segs[0].P0);

            // loop through the stuff
            for (int i = 0; i < segs.Count-1; i++) {
                // find the intersection
                Line l0 = (Line)segs[i];
                Line l1 = (Line)segs[i+1];

                Coordinates pt;
                if (l0.Intersect(l1, out pt)) {
                    // figure out the angle of the intersection
                    double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));
                    double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector));

                    // determine if it's a left or right turn
                    bool leftTurn = angle2 > 0;
                    double f = 2.5;
                    if ((leftTurn && offset > 0) || (!leftTurn && offset < 0)) {
                        // left turn and looking for left bound or right turn and looking for right bound
                        f = 0.5;
                    }

                    // calculate the width expansion factor
                    // 90 deg, 3x width
                    // 45 deg, 1.5x width
                    // 0 deg, 1x width
                    double widthFactor = Math.Pow(angle/(Math.PI/2.0),1.25)*f + 1;

                    // get the line formed by pt and the corresponding path point
                    Coordinates boundLine = pt - path[i+1];

                    boundPoints.Add(widthFactor*Math.Abs(offset)*boundLine.Normalize() + path[i+1]);
                }
                else {
                    boundPoints.Add(segs[i].P1);
                }
            }

            // add the last point
            boundPoints.Add(segs[segs.Count-1].P1);

            return boundPoints;
        }
        public static LinePath FindBoundary(double offset, LinePath path)
        {
            // create a list of shift line segments
            List <LineSegment> segs = new List <LineSegment>();

            foreach (LineSegment ls in path.GetSegmentEnumerator())
            {
                segs.Add(ls.ShiftLateral(offset));
            }

            // find the intersection points between all of the segments
            LinePath boundPoints = new LinePath();

            // add the first point
            boundPoints.Add(segs[0].P0);

            // loop through the stuff
            for (int i = 0; i < segs.Count - 1; i++)
            {
                // find the intersection
                Line l0 = (Line)segs[i];
                Line l1 = (Line)segs[i + 1];

                Coordinates pt;
                if (l0.Intersect(l1, out pt))
                {
                    // figure out the angle of the intersection
                    double angle  = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));
                    double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector));

                    // determine if it's a left or right turn
                    bool   leftTurn = angle2 > 0;
                    double f        = 2.5;
                    if ((leftTurn && offset > 0) || (!leftTurn && offset < 0))
                    {
                        // left turn and looking for left bound or right turn and looking for right bound
                        f = 0.5;
                    }

                    // calculate the width expansion factor
                    // 90 deg, 3x width
                    // 45 deg, 1.5x width
                    // 0 deg, 1x width
                    double widthFactor = Math.Pow(angle / (Math.PI / 2.0), 1.25) * f + 1;

                    // get the line formed by pt and the corresponding path point
                    Coordinates boundLine = pt - path[i + 1];

                    boundPoints.Add(widthFactor * Math.Abs(offset) * boundLine.Normalize() + path[i + 1]);
                }
                else
                {
                    boundPoints.Add(segs[i].P1);
                }
            }

            // add the last point
            boundPoints.Add(segs[segs.Count - 1].P1);

            return(boundPoints);
        }