C# Polygon.Transform方法代码示例

        private Polygon ExtrudeCarPolygon(Polygon obstaclePolygon, double relativeHeading)
        {
            // create a transform to counter rotate the polygon
            Matrix3 transform = Matrix3.Rotation(-relativeHeading);
            Polygon rotatedObstacle = obstaclePolygon.Transform(transform);

            // determine the extreme points along the heading and perpendicular to the heading
            Coordinates headingVec = new Coordinates(1, 0);
            Coordinates perpVec = new Coordinates(0, 1);

            Coordinates topPoint = rotatedObstacle.ExtremePoint(headingVec);
            Coordinates bottomPoint = rotatedObstacle.ExtremePoint(-headingVec);
            Coordinates leftPoint = rotatedObstacle.ExtremePoint(perpVec);
            Coordinates rightPoint = rotatedObstacle.ExtremePoint(-perpVec);

            double height = Math.Abs(topPoint.X-bottomPoint.X);
            double width = Math.Abs(leftPoint.Y-rightPoint.Y);

            // determine the aspect ratio
            double aspectRatio = height/width;

            // target aspect ratio is the tahoe aspect ratio
            double targetAspectRatio = TahoeParams.VL/TahoeParams.T;

            // TODO: fix this so we intelligently determine what directions to expand against instead
            // of just doing it based on adjusting height/width

            if (aspectRatio < targetAspectRatio) {
                // height is less than we would expect
                double newHeight = targetAspectRatio*width;

                // determine the translation vector
                // we'll use the bottom "x" coordinate and left/right average "y" coordinate
                Coordinates offsetVec;
                if (relativeHeading <= Math.PI) {
                    offsetVec = new Coordinates(bottomPoint.X, ((leftPoint+rightPoint)/2.0).Y);
                }
                else {
                    offsetVec = new Coordinates(topPoint.X-newHeight, ((leftPoint+rightPoint)/2.0).Y);
                }

                // create a new polygon with the target width/height
                Polygon poly = new Polygon(4);
                poly.Add((new Coordinates(-extrusion_reverse_dist, width/2.0 + extrusion_extra_width) + offsetVec).Rotate(relativeHeading));
                poly.Add((new Coordinates(-extrusion_reverse_dist, -width/2.0 - extrusion_extra_width) + offsetVec).Rotate(relativeHeading));
                poly.Add((new Coordinates(newHeight, -width/2.0 - extrusion_extra_width) + offsetVec).Rotate(relativeHeading));
                poly.Add((new Coordinates(newHeight, width/2.0 + extrusion_extra_width) + offsetVec).Rotate(relativeHeading));

                return poly;
            }
            else {
                // width is less than we would expect
                double newWidth = height/targetAspectRatio;

                // determine the translation vector
                // we'll use top/bottom average "x" coordinate and left "y" coordinate
                Coordinates offsetVec = new Coordinates(((topPoint+bottomPoint)/2.0).X, leftPoint.Y);

                // create a new polygon with the target width/height and appropriate offset/rotation
                Polygon poly = new Polygon(4);
                poly.Add((new Coordinates(-height/2.0-extrusion_reverse_dist, extrusion_extra_width)+offsetVec).Rotate(relativeHeading));
                poly.Add((new Coordinates(-height/2.0-extrusion_reverse_dist, -newWidth-extrusion_extra_width)+offsetVec).Rotate(relativeHeading));
                poly.Add((new Coordinates(height/2.0, -newWidth-extrusion_extra_width)+offsetVec).Rotate(relativeHeading));
                poly.Add((new Coordinates(height/2.0, extrusion_extra_width)+offsetVec).Rotate(relativeHeading));

                return poly;
            }
        }

        public void PushPolygon(Polygon polygon, CarTimestamp timestamp, string name, bool relative)
        {
            try {
                if (relative) {
                    AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
                    timestamp = absTransform.Timestamp;

                    polygon = polygon.Transform(absTransform);
                }

                Services.Dataset.ItemAs<Polygon>(name).Add(polygon, timestamp);
            }
            catch (Exception ex) {
                OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send polygon data to ui: {0}", ex.Message);
            }
        }

        private double CheckFeelerDist(bool reverse, List<Obstacle> obstacles)
        {
            // create a "feeler box" of 3 m in front of the vehicle

            AbsolutePose pose = Services.StateProvider.GetAbsolutePose();

            Polygon feelerPoly = new Polygon(4);
            if (!reverse) {
                feelerPoly.Add(new Coordinates(TahoeParams.FL, (TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(TahoeParams.FL, -(TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(TahoeParams.FL + 2, -(TahoeParams.T/2.0 + 0.25)));
                feelerPoly.Add(new Coordinates(TahoeParams.FL + 2, (TahoeParams.T/2.0 + 0.25)));
            }
            else {
                feelerPoly.Add(new Coordinates(-TahoeParams.RL, -(TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(-TahoeParams.RL, (TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(-TahoeParams.RL - 2, (TahoeParams.T/2.0 + 0.25)));
                feelerPoly.Add(new Coordinates(-TahoeParams.RL - 2, -(TahoeParams.T/2.0 + 0.25)));
            }

            Matrix3 transform = Matrix3.Translation(pose.xy.X, pose.xy.Y)*Matrix3.Rotation(pose.heading);
            feelerPoly = feelerPoly.Transform(transform);

            double minDist = double.MaxValue;

            foreach (Obstacle obs in obstacles) {
                foreach (Coordinates pt in obs.AvoidancePolygon) {
                    if (feelerPoly.IsInside(pt)) {
                        double dist = pose.xy.DistanceTo(pt);
                        if (dist < minDist) {
                            minDist = dist;
                        }
                    }
                }
            }

            if (reverse) {
                return minDist - TahoeParams.RL;
            }
            else {
                return minDist - TahoeParams.FL;
            }
        }