示例#1
0
        /// <summary>
        /// Finds all candidates points for given GPS track point
        /// </summary>
        /// <param name="gpxPt">GPS point</param>
        /// <returns>Collection of points candidate points on road segments</returns>
        public IEnumerable <CandidatePoint> FindCandidatePoints(GPXPoint gpxPt)
        {
            List <CandidatePoint> result = new List <CandidatePoint>();
            BBox gpxBbox = new BBox(new IPointGeo[] { gpxPt });

            gpxBbox.Inflate(0.0007, 0.0011);

            foreach (var road in _trackCutout)
            {
                if (Topology.Intersects(gpxBbox, road.BBox))
                {
                    Segment <IPointGeo> roadSegment;
                    IPointGeo           projectedPoint = Topology.ProjectPoint(gpxPt, road, out roadSegment);
                    result.Add(new CandidatePoint()
                    {
                        MapPoint               = projectedPoint,
                        Road                   = road,
                        RoadSegment            = roadSegment,
                        ObservationProbability = CalculateObservationProbability(gpxPt, projectedPoint)
                    });
                }
            }

            if (result.Count == 0)
            {
                throw new Exception(string.Format("Can not find any candidate point for {0}", gpxPt));
            }

            return(result);
        }
示例#2
0
文件: Topology.cs 项目: wsgan001/TPM
        /// <summary>
        /// Projects the point to the specific line segment
        /// </summary>
        /// <param name="toProject">The point to be projected</param>
        /// <param name="projectTo">The segment, point will be projected to</param>
        /// <returns>the orthogonaly projected point that lies on the specific line segment</returns>
        /// <remarks>This function uses the sphere Earth approximation</remarks>
        public static IPointGeo ProjectPointSphere(IPointGeo toProject, Segment <IPointGeo> projectTo)
        {
            Vector3 a = new Vector3(projectTo.StartPoint);
            Vector3 b = new Vector3(projectTo.EndPoint);
            Vector3 c = new Vector3(toProject);

            Vector3 greatCircleN  = Vector3.CrossProduct(a, b);
            Vector3 greatCircleCN = Vector3.CrossProduct(c, greatCircleN);
            Vector3 projected     = Vector3.CrossProduct(greatCircleN, greatCircleCN);

            projected.Normalize();

            PointGeo result = projected.ToSpherical();

            double apDistance = Calculations.GetDistance2D(projectTo.StartPoint, result);
            double bpDistance = Calculations.GetDistance2D(projectTo.EndPoint, result);

            if (apDistance + bpDistance - Calculations.GetLength(projectTo) < 0.01)
            {
                return(result);
            }
            else
            {
                return((Calculations.GetDistance2D(projectTo.StartPoint, toProject) < Calculations.GetDistance2D(projectTo.EndPoint, toProject)) ? projectTo.StartPoint : projectTo.EndPoint);
            }
        }
示例#3
0
        /// <summary>
        /// Calculates observation probability
        /// </summary>
        /// <param name="original">GPS track point</param>
        /// <param name="candidate">Candidate point</param>
        /// <returns>double representing probability that GPS track point corresponds with Candidate point</returns>
        double CalculateObservationProbability(GPXPoint original, IPointGeo candidate)
        {
            double sigma    = 30;
            double distance = Calculations.GetDistance2D(original, candidate);

            return(Math.Exp(-distance * distance / (2 * sigma * sigma)) / (sigma * Math.Sqrt(Math.PI * 2)));
        }
示例#4
0
文件: Topology.cs 项目: wsgan001/TPM
            public Vector3(IPointGeo sphericalPoint)
            {
                double lat = Math.PI / 2 - Calculations.ToRadians(sphericalPoint.Latitude);
                double lon = Calculations.ToRadians(sphericalPoint.Longitude);

                X = Math.Sin(lat) * Math.Cos(lon);
                Y = Math.Sin(lat) * Math.Sin(lon);
                Z = Math.Cos(lat);
            }
示例#5
0
        /// <summary>
        /// Initializes a polygon with data from the sigle way
        /// </summary>
        /// <param name="way">The way used to initialize polygon</param>
        //void InitializeWithSingleWay(OSMWay way) {

        //}

        /// <summary>
        /// Adds a vertex to the polygon
        /// </summary>
        /// <param name="vertexId">Vertex id to be added</param>
        /// <exception cref="System.ArgumentException">Throws an ArgumentException if the polygon already contains given vertex.</exception>
        public void AddVertex(IPointGeo vertex)
        {
            if (_vertices.Contains(vertex))
            {
                throw new ArgumentException(String.Format("Polygon already contains vertex {0}", vertex));
            }

            _vertices.Add(vertex);
        }
示例#6
0
        public static double GetBearing(IPointGeo pt1, IPointGeo pt2)
        {
            double dLon = Calculations.ToRadians(pt1.Longitude - pt2.Longitude);
            double dLat = Calculations.ToRadians(pt1.Latitude - pt2.Latitude);

            double y = Math.Sin(dLon) * Math.Cos(Calculations.ToRadians(pt2.Latitude));
            double x = Math.Cos(Calculations.ToRadians(pt1.Latitude)) * Math.Sin(Calculations.ToRadians(pt2.Latitude)) -
                            Math.Sin(Calculations.ToRadians(pt1.Latitude)) * Math.Cos(Calculations.ToRadians(pt2.Latitude)) * Math.Cos(dLon);
            return Calculations.ToDegrees(Math.Atan2(y, x));
        }
示例#7
0
        public static double GetBearing(IPointGeo pt1, IPointGeo pt2)
        {
            double dLon = Calculations.ToRadians(pt1.Longitude - pt2.Longitude);
            double dLat = Calculations.ToRadians(pt1.Latitude - pt2.Latitude);

            double y = Math.Sin(dLon) * Math.Cos(Calculations.ToRadians(pt2.Latitude));
            double x = Math.Cos(Calculations.ToRadians(pt1.Latitude)) * Math.Sin(Calculations.ToRadians(pt2.Latitude)) -
                       Math.Sin(Calculations.ToRadians(pt1.Latitude)) * Math.Cos(Calculations.ToRadians(pt2.Latitude)) * Math.Cos(dLon);

            return(Calculations.ToDegrees(Math.Atan2(y, x)));
        }
示例#8
0
文件: Topology.cs 项目: wsgan001/TPM
        /// <summary>
        /// Translates point in the specific direction by specific distance
        /// </summary>
        /// <param name="point">The point to be translated</param>
        /// <param name="bearing">Bearing from the original point</param>
        /// <param name="distance">Distance from the original point</param>
        /// <returns>the translated point</returns>
        public static PointGeo ProjectPoint(IPointGeo point, double bearing, double distance)
        {
            double lat = Math.Asin(Math.Sin(Calculations.ToRadians(point.Latitude)) * Math.Cos(distance / Calculations.EarthRadius) +
                                   Math.Cos(Calculations.ToRadians(point.Latitude)) * Math.Sin(distance / Calculations.EarthRadius) * Math.Cos(Calculations.ToRadians(bearing)));

            double lon = Calculations.ToRadians(point.Longitude) +
                         Math.Atan2(Math.Sin(Calculations.ToRadians(bearing)) * Math.Sin(distance / Calculations.EarthRadius) * Math.Cos(Calculations.ToRadians(point.Latitude)),
                                    Math.Cos(distance / Calculations.EarthRadius) - Math.Sin(Calculations.ToRadians(point.Latitude)) * Math.Sin(lat));

            return(new PointGeo(Calculations.ToDegrees(lat), Calculations.ToDegrees(lon)));
        }
        /// <summary>
        /// Calculates distance between 2 points on geoid surface (ignores points elevations)
        /// </summary>
        /// <param name="pt1">First point</param>
        /// <param name="pt2">Second point</param>
        /// <returns>distance between given points in meters</returns>
        public double Calculate2D(IPointGeo pt1, IPointGeo pt2)
        {
            const double earthRadius = 6371010.0;

            double dLat = ToRadians(pt2.Latitude - pt1.Latitude);
            double dLon = ToRadians(pt2.Longitude - pt1.Longitude);

            double a = Math.Sin(dLat / 2.0) * Math.Sin(dLat / 2.0) + Math.Cos(ToRadians(pt1.Latitude)) * Math.Cos(ToRadians(pt2.Latitude)) * Math.Sin(dLon / 2.0) * Math.Sin(dLon / 2.0);
            double dAngle = 2 * Math.Asin(Math.Sqrt(a));

            return dAngle * earthRadius;
        }
示例#10
0
        /// <summary>
        /// Saves path to the OSMDB
        /// </summary>
        /// <param name="path">The list of polylines that represent matched path</param>
        /// <returns>OSMDB with path converted to the OSM format</returns>
        public OSMDB SaveToOSM(IList <Polyline <IPointGeo> > path)
        {
            _db        = new OSMDB();
            _dbCounter = -1;

            IPointGeo lastPoint = null;
            OSMNode   node      = null;

            foreach (var line in path)
            {
                if (line.Nodes.Count == 0)
                {
                    continue;
                }

                if (line.Nodes.Count == 1)
                {
                    throw new Exception();
                }

                OSMWay way = new OSMWay(_dbCounter--);
                way.Tags.Add(new OSMTag("way-id", ((PolylineID)line).WayID.ToString()));
                way.Tags.Add(new OSMTag("order", (_db.Ways.Count + 1).ToString()));
                _db.Ways.Add(way);
                foreach (var point in line.Nodes)
                {
                    if (point != lastPoint)
                    {
                        lastPoint = point;
                        PointEx pt = (PointEx)point;

                        node = new OSMNode(_dbCounter--, pt.Latitude, pt.Longitude);
                        if (pt.NodeID != 0)
                        {
                            node.Tags.Add(new OSMTag("node-id", pt.NodeID.ToString()));
                        }
                        if (pt.Time != DateTime.MinValue)
                        {
                            node.Tags.Add(new OSMTag("time", pt.Time.ToString()));
                        }
                        if (pt.Crossroad)
                        {
                            node.Tags.Add(new OSMTag("crossroad", "yes"));
                        }

                        _db.Nodes.Add(node);
                    }
                    way.Nodes.Add(node.ID);
                }
            }

            return(_db);
        }
示例#11
0
        /// <summary>
        /// Calculates distance of the point from the line
        /// </summary>
        /// <param name="point">The point</param>
        /// <param name="line">The line</param>
        /// <returns>the distane of the point from the line in meters</returns>
        public static double GetDistance2D(IPointGeo point, IPolyline <IPointGeo> line)
        {
            double minDistance = double.PositiveInfinity;

            foreach (var segment in line.Segments)
            {
                IPointGeo projectedPoint = Topology.ProjectPoint(point, segment);
                minDistance = Math.Min(minDistance, _distanceCalculator.Calculate2D(point, projectedPoint));
            }

            return(minDistance);
        }
示例#12
0
        /// <summary>
        /// Calculates distance between 2 points on geoid surface (ignores points elevations)
        /// </summary>
        /// <param name="pt1">First point</param>
        /// <param name="pt2">Second point</param>
        /// <returns>distance between given points in meters</returns>
        public double Calculate2D(IPointGeo pt1, IPointGeo pt2)
        {
            const double earthRadius = 6371010.0;

            double dLat = ToRadians(pt2.Latitude - pt1.Latitude);
            double dLon = ToRadians(pt2.Longitude - pt1.Longitude);

            double a      = Math.Sin(dLat / 2.0) * Math.Sin(dLat / 2.0) + Math.Cos(ToRadians(pt1.Latitude)) * Math.Cos(ToRadians(pt2.Latitude)) * Math.Sin(dLon / 2.0) * Math.Sin(dLon / 2.0);
            double dAngle = 2 * Math.Asin(Math.Sqrt(a));

            return(dAngle * earthRadius);
        }
示例#13
0
        /// <summary>
        /// Gets PointEx from the internal storage or creates a new one (for given point)
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        PointEx GetOrCreatePointEx(IPointGeo point, DateTime time)
        {
            if (_points.ContainsKey(point))
            {
                if (_points[point].Time == DateTime.MinValue || _points[point].Time == time)
                {
                    _points[point].Time = time;
                    return(_points[point]);
                }
                else
                {
                    PointEx result = new PointEx()
                    {
                        Latitude = point.Latitude, Longitude = point.Longitude, Elevation = point.Elevation, Time = time
                    };
                    _points[point] = result;

                    OSMNode pointOSM = point as OSMNode;
                    if (pointOSM != null)
                    {
                        result.NodeID = pointOSM.ID;
                        if (pointOSM.Tags.ContainsTag("crossroad"))
                        {
                            result.Crossroad = true;
                        }
                    }
                    return(result);
                }
            }
            else
            {
                PointEx result = new PointEx()
                {
                    Latitude = point.Latitude, Longitude = point.Longitude, Elevation = point.Elevation, Time = time
                };
                _points.Add(point, result);

                OSMNode pointOSM = point as OSMNode;
                if (pointOSM != null)
                {
                    result.NodeID = pointOSM.ID;
                    if (pointOSM.Tags.ContainsTag("crossroad"))
                    {
                        result.Crossroad = true;
                    }
                }
                return(result);
            }
        }
示例#14
0
        /// <summary>
        /// Calculates distance of the point from the line
        /// </summary>
        /// <param name="point">The point</param>
        /// <param name="line">The line</param>
        /// <returns>the distane of the point from the line in meters</returns>
        public static double GetDistance2D(IPointGeo point, IPolyline <IPointGeo> line, ref int closestSegmentIndex)
        {
            double minDistance = double.PositiveInfinity;

            for (int i = 0; i < line.Segments.Count; i++)
            {
                IPointGeo projectedPoint = Topology.ProjectPoint(point, line.Segments[i]);
                double    distance       = _distanceCalculator.Calculate2D(point, projectedPoint);

                if (distance < minDistance)
                {
                    minDistance         = distance;
                    closestSegmentIndex = i;
                }
            }

            return(minDistance);
        }
示例#15
0
文件: Topology.cs 项目: wsgan001/TPM
        /// <summary>
        /// Projects the point to the Polyline
        /// </summary>
        /// <param name="point">The point to be projected</param>
        /// <param name="line">The polyline, point will be projected to</param>
        /// <returns>the orthogonaly projected point that lies on the Polyline</returns>
        public static IPointGeo ProjectPoint(IPointGeo point, IPolyline <IPointGeo> line)
        {
            double    minDistance  = double.PositiveInfinity;
            IPointGeo closestPoint = null;

            foreach (var segment in line.Segments)
            {
                IPointGeo projected = ProjectPoint(point, segment);
                double    distance  = Calculations.GetDistance2D(point, projected);
                if (distance < minDistance)
                {
                    minDistance  = distance;
                    closestPoint = projected;
                }
            }

            return(closestPoint);
        }
示例#16
0
        /// <summary>
        /// Calculates length of the path between two points along the specifid Polyline
        /// </summary>
        /// <param name="from">The point where path starts</param>
        /// <param name="fromSegment">The Segment, where the point From lies</param>
        /// <param name="to">The point where path ends</param>
        /// <param name="toSegment">The Segment, where the point To lies</param>
        /// <param name="path">Polyline that defines path geometry</param>
        /// <returns>The length of the path between points from and to along the polyline</returns>
        public static double GetPathLength(IPointGeo from, Segment <IPointGeo> fromSegment, IPointGeo to, Segment <IPointGeo> toSegment, IPolyline <IPointGeo> path)
        {
            int    pointFound = 0;
            int    pointFoundLast = 0;
            bool   fromFound = false; bool toFound = false;
            double distance = 0;

            foreach (var segment in path.Segments)
            {
                pointFoundLast = pointFound;
                IPointGeo[] points = new IPointGeo[] { segment.StartPoint, segment.EndPoint };

                if (!fromFound && fromSegment.Equals(segment))
                {
                    fromFound            = true;
                    points[pointFound++] = from;
                }

                if (!toFound && toSegment.Equals(segment))
                {
                    toFound = true;
                    points[pointFound++] = to;
                }

                if (pointFound > 0)
                {
                    if (pointFound == pointFoundLast)
                    {
                        distance += segment.Length;
                    }
                    else
                    {
                        distance += Calculations.GetDistance2D(points[0], points[1]);
                    }

                    if (pointFound == 2)
                    {
                        return(distance);
                    }
                }
            }

            throw new ArgumentException("One or more points do not lie on the given path");
        }
示例#17
0
        /// <summary>
        /// Returns nodes on the path between two points
        /// </summary>
        /// <param name="from">The start point</param>
        /// <param name="to">The end point</param>
        /// <param name="path"></param>
        /// <returns></returns>
        public static IEnumerable<IPointGeo> GetNodesBetweenPoints(IPointGeo from, IPointGeo to, IPolyline<IPointGeo> path)
        {
            var segments = path.Segments;

            List<IPointGeo> result = new List<IPointGeo>();

            int fromIndex = -1;
            int toIndex = -1;
            for (int i = 0; i < segments.Count; i++) {
                if (Calculations.GetDistance2D(from, segments[i]) < Calculations.EpsLength) {
                    if (fromIndex > -1 && toIndex > -1 && toIndex <= fromIndex)
                        ;
                    else
                        fromIndex = i;
                }
                if (Calculations.GetDistance2D(to, segments[i]) < Calculations.EpsLength) {
                    if (fromIndex > -1 && toIndex > -1 && toIndex >= fromIndex)
                        ;
                    else
                        toIndex = i;
                }
            }
            if (fromIndex == -1 || toIndex == -1)
                return result;

            if (fromIndex == toIndex - 1) {
                result.Add(segments[fromIndex].EndPoint);
            }
            else if (fromIndex - 1 == toIndex) {
                result.Add(segments[toIndex].EndPoint);
            }
            else if (fromIndex < toIndex) {
                for (int i = fromIndex; i < toIndex; i++) {
                    result.Add(segments[i].EndPoint);
                }
            }
            else if (toIndex < fromIndex) {
                for (int i = fromIndex; i > toIndex; i--) {
                    result.Add(segments[i].StartPoint);
                }
            }

            return result;
        }
示例#18
0
文件: Topology.cs 项目: wsgan001/TPM
        /// <summary>
        /// Projects the point to the specific line segment, this function uses the flat earth aproximation
        /// </summary>
        /// <param name="toProject">The point to be projected</param>
        /// <param name="projectTo">The segment, point will be projected to</param>
        /// <returns>the orthogonaly projected point that lies on the specific line segment</returns>
        /// <remarks>This function uses the flat Earth approximation</remarks>
        public static IPointGeo ProjectPoint(IPointGeo toProject, Segment <IPointGeo> projectTo)
        {
            double u = ((projectTo.EndPoint.Longitude - projectTo.StartPoint.Longitude) * (toProject.Longitude - projectTo.StartPoint.Longitude) +
                        (projectTo.EndPoint.Latitude - projectTo.StartPoint.Latitude) * (toProject.Latitude - projectTo.StartPoint.Latitude)) /
                       (Math.Pow(projectTo.EndPoint.Longitude - projectTo.StartPoint.Longitude, 2) + Math.Pow(projectTo.EndPoint.Latitude - projectTo.StartPoint.Latitude, 2));

            if (u <= 0)
            {
                return(projectTo.StartPoint);
            }
            if (u >= 1)
            {
                return(projectTo.EndPoint);
            }

            double lon = projectTo.StartPoint.Longitude + u * (projectTo.EndPoint.Longitude - projectTo.StartPoint.Longitude);
            double lat = projectTo.StartPoint.Latitude + u * (projectTo.EndPoint.Latitude - projectTo.StartPoint.Latitude);

            return(new PointGeo(lat, lon));
        }
示例#19
0
        /// <summary>
        /// Tests if the specific point is inside of this polygon.
        /// </summary>
        /// <param name="point">The point to be tested.</param>
        /// <returns>Returns true, if the point is inside this polygon, otherwise returns false.</returns>
        /// <remarks>If the point is on the bounadary, the result is undefined</remarks>
        public bool IsInside(IPointGeo point)
        {
            bool oddNodes = false;
            int  j        = _vertices.Count - 1;

            for (int i = 0; i < _vertices.Count; i++)
            {
                if (_vertices[i].Latitude <= point.Latitude && _vertices[j].Latitude >= point.Latitude ||
                    _vertices[j].Latitude <= point.Latitude && _vertices[i].Latitude >= point.Latitude)
                {
                    if (_vertices[i].Longitude + (point.Latitude - _vertices[i].Latitude) /
                        (_vertices[j].Latitude - _vertices[i].Latitude) *
                        (_vertices[j].Longitude - _vertices[i].Longitude) < point.Longitude)
                    {
                        oddNodes = !oddNodes;
                    }
                }
                j = i;
            }

            return(oddNodes);
        }
示例#20
0
        /// <summary>
        /// Creates a new Polyline as part of the result and assigns times for the start and end points
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <param name="fromTime"></param>
        /// <param name="totime"></param>
        /// <param name="road"></param>
        /// <returns></returns>
        Polyline <IPointGeo> CreateLine(IPointGeo from, IPointGeo to, DateTime fromTime, DateTime totime, ConnectionGeometry road, long Id)
        {
            Polyline <IPointGeo> line = new Polyline <IPointGeo>()
            {
                WayID = road.WayID, Id = Id
            };

            PointEx toAdd = GetOrCreatePointEx(from, fromTime);

            line.Nodes.Add(toAdd);

            var points = Topology.GetNodesBetweenPoints(from, to, road);

            foreach (var point in points)
            {
                line.Nodes.Add(GetOrCreatePointEx(point, DateTime.MinValue));
            }

            toAdd = GetOrCreatePointEx(to, totime);
            line.Nodes.Add(toAdd);

            return(line);
        }
示例#21
0
文件: BBox.cs 项目: wsgan001/TPM
        /// <summary>
        /// Extends the BBox to cover the given IPointGeo
        /// </summary>
        /// <param name="toCover">The point to be covered by this BBox</param>
        public void ExtendToCover(IPointGeo toCover)
        {
            if (initialized == false)
            {
                _north = toCover.Latitude;
                _south = toCover.Latitude;
                _east  = toCover.Longitude;
                _west  = toCover.Longitude;

                _minElevation = toCover.Elevation;
                _maxElevation = toCover.Elevation;

                initialized = true;
            }

            _north = Math.Max(_north, toCover.Latitude);
            _south = Math.Min(_south, toCover.Latitude);
            _east  = Math.Max(_east, toCover.Longitude);
            _west  = Math.Min(_west, toCover.Longitude);

            _minElevation = Math.Min(_minElevation, toCover.Elevation);
            _maxElevation = Math.Max(_maxElevation, toCover.Elevation);
        }
示例#22
0
        /// <summary>
        /// Projects the point to the specific line segment
        /// </summary>
        /// <param name="toProject">The point to be projected</param>
        /// <param name="projectTo">The segment, point will be projected to</param>
        /// <returns>the orthogonaly projected point that lies on the specific line segment</returns>
        /// <remarks>This function uses the sphere Earth approximation</remarks>
        public static IPointGeo ProjectPointSphere(IPointGeo toProject, Segment<IPointGeo> projectTo)
        {
            Vector3 a = new Vector3(projectTo.StartPoint);
            Vector3 b = new Vector3(projectTo.EndPoint);
            Vector3 c = new Vector3(toProject);

            Vector3 greatCircleN = Vector3.CrossProduct(a, b);
            Vector3 greatCircleCN = Vector3.CrossProduct(c, greatCircleN);
            Vector3 projected = Vector3.CrossProduct(greatCircleN, greatCircleCN);
            projected.Normalize();

            PointGeo result = projected.ToSpherical();

            double apDistance = Calculations.GetDistance2D(projectTo.StartPoint, result);
            double bpDistance = Calculations.GetDistance2D(projectTo.EndPoint, result);
            if (apDistance + bpDistance - Calculations.GetLength(projectTo) < 0.01) {
                return result;
            }
            else {
                return (Calculations.GetDistance2D(projectTo.StartPoint, toProject) < Calculations.GetDistance2D(projectTo.EndPoint, toProject)) ? projectTo.StartPoint : projectTo.EndPoint;
            }
        }
示例#23
0
        /// <summary>
        /// Translates point in the specific direction by specific distance
        /// </summary>
        /// <param name="point">The point to be translated</param>
        /// <param name="bearing">Bearing from the original point</param>
        /// <param name="distance">Distance from the original point</param>
        /// <returns>the translated point</returns>
        public static PointGeo ProjectPoint(IPointGeo point, double bearing, double distance)
        {
            double lat = Math.Asin(Math.Sin(Calculations.ToRadians(point.Latitude))*Math.Cos(distance / Calculations.EarthRadius) +
                                     Math.Cos(Calculations.ToRadians(point.Latitude))*Math.Sin(distance / Calculations.EarthRadius) * Math.Cos(Calculations.ToRadians(bearing)));

            double lon = Calculations.ToRadians(point.Longitude) +
                           Math.Atan2(Math.Sin(Calculations.ToRadians(bearing)) * Math.Sin(distance/Calculations.EarthRadius) * Math.Cos(Calculations.ToRadians(point.Latitude)),
                              Math.Cos(distance / Calculations.EarthRadius) - Math.Sin(Calculations.ToRadians(point.Latitude)) * Math.Sin(lat));

            return new PointGeo(Calculations.ToDegrees(lat), Calculations.ToDegrees(lon));
        }
示例#24
0
        /// <summary>
        /// Projects the point to the Polyline and sets onSegment out paramater
        /// </summary>
        /// <param name="point">The point to project</param>
        /// <param name="line">The polyline, point will be projected to</param>
        /// <param name="onSegment">The Segment of the Polyline, on which the projected point lies</param>
        /// <returns>the orthogonaly projected point that lies on the Polyline</returns>
        public static IPointGeo ProjectPoint(IPointGeo point, IPolyline<IPointGeo> line, out Segment<IPointGeo> onSegment)
        {
            double minDiatance = double.PositiveInfinity;
            IPointGeo closestPoint = null;
            onSegment = null;

            foreach (var segment in line.Segments) {
                IPointGeo projected = ProjectPoint(point, segment);
                double distance = Calculations.GetDistance2D(point, projected);
                if (distance < minDiatance) {
                    minDiatance = distance;
                    closestPoint = projected;
                    onSegment = segment;
                }
            }

            return closestPoint;
        }
示例#25
0
        /// <summary>
        /// Projects the point to the specific line segment, this function uses the flat earth aproximation
        /// </summary>
        /// <param name="toProject">The point to be projected</param>
        /// <param name="projectTo">The segment, point will be projected to</param>
        /// <returns>the orthogonaly projected point that lies on the specific line segment</returns>
        /// <remarks>This function uses the flat Earth approximation</remarks>
        public static IPointGeo ProjectPoint(IPointGeo toProject, Segment<IPointGeo> projectTo)
        {
            double u = ((projectTo.EndPoint.Longitude - projectTo.StartPoint.Longitude) * (toProject.Longitude - projectTo.StartPoint.Longitude) +
                      (projectTo.EndPoint.Latitude - projectTo.StartPoint.Latitude) * (toProject.Latitude - projectTo.StartPoint.Latitude)) /
                      (Math.Pow(projectTo.EndPoint.Longitude - projectTo.StartPoint.Longitude, 2) + Math.Pow(projectTo.EndPoint.Latitude - projectTo.StartPoint.Latitude, 2));

            if (u <= 0)
                return projectTo.StartPoint;
            if (u >= 1)
                return projectTo.EndPoint;

              double lon = projectTo.StartPoint.Longitude + u * (projectTo.EndPoint.Longitude - projectTo.StartPoint.Longitude);
              double lat = projectTo.StartPoint.Latitude + u * (projectTo.EndPoint.Latitude - projectTo.StartPoint.Latitude);

              return new PointGeo(lat, lon);
        }
示例#26
0
 /// <summary>
 /// Calculates length of the pathe between two points along the specific segment
 /// </summary>
 /// <param name="from">The point where path starts</param>
 /// <param name="to">The point where path ends</param>
 /// <param name="path">Segment that defines path geometry</param>
 /// <returns>The length of the path between points from and to along the segment</returns>
 public static double GetPathLength(IPointGeo from, IPointGeo to, Segment<IPointGeo> path)
 {
     return Calculations.GetDistance2D(from, to);
 }
示例#27
0
 /// <summary>
 /// Removes given vertex from the polygon.
 /// </summary>
 /// <param name="vertex">The vertex to be removed</param>
 /// <returns>Returns true if the vertex was succefully removed, otherwise returns false.</returns>
 public bool RemoveVertex(IPointGeo vertex)
 {
     return(_vertices.Remove(vertex));
 }
示例#28
0
文件: BBox.cs 项目: wsgan001/TPM
 /// <summary>
 /// Tests if the specific point is inside this BBox
 /// </summary>
 /// <param name="point">The point to be tested</param>
 /// <returns>True if the point is inside this BBox or on the boundary of this BBox otherwise returns false</returns>
 public bool IsInside(IPointGeo point)
 {
     return(point.Latitude <= _north && point.Latitude >= _south &&
            point.Longitude <= _east && point.Longitude >= _west &&
            point.Elevation <= _maxElevation && point.Elevation >= _minElevation);
 }
示例#29
0
 /// <summary>
 /// Creates a new Node with the specific position
 /// </summary>
 /// <param name="mapPoint">The position of the node in geographic coordinates</param>
 public Node(IPointGeo mapPoint)
 {
     this._connections = new List <Connection>();
     this.MapPoint     = mapPoint;
 }
示例#30
0
        /// <summary>
        /// Calculates distance of the point from the line
        /// </summary>
        /// <param name="point">The point</param>
        /// <param name="line">The line</param>
        /// <returns>the distane of the point from the line in meters</returns>
        public static double GetDistance2D(IPointGeo point, IPolyline<IPointGeo> line, ref int closestSegmentIndex)
        {
            double minDistance = double.PositiveInfinity;

            for(int i = 0; i < line.Segments.Count; i++) {
                IPointGeo projectedPoint = Topology.ProjectPoint(point, line.Segments[i]);
                double distance = _distanceCalculator.Calculate2D(point, projectedPoint);

                if (distance < minDistance) {
                    minDistance = distance;
                    closestSegmentIndex = i;
                }
            }

            return minDistance;
        }
示例#31
0
        /// <summary>
        /// Calculates distance of the point from the segment
        /// </summary>
        /// <param name="point">The point</param>
        /// <param name="segment">The segment</param>
        /// <returns>the distance of point from the segment in meters</returns>
        public static double GetDistance2D(IPointGeo point, Segment <IPointGeo> segment)
        {
            IPointGeo projectedPoint = Topology.ProjectPoint(point, segment);

            return(_distanceCalculator.Calculate2D(point, projectedPoint));
        }
示例#32
0
 /// <summary>
 /// Calculates length of the pathe between two points along the specific segment
 /// </summary>
 /// <param name="from">The point where path starts</param>
 /// <param name="to">The point where path ends</param>
 /// <param name="path">Segment that defines path geometry</param>
 /// <returns>The length of the path between points from and to along the segment</returns>
 public static double GetPathLength(IPointGeo from, IPointGeo to, Segment <IPointGeo> path)
 {
     return(Calculations.GetDistance2D(from, to));
 }
示例#33
0
 /// <summary>
 /// Calculates distance between 2 points on geoid surface (ignores points elevations)
 /// </summary>
 /// <param name="pt1">First point</param>
 /// <param name="pt2">Second point</param>
 /// <returns>distance between given points in meters</returns>
 public static double GetDistance2D(IPointGeo pt1, IPointGeo pt2)
 {
     return _distanceCalculator.Calculate2D(pt1, pt2);
 }
示例#34
0
 public Vector3(IPointGeo sphericalPoint)
 {
     double lat = Math.PI / 2 - Calculations.ToRadians(sphericalPoint.Latitude);
     double lon = Calculations.ToRadians(sphericalPoint.Longitude);
     X = Math.Sin(lat) * Math.Cos(lon);
     Y = Math.Sin(lat) * Math.Sin(lon);
     Z = Math.Cos(lat);
 }
示例#35
0
文件: Topology.cs 项目: wsgan001/TPM
        /// <summary>
        /// Returns nodes on the path between two points
        /// </summary>
        /// <param name="from">The start point</param>
        /// <param name="to">The end point</param>
        /// <param name="path"></param>
        /// <returns></returns>
        public static IEnumerable <IPointGeo> GetNodesBetweenPoints(IPointGeo from, IPointGeo to, IPolyline <IPointGeo> path)
        {
            var segments = path.Segments;

            List <IPointGeo> result = new List <IPointGeo>();

            int fromIndex = -1;
            int toIndex   = -1;

            for (int i = 0; i < segments.Count; i++)
            {
                if (Calculations.GetDistance2D(from, segments[i]) < Calculations.EpsLength)
                {
                    if (fromIndex > -1 && toIndex > -1 && toIndex <= fromIndex)
                    {
                        ;
                    }
                    else
                    {
                        fromIndex = i;
                    }
                }
                if (Calculations.GetDistance2D(to, segments[i]) < Calculations.EpsLength)
                {
                    if (fromIndex > -1 && toIndex > -1 && toIndex >= fromIndex)
                    {
                        ;
                    }
                    else
                    {
                        toIndex = i;
                    }
                }
            }
            if (fromIndex == -1 || toIndex == -1)
            {
                return(result);
            }

            if (fromIndex == toIndex - 1)
            {
                result.Add(segments[fromIndex].EndPoint);
            }
            else if (fromIndex - 1 == toIndex)
            {
                result.Add(segments[toIndex].EndPoint);
            }
            else if (fromIndex < toIndex)
            {
                for (int i = fromIndex; i < toIndex; i++)
                {
                    result.Add(segments[i].EndPoint);
                }
            }
            else if (toIndex < fromIndex)
            {
                for (int i = fromIndex; i > toIndex; i--)
                {
                    result.Add(segments[i].StartPoint);
                }
            }

            return(result);
        }
示例#36
0
 /// <summary>
 /// Tests if the specific point is inside this BBox
 /// </summary>
 /// <param name="point">The point to be tested</param>
 /// <returns>True if the point is inside this BBox or on the boundary of this BBox otherwise returns false</returns>
 public bool IsInside2D(IPointGeo point)
 {
     return point.Latitude <= _north && point.Latitude >= _south &&
                  point.Longitude <= _east && point.Longitude >= _west;
 }
示例#37
0
 /// <summary>
 /// Tests if the specific point is inside this BBox
 /// </summary>
 /// <param name="point">The point to be tested</param>
 /// <returns>True if the point is inside this BBox or on the boundary of this BBox otherwise returns false</returns>
 public bool IsInside(IPointGeo point)
 {
     return point.Latitude <= _north && point.Latitude >= _south &&
                  point.Longitude <= _east && point.Longitude >= _west &&
                  point.Elevation <= _maxElevation && point.Elevation >= _minElevation;
 }
示例#38
0
文件: BBox.cs 项目: wsgan001/TPM
 /// <summary>
 /// Tests if the specific point is inside this BBox
 /// </summary>
 /// <param name="point">The point to be tested</param>
 /// <returns>True if the point is inside this BBox or on the boundary of this BBox otherwise returns false</returns>
 public bool IsInside2D(IPointGeo point)
 {
     return(point.Latitude <= _north && point.Latitude >= _south &&
            point.Longitude <= _east && point.Longitude >= _west);
 }
示例#39
0
        /// <summary>
        /// Calculates distance of the point from the line
        /// </summary>
        /// <param name="point">The point</param>
        /// <param name="line">The line</param>
        /// <returns>the distane of the point from the line in meters</returns>
        public static double GetDistance2D(IPointGeo point, IPolyline<IPointGeo> line)
        {
            double minDistance = double.PositiveInfinity;

            foreach (var segment in line.Segments) {
                IPointGeo projectedPoint = Topology.ProjectPoint(point, segment);
                minDistance = Math.Min(minDistance, _distanceCalculator.Calculate2D(point, projectedPoint));
            }

            return minDistance;
        }
示例#40
0
 /// <summary>
 /// Creates a new Polyline as part of the result
 /// </summary>
 /// <param name="from"></param>
 /// <param name="to"></param>
 /// <param name="road"></param>
 /// <returns></returns>
 Polyline <IPointGeo> CreateLine(IPointGeo from, IPointGeo to, ConnectionGeometry road, long Id)
 {
     return(CreateLine(from, to, DateTime.MinValue, DateTime.MinValue, road, Id));
 }
示例#41
0
        /// <summary>
        /// Calculates length of the path between two points along the specifid Polyline
        /// </summary>
        /// <param name="from">The point where path starts</param>
        /// <param name="fromSegment">The Segment, where the point From lies</param>
        /// <param name="to">The point where path ends</param>
        /// <param name="toSegment">The Segment, where the point To lies</param>
        /// <param name="path">Polyline that defines path geometry</param>
        /// <returns>The length of the path between points from and to along the polyline</returns>
        public static double GetPathLength(IPointGeo from, Segment<IPointGeo> fromSegment, IPointGeo to, Segment<IPointGeo> toSegment, IPolyline<IPointGeo> path)
        {
            int pointFound = 0;
            int pointFoundLast = 0;
            bool fromFound = false; bool toFound = false;
            double distance = 0;
            foreach (var segment in path.Segments) {
                pointFoundLast = pointFound;
                IPointGeo[] points = new IPointGeo[] { segment.StartPoint, segment.EndPoint };

                if (!fromFound && fromSegment.Equals(segment)) {
                    fromFound = true;
                    points[pointFound++] = from;
                }

                if (!toFound && toSegment.Equals(segment)) {
                    toFound = true;
                    points[pointFound++] = to;
                }

                if (pointFound > 0) {
                    if (pointFound == pointFoundLast)
                        distance += segment.Length;
                    else
                        distance += Calculations.GetDistance2D(points[0], points[1]);

                    if (pointFound == 2)
                        return distance;
                }
            }

            throw new ArgumentException("One or more points do not lie on the given path");
        }
示例#42
0
 /// <summary>
 /// Calculates distance of the point from the segment
 /// </summary>
 /// <param name="point">The point</param>
 /// <param name="segment">The segment</param>
 /// <returns>the distance of point from the segment in meters</returns>
 public static double GetDistance2D(IPointGeo point, Segment<IPointGeo> segment)
 {
     IPointGeo projectedPoint = Topology.ProjectPoint(point, segment);
     return _distanceCalculator.Calculate2D(point, projectedPoint);
 }
示例#43
0
        /// <summary>
        /// Filtes Uturns shorter then mamUTurnLength from the path
        /// </summary>
        /// <param name="path">List of the polylines that represents matched path</param>
        /// <param name="maxUTurnLength">Maximal length of the u-turn in meters</param>
        public void FilterUturns(IList <Polyline <IPointGeo> > path, double maxUTurnLength)
        {
            if (path == null || path.Count == 0)
            {
                return;
            }

            IPointGeo            lastNonUTurn    = path[0].Nodes[0];
            Polyline <IPointGeo> lastNonUTurnWay = path[0];

            int index = 0;

            while (index < path.Count)
            {
                Polyline <IPointGeo> current = path[index];

                if (current.Length < Calculations.EpsLength)
                {
                    index++;
                    continue;
                }

                if (current.Length > maxUTurnLength)
                {
                    lastNonUTurn    = current.Nodes.Last();
                    lastNonUTurnWay = current;
                    index++;
                    continue;
                }

                int previousLineIndex = GetPreviousNonZeroLengthLineIndex(path, index - 1);

                if (previousLineIndex > -1 && IsUTurn(path[previousLineIndex], current))
                {
                    for (int i = previousLineIndex + 1; i < index; i++)
                    {
                        path[i].Nodes.Clear();
                    }

                    // forth and back to the same point
                    if (Calculations.GetDistance2D(path[previousLineIndex].Nodes[0], current.Nodes[current.Nodes.Count - 1]) < Calculations.EpsLength)
                    {
                        IPointGeo from = path[previousLineIndex].Nodes[0];
                        IPointGeo to   = current.Nodes[current.Nodes.Count - 1];
                        ((PointEx)to).Time = DateTime.MinValue;

                        path[previousLineIndex].Nodes.Clear();
                        current.Nodes.Clear();
                        current.Nodes.Add(from);
                        current.Nodes.Add(to);
                    }
                    else
                    {
                        int currentSegmentIndex = 0;
                        int lastSegmentIndex    = path[previousLineIndex].Segments.Count - 1;

                        int j = 0;
                        while (j < current.Segments.Count &&
                               Calculations.GetDistance2D(current.Segments[j].EndPoint, path[previousLineIndex], ref lastSegmentIndex) < Calculations.EpsLength)
                        {
                            currentSegmentIndex = j;
                            j++;
                        }

                        // delete the whole previous line
                        if (lastSegmentIndex == 0)
                        {
                            IPointGeo from = path[previousLineIndex].Nodes[0];
                            IPointGeo to   = current.Segments[currentSegmentIndex].EndPoint;
                            ((PointEx)to).Time = DateTime.MinValue;

                            while (current.Nodes[0] != to)
                            {
                                current.Nodes.RemoveAt(0);
                            }
                            current.Nodes.Insert(0, from);

                            path[previousLineIndex].Nodes.Clear();
                        }
                        // delete the whole current line
                        else if (currentSegmentIndex == current.Segments.Count - 1)
                        {
                            IPointGeo from = path[previousLineIndex].Segments[lastSegmentIndex].StartPoint;
                            IPointGeo to   = current.Nodes[current.Nodes.Count - 1];
                            ((PointEx)to).Time = DateTime.MinValue;

                            while (path[previousLineIndex].Nodes[path[previousLineIndex].Nodes.Count - 1] != from)
                            {
                                path[previousLineIndex].Nodes.RemoveAt(path[previousLineIndex].Nodes.Count - 1);
                            }

                            path[previousLineIndex].Nodes.Add(to);
                            current.Nodes.Clear();
                        }
                    }
                }
                else
                {
                    index++;
                }
            }

            index = 0;
            while (index < path.Count)
            {
                if (path[index].Nodes.Count == 0)
                {
                    path.RemoveAt(index);
                }
                else
                {
                    index++;
                }
            }

            while (path.Count > 0 && ((PointEx)path[0].Nodes[0]).Time == DateTime.MinValue)
            {
                path.RemoveAt(0);
            }

            while (path.Count > 0 && ((PointEx)path.Last().Nodes.Last()).Time == DateTime.MinValue)
            {
                path.RemoveAt(path.Count - 1);
            }
        }
示例#44
0
 /// <summary>
 /// Calculates distance between 2 points on geoid surface (ignores points elevations)
 /// </summary>
 /// <param name="pt1">First point</param>
 /// <param name="pt2">Second point</param>
 /// <returns>distance between given points in meters</returns>
 public static double GetDistance2D(IPointGeo pt1, IPointGeo pt2)
 {
     return(_distanceCalculator.Calculate2D(pt1, pt2));
 }
示例#45
0
        /// <summary>
        /// Extends the BBox to cover the given IPointGeo
        /// </summary>
        /// <param name="toCover">The point to be covered by this BBox</param>
        public void ExtendToCover(IPointGeo toCover)
        {
            if (initialized == false) {
                _north = toCover.Latitude;
                _south = toCover.Latitude;
                _east = toCover.Longitude;
                _west = toCover.Longitude;

                _minElevation = toCover.Elevation;
                _maxElevation = toCover.Elevation;

                initialized = true;
            }

            _north = Math.Max(_north, toCover.Latitude);
            _south = Math.Min(_south, toCover.Latitude);
            _east = Math.Max(_east, toCover.Longitude);
            _west = Math.Min(_west, toCover.Longitude);

            _minElevation = Math.Min(_minElevation, toCover.Elevation);
            _maxElevation = Math.Max(_maxElevation, toCover.Elevation);
        }
示例#46
0
 /// <summary>
 /// Calculates observation probability
 /// </summary>
 /// <param name="original">GPS track point</param>
 /// <param name="candidate">Candidate point</param>
 /// <returns>double representing probability that GPS track point corresponds with Candidate point</returns>
 double CalculateObservationProbability(GPXPoint original, IPointGeo candidate)
 {
     double sigma = 30;
     double distance = Calculations.GetDistance2D(original, candidate);
     return Math.Exp(-distance * distance / (2 * sigma * sigma)) / (sigma * Math.Sqrt(Math.PI * 2));
 }
示例#47
0
 /// <summary>
 /// Creates a new Node with the specific position
 /// </summary>
 /// <param name="mapPoint">The position of the node in geographic coordinates</param>
 public Node(IPointGeo mapPoint)
 {
     this._connections = new List<Connection>();
     this.MapPoint = mapPoint;
 }