C# Circle.Intersect方法代码示例

        private ITrackingCommand BuildBackwardPass()
        {
            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Initialize Backward Pass");

            // rotate the polygon into the current relative frame
            CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
            RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
            relTransform.TransformPointsInPlace(polygon);
            finalOrientation = finalOrientation.Transform(relTransform);
            polygonTimestamp = curTimestamp;

            // retrieve the vehicle state
            Coordinates headingVec = new Coordinates(1, 0);
            Coordinates headingVec180 = headingVec.Rotate180();
            Coordinates headingVec90 = headingVec.Rotate90();

            // figure out center point of turn
            Circle rearAxleCircle = Circle.FromPointSlopeRadius(new Coordinates(0,0), headingVec180, minRadius);
            Coordinates center = rearAxleCircle.center;

            // calculate the points of the wheels
            Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2;
            Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2;
            Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2;
            Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2;

            double minHit = Math.PI/2.1;
            GetMinHitAngle(rearLeftPoint, center, ref minHit);
            GetMinHitAngle(rearRightPoint, center, ref minHit);
            //GetMinHitAngle(frontLeftPoint, center, false, ref minHit);
            //GetMinHitAngle(frontRightPoint, center, false, ref minHit);

            frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2;
            frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0;
            rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0;
            rearLeftPoint = -headingVec*TahoeParams.RL + headingVec90*TahoeParams.T/2.0;
            List<Polygon> obstacles = GetObstacles(curTimestamp);
            GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
            GetObstacleHitAngle(rearLeftPoint, center, obstacles, ref minHit);
            GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);

            // trim some off the hit for safety'
            minHit -= (0.3/minRadius);
            // move at least 0.6 meters
            minHit = Math.Max(minHit, 0.6 / minRadius);

            // calculate the exit stopping point
            // shift the line by the minimum turning radius
            Coordinates u = finalOrientation.P1 - finalOrientation.P0;
            u = u.Normalize().Rotate90();
            Line offsetLine = new Line();
            offsetLine.P0 = finalOrientation.P0 + u*(minRadius+2);
            offsetLine.P1 = finalOrientation.P1 + u*(minRadius+2);

            // final the intersection of the current turn circle with a radius of twice the min turn radius and the offset line
            Circle twoTurn = new Circle(2*minRadius + 2, center);
            Coordinates[] intersections;

            double startAngle = (-center).ArcTan;
            if (twoTurn.Intersect(offsetLine, out intersections)) {
                // figure out where there were hits
                for (int i = 0; i < intersections.Length; i++) {
                    // get the angle of the hit
                    double angle = (intersections[i] - center).ArcTan;

                    if (angle < startAngle)
                        angle += 2*Math.PI;

                    angle -= startAngle;

                    if (angle < minHit)
                        minHit = angle;
                }
            }

            minHit = Math.Max(minHit, 0.6 / minRadius);

            // set the stopping point at the min hit point
            Coordinates stopPoint = rearAxleCircle.GetPoint(startAngle+minHit);

            // calculate the stop distance
            stopDistance = rearAxleCircle.r*minHit;
            stopTimestamp = curTimestamp;
            curvature = 1/minRadius;
            // calculate the required steering angle
            double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(-1/minRadius, uturnSpeed);

            ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.Reverse);
            ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);

            ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
            ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);

            ChainedTrackingCommand cmd = new ChainedTrackingCommand(
                new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
                new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
            cmd.Label = backwardLabel;

            Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
            Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
//.........这里部分代码省略.........

 public bool Intersect(Circle c, out Vector2[] pts)
 {
     return c.Intersect(this, out pts);
 }

 public bool Intersect(Circle c, out Coordinates[] pts)
 {
     return c.Intersect(this, out pts);
 }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="goalPoint"></param>
        /// <param name="obstacles"></param>
        /// <param name="goal"></param>
        /// <param name="foundPath"></param>
        /// <param name="samplePoint"></param>
        /// <param name="closestNode"></param>
        /// <returns>true if the node was added to the tree (i.e. didnt hit crap)</returns>
        private RRTNode ExtendNode(ref Vector2 goalPoint, List<Polygon> obstacles, ref RRTNode goal, ref bool foundPath, ref Vector2 samplePoint, RRTNode closestNode, Random rand)
        {
            //3) generate a control input that drives towards the sample point also biased with our initial control inputs
            //3a)   -Biasing Details:
            //		Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity
            //		Select Turn Rate:
            //			Apply the following heuristic:  mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT)
            //																			sigma = SigmaTurnRate

            // velocity distribution

            MathNet.Numerics.Distributions.NormalDistribution vDist = new MathNet.Numerics.Distributions.NormalDistribution(closestNode.State.Command.velocity, vSigma);

            // turn-rate biased
            double mixingSample = rand.NextDouble();
            double wMean = 0;
            if (mixingSample > mixingProportion)
            {
                double angleToClosestNode = Math.Atan2((samplePoint.Y - closestNode.Point.Y), (samplePoint.X - closestNode.Point.X));
                //wMean = -kPwSample * angleToClosestNode;
                wMean = ((angleToClosestNode - closestNode.State.Pose.yaw)) * 180.0 / Math.PI / timeStep;
                if (wMean > MAX_TURN - 20)
                    wMean = MAX_TURN - 20;
                if (wMean < MIN_TURN + 20)
                    wMean = MIN_TURN + 20;
            }
            else
                wMean = 0;

            MathNet.Numerics.Distributions.NormalDistribution wDist = new MathNet.Numerics.Distributions.NormalDistribution(wMean, wSigma);

            double velSampled = vDist.NextDouble();
            double wSampled = wDist.NextDouble();
            while (velSampled > MAX_VEL || velSampled < MIN_VEL)
                velSampled = vDist.NextDouble();
            while (wSampled > MAX_TURN || wSampled < MIN_TURN)
                wSampled = wDist.NextDouble();

            // 4) Predict a node
            RRTNode predictedNode = CalculateNextNode(closestNode, velSampled, wSampled, obstacles);
            if (predictedNode != null)
            {
                closestNode.AddChild(predictedNode);
                //5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1.
                //Polygon goalPolygon = Polygon.VehiclePolygonWithRadius(0.5, goalPoint);
                Circle c = new Circle(.5, goalPoint);
                LineSegment nodeToParent = new LineSegment(predictedNode.Point, predictedNode.Parent.Point);
                Vector2[] pts = new Vector2[2];
                if (c.Intersect(nodeToParent, out pts))
                {
                    foundPath = true;
                    goal = predictedNode;
                }
                //if (predictedNode.DistanceTo(goalPoint) < goalTolerance)
                //{
                //    foundPath = true;
                //    goal = predictedNode;
                //}
                return predictedNode;
            }
            return null;
        }