C# Coordinates.Rotate方法代码示例

        public static void DrawArrow(IGraphics g, Coordinates startingLoc, Coordinates direction, double len, double headSize, Color lineColor, WorldTransform wt)
        {
            Coordinates endingLoc = startingLoc + direction.Normalize(len);
            Coordinates headPt0 = endingLoc + direction.Rotate(135*Math.PI/180.0).Normalize(headSize);
            Coordinates headPt1 = endingLoc + direction.Rotate(-135*Math.PI/180.0).Normalize(headSize);

            IPen pen = g.CreatePen();
            pen.Width = 3/wt.Scale;
            pen.Color = Color.White;

            PointF ptfStart = Utility.ToPointF(startingLoc);
            PointF ptfEnd = Utility.ToPointF(endingLoc);
            PointF ptfHeadPt0 = Utility.ToPointF(headPt0);
            PointF ptfHeadPt1 = Utility.ToPointF(headPt1);

            PointF[] headPts = new PointF[] { ptfHeadPt0, ptfEnd, ptfHeadPt1 };
            g.DrawLine(pen, ptfStart, ptfEnd);
            g.DrawLines(pen, headPts);

            pen.Width = 1/wt.Scale;
            pen.Color = lineColor;
            g.DrawLine(pen, ptfStart, ptfEnd);
            g.DrawLines(pen, headPts);
        }

        // get approximated obstacle points centered on grid cells
        public void GetObstacles(double vehicleHeading, out Coordinates[] obstaclePoints)
        {
            List<Coordinates> points = new List<Coordinates>();

            // convert occupancy grid data back to obstacle points (approximate using center of grid cell)
            Coordinates point;
            for (int x = 0; x < xSize; x++) {
                for (int y = 0; y < ySize; y++) {
                    if (GetValue(x, y) == CELL_FULL) {
                        // transform to vehicle relative coordinates
                        point = new Coordinates((x - middle.X) * gridStep, (y - middle.Y) * gridStep);
                        points.Add(point.Rotate(-vehicleHeading));
                    }
                }
            }

            obstaclePoints = points.ToArray();
        }

        /// <summary>
        /// Set static obstacles given as vectors relative from imu
        /// </summary>
        /// <param name="obstacles">observed obstacles in relative vectors from imu</param>
        private void SetStaticObstacles(Path path, ObservedObstacles observedObstacles)
        {
            // clear static obstacles
            staticObstaclesIn.Clear();
            staticObstaclesOut.Clear();
            staticObstaclesFake.Clear();

            // determine sensor region to group obstacles
            DefineSensorRegion(path);

            List<PointOnPath> pop = new List<PointOnPath>();

            // prepare static obstacles for obstacle reasoning
            for (int i = 0; i < observedObstacles.Obstacles.Length; i++) {
                // transform obstacle to absolute coordinates
                Coordinates obs = new Coordinates(TahoeParams.IL, 0);
                obs += observedObstacles.Obstacles[i].ObstacleVector;
                obs = obs.Rotate(vehicleHeading) + vehiclePosition;

                // sort out static obstacles
                if (sensorPolygon.IsInside(obs) == true)
                    staticObstaclesIn.Add(obs);
                else
                    staticObstaclesOut.Add(obs);
            }
        }

        /// <summary>
        /// Get the vehicle direction along a lane
        /// </summary>
        /// <param name="lane"></param>
        /// <returns></returns>
        public VehicleDirectionAlongPath VehicleDirectionAlong(IFQMPlanable lane)
        {
            if (this.IsStopped || !this.StateMonitor.Observed.headingValid)
                return VehicleDirectionAlongPath.Forwards;

            // get point on the lane path
            LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(this.ClosestPosition);

            // get heading of the lane path there
            Coordinates pathVector = lane.LanePath().GetSegment(pop.Index).UnitVector;

            // get vehicle heading
            Coordinates unit = new Coordinates(1, 0);
            Coordinates headingVector = unit.Rotate(this.StateMonitor.Observed.absoluteHeading);

            // rotate vehicle heading
            Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan);

            // get path heading
            double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees());

            if (relativeVehicleDegrees < 70)
                return VehicleDirectionAlongPath.Forwards;
            else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
                return VehicleDirectionAlongPath.Perpendicular;
            else
                return VehicleDirectionAlongPath.Reverse;
        }

 public Coordinates TransformCoordAbs(Coordinates c, VehicleState state)
 {
     c = c.Rotate(state.Heading.ArcTan);
     c = c + state.Position;
     return c;
 }

        public DirectionAlong DirectionAlongSegment(LinePath lp)
        {
            // get heading of the lane path there
            Coordinates pathVector = lp.GetSegment(0).UnitVector;

            // get vehicle heading
            Coordinates unit = new Coordinates(1, 0);
            Coordinates headingVector = unit.Rotate(this.Heading.ArcTan);

            // rotate vehicle heading
            Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan);

            // get path heading
            double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees());

            if (relativeVehicleDegrees < 70)
                return DirectionAlong.Forwards;
            else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
                return DirectionAlong.Perpendicular;
            else
                return DirectionAlong.Reverse;
        }

        public void SetTurnDirection(ArbiterInterconnect ai)
        {
            #region Turn Direction

            if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
            {
                ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
                ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;

                // check not uturn
                if (!initWp.Lane.Way.Segment.Equals(finWp.Lane.Way.Segment) || initWp.Lane.Way.Equals(finWp.Lane.Way))
                {
                    Coordinates iVec = initWp.PreviousPartition != null ? initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
                    double iRot = -iVec.ArcTan;

                    Coordinates fVec = finWp.NextPartition != null ? finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    double fDeg = fVec.ToDegrees();

                    double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;

                    if (arcTan > 45.0)
                        ai.TurnDirection = ArbiterTurnDirection.Left;
                    else if (arcTan < -45.0)
                        ai.TurnDirection = ArbiterTurnDirection.Right;
                    else
                        ai.TurnDirection = ArbiterTurnDirection.Straight;
                }
                else
                {
                    Coordinates iVec = initWp.PreviousPartition != null ? initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
                    double iRot = -iVec.ArcTan;

                    Coordinates fVec = finWp.NextPartition != null ? finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    double fDeg = fVec.ToDegrees();

                    double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;

                    if (arcTan > 45.0 && arcTan < 135.0)
                        ai.TurnDirection = ArbiterTurnDirection.Left;
                    else if (arcTan < -45.0 && arcTan > -135.0)
                        ai.TurnDirection = ArbiterTurnDirection.Right;
                    else if (Math.Abs(arcTan) < 45.0)
                        ai.TurnDirection = ArbiterTurnDirection.Straight;
                    else
                        ai.TurnDirection = ArbiterTurnDirection.UTurn;
                }
            }
            else
            {
                Coordinates iVec = new Coordinates();
                double iRot = 0.0;
                Coordinates fVec = new Coordinates();
                double fDeg = 0.0;

                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
                    iVec = initWp.PreviousPartition != null ? initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
                    iRot = -iVec.ArcTan;
                }
                else if (ai.InitialGeneric is ArbiterPerimeterWaypoint)
                {
                    ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.InitialGeneric;
                    Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
                    iVec = apw.Position - centerPoly;
                    iVec = iVec.Normalize(1.0);
                    iRot = -iVec.ArcTan;
                }

                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;
                    fVec = finWp.NextPartition != null ? finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    fDeg = fVec.ToDegrees();
                }
                else if (ai.FinalGeneric is ArbiterPerimeterWaypoint)
                {
                    ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
                    Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
                    fVec = centerPoly - apw.Position;
                    fVec = fVec.Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    fDeg = fVec.ToDegrees();
                }

                double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;

                if (arcTan > 45.0)
                    ai.TurnDirection = ArbiterTurnDirection.Left;
                else if (arcTan < -45.0)
                    ai.TurnDirection = ArbiterTurnDirection.Right;
                else
                    ai.TurnDirection = ArbiterTurnDirection.Straight;
            }

            #endregion
        }