C# Coordinates.Normalize方法代码示例

        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);
        }

        /// <summary>
        /// Produces an obstacle reasoning path for lane type situations (Version 2b - Latest)
        /// </summary>
        /// <param name="leftLanePath">The path representing the left lane</param>
        /// <param name="leftLaneIsOncoming">Whether the left lane path is oncoming or not</param>
        /// <param name="leftLaneVehicles">The vehicles referenced to the left lane</param>
        /// <param name="currentLaneDefaultPath">The default path for the current lane</param>
        /// <param name="rightLanePath">The path of the right lane, always going in our same direction</param>
        /// <param name="rightLaneVehicles">The vehicles referenced to the right lane</param>
        /// <param name="vehicleState">Our current vehicle state</param>
        /// <returns>A modified lane path that avoids the vehicles in the adjacent lanes while staying in the current lane</returns>
        public Path LaneObstacleReasoning(Path leftLanePath, bool leftLaneIsOncoming,
            ObservedVehicle[] leftLaneVehicles,
            Path currentLanePath,
            Path rightLanePath,
            ObservedVehicle[] rightLaneVehicles,
            VehicleState vehicleState)
        {
            // set up vehicle and lane information
            InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
                                                        leftLanePath, currentLanePath, rightLanePath);

            // set up static obstacles (none for now)
            staticObstaclesIn.Clear();
            staticObstaclesOut.Clear();
            staticObstaclesFake.Clear();

            // set up dynamic obstacles
            dynamicObstacles.Clear();
            dynamicObstaclesPaths.Clear();
            SetDynamicObstacles(leftLanePath, leftLaneVehicles);
            SetDynamicObstacles(rightLanePath, rightLaneVehicles);

            double projectionDist = Math.Max(vehicleSpeed * 5, 10) + TahoeParams.FL;
            double origProjectionDist = projectionDist;

            // set up number of paths based on lane width
            double spacing = 0.25;
            int numPaths = (int)Math.Round(currentLaneWidth / spacing);
            if ((int)Math.IEEERemainder((double)numPaths, 2.0) == 0)
                numPaths -= 1;

            // increase number of drift paths
            int midPathIndex;
            if (leftLaneIsOncoming == true) {
                midPathIndex = (numPaths - 1) / 2;
                numPaths += 6;
            }
            else {
                numPaths += 12;
                midPathIndex = (numPaths - 1) / 2;
            }

            double[] pathsRisk, pathsRiskDist, pathsSepDist, pathsCost;
            Path[] paths = new Path[numPaths];
            int selectedPathIndex;

            do {
                // lookahead point
                double lookaheadDist = projectionDist;
                PointOnPath lookaheadPt = currentLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);

                // extend point if at end of path
                Coordinates offsetVec = new Coordinates(0, 0);
                if (lookaheadDist > 0.5)
                    offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);

                // prepare ctrl points for spline path
                Coordinates startPoint = vehiclePosition;
                Coordinates endPoint = lookaheadPt.pt + offsetVec;
                Coordinates startVec = new Coordinates(1, 0).Rotate(vehicleHeading).Normalize(Math.Max(vehicleSpeed, 2.0));
                Coordinates endVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));

                // generate mid points of path
                int midPointsTotal = (int)Math.Round(projectionDist / 5.0) - 1;
                double midPointStepDist = projectionDist / (midPointsTotal + 1);
                Coordinates[] midPoints = new Coordinates[midPointsTotal];
                Coordinates[] midVecs		= new Coordinates[midPointsTotal];
                Coordinates[] midShiftVecs = new Coordinates[midPointsTotal];
                for (int i = 0; i < midPointsTotal; i++) {
                    // lookahead point
                    lookaheadDist = projectionDist * (i + 1) / (midPointsTotal + 1);
                    lookaheadPt = currentLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);

                    // extend point if at end of path
                    offsetVec = new Coordinates(0, 0);
                    if (lookaheadDist > 0.5)
                        offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);

                    // prepare ctrl points for spline path
                    midPoints[i] = lookaheadPt.pt + offsetVec;
                    midVecs[i]	 = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
                    midShiftVecs[i] = midVecs[i].Rotate90();
                }

                Coordinates endShiftVec = endVec.Rotate90();

                // generate multiple spline paths
                for (int i = 0; i < numPaths; i++) {

//.........这里部分代码省略.........

        /// <summary>
        /// Gets the middle two control points of an intersection given the starting and ending vectors
        /// </summary>
        /// <param name="startPoint"></param>
        /// <param name="endPoint"></param>
        /// <param name="startVec"></param>
        /// <param name="endVec"></param>
        /// <returns></returns>
        /// <remarks>Start point in and End should also pouint it</remarks>
        public static MiddleControlPoints MiddleIntersectionControlPoints(Coordinates exit, 
            Coordinates entry, Coordinates exitInVector, Coordinates entryInVector)
        {
            // equally normalize the heading vectos
            entryInVector.Normalize();
            exitInVector.Normalize();

            // get the vector from the exit point to the entry point
            Coordinates p = exit - entry;

            // get the difference in heading vectors
            Coordinates w = exitInVector - entryInVector;

            // get the magnitude of the difference in headings
            double d = w.Length;

            // the time given that the headings represent velocities
            double time2cpa = 0;

            // do something
            if (Math.Abs(d) > 1e-6)
                time2cpa = -p.Dot(w) / Math.Pow(d, 2);

            // get the middle control points
            MiddleControlPoints mcps = new MiddleControlPoints(exit + exitInVector.Normalize(time2cpa),
                entry + entryInVector.Normalize(time2cpa));

            return mcps;
        }

        public void SymmetricEigenDecomposition(out double[] eigenvalues, out Coordinates[] eigenvectors)
        {
            double t = Math.Sqrt(4*d12*d12 + (d11-d22)*(d11-d22));
            double lamda1 = (d11+d22+t)/2;
            double lamda2 = (d11+d22-t)/2;

            Coordinates v1 = new Coordinates(1, (d22-d11+t)/(2*d12));
            Coordinates v2 = new Coordinates(1, (d22-d11-t)/(2*d12));

            eigenvalues = new double[] { lamda1, lamda2 };
            eigenvectors = new Coordinates[] { v1.Normalize(), v2.Normalize() };
        }

        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
        }