C# Coordinates.Rotate90方法代码示例

        /// <summary>
        /// Update grid lane bound 
        /// </summary>
        /// <param name="leftBoundPoints">Lane left bound points</param>
        /// <param name="rightBoundPoints">Lane right bound points</param>
        private void UpdateGridLaneBound(IList<LinePath> leftBounds, IList<LinePath> rightBounds)
        {
            StartWatch();	// start stopwatch

            Coordinates extPoint = new Coordinates();
            List<GridCoordinates> gridExtendLocs;

            foreach (LinePath leftBoundPoints in leftBounds) {
                // check if there is at least 2 left bound points
                if (leftBoundPoints.Count > 1) {
                    // find grid lane bound left points
                    FindPoints(leftBoundPoints, out gridLaneBoundLeft);
                    gridLaneBound.SetValues(gridLaneBoundLeft, maxCost);

                    // check if there are grid lane bound left points
                    if (gridLaneBoundLeft.Count != 0) {
                      // extend start of grid left lane bound to grid border
                        extPoint = leftBoundPoints[0] - leftBoundPoints[1];
                        extPoint = leftBoundPoints[0] + extPoint.RotateM90();
                        ExtendPoints(leftBoundPoints[0], extPoint, out gridExtendLocs);
                      gridLaneBound.SetValues(gridExtendLocs, maxCost);

                      // extend end of grid left lane bound to grid border
                      int lastIndex = leftBoundPoints.Count - 1;
                        extPoint = leftBoundPoints[lastIndex] - leftBoundPoints[lastIndex - 1];
                        extPoint = leftBoundPoints[lastIndex] + extPoint.Rotate90().Normalize();
                      ExtendPoints(leftBoundPoints[lastIndex], extPoint, out gridExtendLocs);
                      gridLaneBound.SetValues(gridExtendLocs, maxCost);

                        // floodfill lane bound area
                        List<Coordinates> partialLaneBoundPoints = new List<Coordinates>();
                        partialLaneBoundPoints.Add(leftBoundPoints[lastIndex]);
                        partialLaneBoundPoints.Add(leftBoundPoints[lastIndex - 1]);
                        partialLaneBoundPoints.Add(extPoint);
                        Polygon partialLaneBoundArea = new Polygon(partialLaneBoundPoints);
                        Coordinates partialLaneBoundAreaCenter = partialLaneBoundArea.Center;
                        GridCoordinates partialGridLaneBoundAreaCenter = new GridCoordinates(
                            (int)Math.Round(partialLaneBoundAreaCenter.X / gridStep) + gridMiddleX,
                            (int)Math.Round(partialLaneBoundAreaCenter.Y / gridStep) + gridMiddleY);
                        gridLaneBound.FloodFill(partialGridLaneBoundAreaCenter, maxCost, 0);
                    }
                }
            }

            foreach (LinePath rightBoundPoints in rightBounds) {
                // check if there is at least 2 right bound points
                if (rightBoundPoints.Count > 1) {
                    // find grid lane bound right points
                    FindPoints(rightBoundPoints, out gridLaneBoundRight);
                    gridLaneBound.SetValues(gridLaneBoundRight, maxCost);

                    // check if there are grid lane bound right points
                    if (gridLaneBoundRight.Count != 0) {
                        // extend start of grid right lane bound to grid border
                        extPoint = rightBoundPoints[0] - rightBoundPoints[1];
                        extPoint = rightBoundPoints[0] + extPoint.Rotate90();
                        ExtendPoints(rightBoundPoints[0], extPoint, out gridExtendLocs);
                        gridLaneBound.SetValues(gridExtendLocs, maxCost);

                        // extend end of grid right lane bound to grid border
                        int lastIndex = rightBoundPoints.Count - 1;
                        extPoint = rightBoundPoints[lastIndex] - rightBoundPoints[lastIndex - 1];
                        extPoint = rightBoundPoints[lastIndex] + extPoint.RotateM90().Normalize();
                        ExtendPoints(rightBoundPoints[lastIndex], extPoint, out gridExtendLocs);
                        gridLaneBound.SetValues(gridExtendLocs, maxCost);

                        // floodfill lane bound area
                        List<Coordinates> partialLaneBoundPoints = new List<Coordinates>();
                        partialLaneBoundPoints.Add(rightBoundPoints[lastIndex]);
                        partialLaneBoundPoints.Add(rightBoundPoints[lastIndex - 1]);
                        partialLaneBoundPoints.Add(extPoint);
                        Polygon partialLaneBoundArea = new Polygon(partialLaneBoundPoints);
                        Coordinates partialLaneBoundAreaCenter = partialLaneBoundArea.Center;
                        GridCoordinates partialGridLaneBoundAreaCenter = new GridCoordinates(
                            (int)Math.Round(partialLaneBoundAreaCenter.X / gridStep) + gridMiddleX,
                            (int)Math.Round(partialLaneBoundAreaCenter.Y / gridStep) + gridMiddleY);
                        gridLaneBound.FloodFill(partialGridLaneBoundAreaCenter, maxCost, 0);
                    }
                }
            }

            // generate grid lane bound using wavefront propagation
            float sepDist = 1;	// in m
            float step = maxCost * gridStep / sepDist;
            //gridLaneBound.WaveFront(maxCost, 0, -step);

            // display time to process grid lane bound
            gridLaneBoundTime = StopWatch("ObstacleManager - GridLaneBound - ");

            // write grid data to file (for debugging)
            if (writeToFileFlag == true) {
                StartWatch();
                gridLaneBound.WriteGridToFile("GridLaneBound.txt");
                StopWatch("ObstacleManager - GridLaneBound File Write - ");
            }
//.........这里部分代码省略.........

        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);
//.........这里部分代码省略.........

        private ITrackingCommand BuildForwardPass(out bool finalPass)
        {
            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Determine Forward 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 headingVec90 = headingVec.Rotate90();

            // check if we can make it out now
            Line curLine = new Line(new Coordinates(0,0), headingVec);
            Coordinates intersectionPoint;
            LineSegment finalLine = finalOrientation;
            Circle outCircle = Circle.FromLines(curLine, (Line)finalLine, out intersectionPoint);

            double steeringCommand;

            if (!outCircle.Equals(Circle.Infinite) && outCircle.r > minRadius) {
                // we found an out circle
                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "found final pass output");

                // build the circle segment
                double hitAngle = (intersectionPoint - outCircle.center).ArcTan;
                double startAngle = (-outCircle.center).ArcTan;
                if (hitAngle < startAngle)
                    hitAngle += 2*Math.PI;

                hitAngle -= startAngle;

                if (stopOnLine) {
                    // get the angle of the end point of the line segment
                    double endPointAngle = (finalLine.P1 - outCircle.center).ArcTan;
                    if (endPointAngle < startAngle) endPointAngle += 2*Math.PI;
                    endPointAngle -= startAngle;

                    if (endPointAngle < hitAngle) {
                        hitAngle = endPointAngle;
                    }
                }

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

                // calculate stopping distance
                stopDistance = outCircle.r*hitAngle;
                stopTimestamp = curTimestamp;
                curvature = 1/outCircle.r;

                intersectionPoint = outCircle.center + Coordinates.FromAngle(startAngle + hitAngle)*outCircle.r;

                // calculate steering angle
                steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/outCircle.r, uturnSpeed);

                // mark that this will be the final pass
                finalPass = true;

                Services.UIService.PushCircle(outCircle, curTimestamp, "uturn circle", true);
                Services.UIService.PushPoint(intersectionPoint, curTimestamp, "uturn stop point", true);
                Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
            }
            else {
                finalPass = false;
                // draw out 3 circles
                //  - front right wheel
                //  - front left wheel
                //  - rear left wheel

                // figure out center point of turn
                Circle rearAxleCircle = Circle.FromPointSlopeRadius(Coordinates.Zero, headingVec, 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;

                // initialize min hit angle to slightly less than 90 degrees
                double minHit = Math.PI/2.1;
                //GetMinHitAngle(rearLeftPoint, center, true, ref minHit);
                //GetMinHitAngle(rearRightPoint, center, true, ref minHit);
                GetMinHitAngle(frontLeftPoint, center, ref minHit);
                GetMinHitAngle(frontRightPoint, center, ref minHit);

                // get the obstacles
                List<Polygon> obstacles = GetObstacles(curTimestamp);
                frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2;
                frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0;
//.........这里部分代码省略.........

 /// <summary>
 /// 
 /// </summary>
 /// <param name="anchor"></param>
 /// <param name="vector"></param>
 /// <param name="width"></param>
 /// <returns></returns>
 private static List<Coordinates> InflatePartition(Coordinates anchor, Coordinates vector, double width)
 {
     List<Coordinates> coordList = new List<Coordinates>();
     Coordinates leftVec = vector.Rotate90().Normalize(width / 2.0);
     Coordinates rightVec = vector.RotateM90().Normalize(width / 2.0);
     coordList.Add(anchor + leftVec);
     coordList.Add(anchor + rightVec);
     coordList.Add(anchor + vector + leftVec);
     coordList.Add(anchor + vector + rightVec);
     return coordList;
 }