001    // License: GPL. For details, see LICENSE file.
002    package org.openstreetmap.josm.tools;
003    
004    import java.awt.Rectangle;
005    import java.awt.geom.Area;
006    import java.awt.geom.Line2D;
007    import java.awt.geom.Path2D;
008    import java.math.BigDecimal;
009    import java.math.MathContext;
010    import java.util.ArrayList;
011    import java.util.Comparator;
012    import java.util.LinkedHashSet;
013    import java.util.List;
014    import java.util.Set;
015    
016    import org.openstreetmap.josm.Main;
017    import org.openstreetmap.josm.command.AddCommand;
018    import org.openstreetmap.josm.command.ChangeCommand;
019    import org.openstreetmap.josm.command.Command;
020    import org.openstreetmap.josm.data.coor.EastNorth;
021    import org.openstreetmap.josm.data.coor.LatLon;
022    import org.openstreetmap.josm.data.osm.BBox;
023    import org.openstreetmap.josm.data.osm.Node;
024    import org.openstreetmap.josm.data.osm.NodePositionComparator;
025    import org.openstreetmap.josm.data.osm.Way;
026    
027    /**
028     * Some tools for geometry related tasks.
029     *
030     * @author viesturs
031     */
032    public class Geometry {
033        public enum PolygonIntersection {FIRST_INSIDE_SECOND, SECOND_INSIDE_FIRST, OUTSIDE, CROSSING}
034    
035        /**
036         * Will find all intersection and add nodes there for list of given ways.
037         * Handles self-intersections too.
038         * And makes commands to add the intersection points to ways.
039         *
040         * Prerequisite: no two nodes have the same coordinates.
041         * 
042         * @param ways  a list of ways to test
043         * @param test  if false, do not build list of Commands, just return nodes
044         * @param cmds  list of commands, typically empty when handed to this method.
045         *              Will be filled with commands that add intersection nodes to
046         *              the ways.
047         * @return list of new nodes
048         */
049        public static Set<Node> addIntersections(List<Way> ways, boolean test, List<Command> cmds) {
050    
051            //stupid java, cannot instantiate array of generic classes..
052            @SuppressWarnings("unchecked")
053            ArrayList<Node>[] newNodes = new ArrayList[ways.size()];
054            BBox[] wayBounds = new BBox[ways.size()];
055            boolean[] changedWays = new boolean[ways.size()];
056    
057            Set<Node> intersectionNodes = new LinkedHashSet<Node>();
058    
059            //copy node arrays for local usage.
060            for (int pos = 0; pos < ways.size(); pos ++) {
061                newNodes[pos] = new ArrayList<Node>(ways.get(pos).getNodes());
062                wayBounds[pos] = getNodesBounds(newNodes[pos]);
063                changedWays[pos] = false;
064            }
065    
066            //iterate over all way pairs and introduce the intersections
067            Comparator<Node> coordsComparator = new NodePositionComparator();
068    
069            WayLoop: for (int seg1Way = 0; seg1Way < ways.size(); seg1Way ++) {
070                for (int seg2Way = seg1Way; seg2Way < ways.size(); seg2Way ++) {
071    
072                    //do not waste time on bounds that do not intersect
073                    if (!wayBounds[seg1Way].intersects(wayBounds[seg2Way])) {
074                        continue;
075                    }
076    
077                    ArrayList<Node> way1Nodes = newNodes[seg1Way];
078                    ArrayList<Node> way2Nodes = newNodes[seg2Way];
079    
080                    //iterate over primary segmemt
081                    for (int seg1Pos = 0; seg1Pos + 1 < way1Nodes.size(); seg1Pos ++) {
082    
083                        //iterate over secondary segment
084                        int seg2Start = seg1Way != seg2Way ? 0: seg1Pos + 2;//skip the adjacent segment
085    
086                        for (int seg2Pos = seg2Start; seg2Pos + 1< way2Nodes.size(); seg2Pos ++) {
087    
088                            //need to get them again every time, because other segments may be changed
089                            Node seg1Node1 = way1Nodes.get(seg1Pos);
090                            Node seg1Node2 = way1Nodes.get(seg1Pos + 1);
091                            Node seg2Node1 = way2Nodes.get(seg2Pos);
092                            Node seg2Node2 = way2Nodes.get(seg2Pos + 1);
093    
094                            int commonCount = 0;
095                            //test if we have common nodes to add.
096                            if (seg1Node1 == seg2Node1 || seg1Node1 == seg2Node2) {
097                                commonCount ++;
098    
099                                if (seg1Way == seg2Way &&
100                                        seg1Pos == 0 &&
101                                        seg2Pos == way2Nodes.size() -2) {
102                                    //do not add - this is first and last segment of the same way.
103                                } else {
104                                    intersectionNodes.add(seg1Node1);
105                                }
106                            }
107    
108                            if (seg1Node2 == seg2Node1 || seg1Node2 == seg2Node2) {
109                                commonCount ++;
110    
111                                intersectionNodes.add(seg1Node2);
112                            }
113    
114                            //no common nodes - find intersection
115                            if (commonCount == 0) {
116                                EastNorth intersection = getSegmentSegmentIntersection(
117                                        seg1Node1.getEastNorth(), seg1Node2.getEastNorth(),
118                                        seg2Node1.getEastNorth(), seg2Node2.getEastNorth());
119    
120                                if (intersection != null) {
121                                    if (test) {
122                                        intersectionNodes.add(seg2Node1);
123                                        return intersectionNodes;
124                                    }
125    
126                                    Node newNode = new Node(Main.getProjection().eastNorth2latlon(intersection));
127                                    Node intNode = newNode;
128                                    boolean insertInSeg1 = false;
129                                    boolean insertInSeg2 = false;
130    
131                                    //find if the intersection point is at end point of one of the segments, if so use that point
132    
133                                    //segment 1
134                                    if (coordsComparator.compare(newNode, seg1Node1) == 0) {
135                                        intNode = seg1Node1;
136                                    } else if (coordsComparator.compare(newNode, seg1Node2) == 0) {
137                                        intNode = seg1Node2;
138                                    } else {
139                                        insertInSeg1 = true;
140                                    }
141    
142                                    //segment 2
143                                    if (coordsComparator.compare(newNode, seg2Node1) == 0) {
144                                        intNode = seg2Node1;
145                                    } else if (coordsComparator.compare(newNode, seg2Node2) == 0) {
146                                        intNode = seg2Node2;
147                                    } else {
148                                        insertInSeg2 = true;
149                                    }
150    
151                                    if (insertInSeg1) {
152                                        way1Nodes.add(seg1Pos +1, intNode);
153                                        changedWays[seg1Way] = true;
154    
155                                        //fix seg2 position, as indexes have changed, seg2Pos is always bigger than seg1Pos on the same segment.
156                                        if (seg2Way == seg1Way) {
157                                            seg2Pos ++;
158                                        }
159                                    }
160    
161                                    if (insertInSeg2) {
162                                        way2Nodes.add(seg2Pos +1, intNode);
163                                        changedWays[seg2Way] = true;
164    
165                                        //Do not need to compare again to already split segment
166                                        seg2Pos ++;
167                                    }
168    
169                                    intersectionNodes.add(intNode);
170    
171                                    if (intNode == newNode) {
172                                        cmds.add(new AddCommand(intNode));
173                                    }
174                                }
175                            }
176                            else if (test && intersectionNodes.size() > 0)
177                                return intersectionNodes;
178                        }
179                    }
180                }
181            }
182    
183    
184            for (int pos = 0; pos < ways.size(); pos ++) {
185                if (changedWays[pos] == false) {
186                    continue;
187                }
188    
189                Way way = ways.get(pos);
190                Way newWay = new Way(way);
191                newWay.setNodes(newNodes[pos]);
192    
193                cmds.add(new ChangeCommand(way, newWay));
194            }
195    
196            return intersectionNodes;
197        }
198    
199        private static BBox getNodesBounds(ArrayList<Node> nodes) {
200    
201            BBox bounds = new BBox(nodes.get(0));
202            for(Node n: nodes) {
203                bounds.add(n.getCoor());
204            }
205            return bounds;
206        }
207    
208        /**
209         * Tests if given point is to the right side of path consisting of 3 points.
210         * @param lineP1 first point in path
211         * @param lineP2 second point in path
212         * @param lineP3 third point in path
213         * @param testPoint
214         * @return true if to the right side, false otherwise
215         */
216        public static boolean isToTheRightSideOfLine(Node lineP1, Node lineP2, Node lineP3, Node testPoint) {
217            boolean pathBendToRight = angleIsClockwise(lineP1, lineP2, lineP3);
218            boolean rightOfSeg1 = angleIsClockwise(lineP1, lineP2, testPoint);
219            boolean rightOfSeg2 = angleIsClockwise(lineP2, lineP3, testPoint);
220    
221            if (pathBendToRight)
222                return rightOfSeg1 && rightOfSeg2;
223            else
224                return !(!rightOfSeg1 && !rightOfSeg2);
225        }
226    
227        /**
228         * This method tests if secondNode is clockwise to first node.
229         * @param commonNode starting point for both vectors
230         * @param firstNode first vector end node
231         * @param secondNode second vector end node
232         * @return true if first vector is clockwise before second vector.
233         */
234        public static boolean angleIsClockwise(Node commonNode, Node firstNode, Node secondNode) {
235            return angleIsClockwise(commonNode.getEastNorth(), firstNode.getEastNorth(), secondNode.getEastNorth());
236        }
237    
238        /**
239         * Finds the intersection of two line segments
240         * @return EastNorth null if no intersection was found, the EastNorth coordinates of the intersection otherwise
241         */
242        public static EastNorth getSegmentSegmentIntersection(
243                EastNorth p1, EastNorth p2,
244                EastNorth p3, EastNorth p4) {
245            double x1 = p1.getX();
246            double y1 = p1.getY();
247            double x2 = p2.getX();
248            double y2 = p2.getY();
249            double x3 = p3.getX();
250            double y3 = p3.getY();
251            double x4 = p4.getX();
252            double y4 = p4.getY();
253    
254            //TODO: do this locally.
255            if (!Line2D.linesIntersect(x1, y1, x2, y2, x3, y3, x4, y4)) return null;
256    
257            // Convert line from (point, point) form to ax+by=c
258            double a1 = y2 - y1;
259            double b1 = x1 - x2;
260            double c1 = x2*y1 - x1*y2;
261    
262            double a2 = y4 - y3;
263            double b2 = x3 - x4;
264            double c2 = x4*y3 - x3*y4;
265    
266            // Solve the equations
267            double det = a1*b2 - a2*b1;
268            if (det == 0) return null; // Lines are parallel
269    
270            double x = (b1*c2 - b2*c1)/det;
271            double y = (a2*c1 -a1*c2)/det;
272    
273            return new EastNorth(x, y);
274        }
275    
276        /**
277         * Finds the intersection of two lines of infinite length.
278         * @return EastNorth null if no intersection was found, the coordinates of the intersection otherwise
279         */
280        public static EastNorth getLineLineIntersection(EastNorth p1, EastNorth p2, EastNorth p3, EastNorth p4) {
281    
282            // Convert line from (point, point) form to ax+by=c
283            double a1 = p2.getY() - p1.getY();
284            double b1 = p1.getX() - p2.getX();
285            double c1 = p2.getX() * p1.getY() - p1.getX() * p2.getY();
286    
287            double a2 = p4.getY() - p3.getY();
288            double b2 = p3.getX() - p4.getX();
289            double c2 = p4.getX() * p3.getY() - p3.getX() * p4.getY();
290    
291            // Solve the equations
292            double det = a1 * b2 - a2 * b1;
293            if (det == 0)
294                return null; // Lines are parallel
295    
296            return new EastNorth((b1 * c2 - b2 * c1) / det, (a2 * c1 - a1 * c2) / det);
297        }
298    
299        public static boolean segmentsParallel(EastNorth p1, EastNorth p2, EastNorth p3, EastNorth p4) {
300            // Convert line from (point, point) form to ax+by=c
301            double a1 = p2.getY() - p1.getY();
302            double b1 = p1.getX() - p2.getX();
303    
304            double a2 = p4.getY() - p3.getY();
305            double b2 = p3.getX() - p4.getX();
306    
307            // Solve the equations
308            double det = a1 * b2 - a2 * b1;
309            // remove influence of of scaling factor
310            det /= Math.sqrt(a1*a1 + b1*b1) * Math.sqrt(a2*a2 + b2*b2);
311            return Math.abs(det) < 1e-3;
312        }
313    
314        /**
315         * Calculates closest point to a line segment.
316         * @param segmentP1
317         * @param segmentP2
318         * @param point
319         * @return segmentP1 if it is the closest point, segmentP2 if it is the closest point,
320         * a new point if closest point is between segmentP1 and segmentP2.
321         */
322        public static EastNorth closestPointToSegment(EastNorth segmentP1, EastNorth segmentP2, EastNorth point) {
323    
324            double ldx = segmentP2.getX() - segmentP1.getX();
325            double ldy = segmentP2.getY() - segmentP1.getY();
326    
327            if (ldx == 0 && ldy == 0) //segment zero length
328                return segmentP1;
329    
330            double pdx = point.getX() - segmentP1.getX();
331            double pdy = point.getY() - segmentP1.getY();
332    
333            double offset = (pdx * ldx + pdy * ldy) / (ldx * ldx + ldy * ldy);
334    
335            if (offset <= 0)
336                return segmentP1;
337            else if (offset >= 1)
338                return segmentP2;
339            else
340                return new EastNorth(segmentP1.getX() + ldx * offset, segmentP1.getY() + ldy * offset);
341        }
342    
343        public static EastNorth closestPointToLine(EastNorth lineP1, EastNorth lineP2, EastNorth point) {
344            double ldx = lineP2.getX() - lineP1.getX();
345            double ldy = lineP2.getY() - lineP1.getY();
346    
347            if (ldx == 0 && ldy == 0) //segment zero length
348                return lineP1;
349    
350            double pdx = point.getX() - lineP1.getX();
351            double pdy = point.getY() - lineP1.getY();
352    
353            double offset = (pdx * ldx + pdy * ldy) / (ldx * ldx + ldy * ldy);
354            return new EastNorth(lineP1.getX() + ldx * offset, lineP1.getY() + ldy * offset);
355        }
356    
357        /**
358         * This method tests if secondNode is clockwise to first node.
359         * @param commonNode starting point for both vectors
360         * @param firstNode first vector end node
361         * @param secondNode second vector end node
362         * @return true if first vector is clockwise before second vector.
363         */
364        public static boolean angleIsClockwise(EastNorth commonNode, EastNorth firstNode, EastNorth secondNode) {
365            double dy1 = (firstNode.getY() - commonNode.getY());
366            double dy2 = (secondNode.getY() - commonNode.getY());
367            double dx1 = (firstNode.getX() - commonNode.getX());
368            double dx2 = (secondNode.getX() - commonNode.getX());
369    
370            return dy1 * dx2 - dx1 * dy2 > 0;
371        }
372    
373        private static Area getArea(List<Node> polygon) {
374            Path2D path = new Path2D.Double();
375    
376            boolean begin = true;
377            for (Node n : polygon) {
378                if (begin) {
379                    path.moveTo(n.getEastNorth().getX(), n.getEastNorth().getY());
380                    begin = false;
381                } else {
382                    path.lineTo(n.getEastNorth().getX(), n.getEastNorth().getY());
383                }
384            }
385            path.closePath();
386            
387            return new Area(path);
388        }
389        
390        /**
391         * Tests if two polygons intersect.
392         * @param first
393         * @param second
394         * @return intersection kind
395         */
396        public static PolygonIntersection polygonIntersection(List<Node> first, List<Node> second) {
397            
398            Area a1 = getArea(first);
399            Area a2 = getArea(second);
400            
401            Area inter = new Area(a1);
402            inter.intersect(a2);
403            
404            Rectangle bounds = inter.getBounds();
405            
406            if (inter.isEmpty() || bounds.getHeight()*bounds.getWidth() <= 1.0) {
407                return PolygonIntersection.OUTSIDE;
408            } else if (inter.equals(a1)) {
409                return PolygonIntersection.FIRST_INSIDE_SECOND;
410            } else if (inter.equals(a2)) {
411                return PolygonIntersection.SECOND_INSIDE_FIRST;
412            } else {
413                return PolygonIntersection.CROSSING;
414            }
415        }
416    
417        /**
418         * Tests if point is inside a polygon. The polygon can be self-intersecting. In such case the contains function works in xor-like manner.
419         * @param polygonNodes list of nodes from polygon path.
420         * @param point the point to test
421         * @return true if the point is inside polygon.
422         */
423        public static boolean nodeInsidePolygon(Node point, List<Node> polygonNodes) {
424            if (polygonNodes.size() < 2)
425                return false;
426    
427            boolean inside = false;
428            Node p1, p2;
429    
430            //iterate each side of the polygon, start with the last segment
431            Node oldPoint = polygonNodes.get(polygonNodes.size() - 1);
432    
433            for (Node newPoint : polygonNodes) {
434                //skip duplicate points
435                if (newPoint.equals(oldPoint)) {
436                    continue;
437                }
438    
439                //order points so p1.lat <= p2.lat;
440                if (newPoint.getEastNorth().getY() > oldPoint.getEastNorth().getY()) {
441                    p1 = oldPoint;
442                    p2 = newPoint;
443                } else {
444                    p1 = newPoint;
445                    p2 = oldPoint;
446                }
447    
448                //test if the line is crossed and if so invert the inside flag.
449                if ((newPoint.getEastNorth().getY() < point.getEastNorth().getY()) == (point.getEastNorth().getY() <= oldPoint.getEastNorth().getY())
450                        && (point.getEastNorth().getX() - p1.getEastNorth().getX()) * (p2.getEastNorth().getY() - p1.getEastNorth().getY())
451                        < (p2.getEastNorth().getX() - p1.getEastNorth().getX()) * (point.getEastNorth().getY() - p1.getEastNorth().getY()))
452                {
453                    inside = !inside;
454                }
455    
456                oldPoint = newPoint;
457            }
458    
459            return inside;
460        }
461    
462        /**
463         * Returns area of a closed way in square meters.
464         * (approximate(?), but should be OK for small areas)
465         *
466         * Relies on the current projection: Works correctly, when
467         * one unit in projected coordinates corresponds to one meter.
468         * This is true for most projections, but not for WGS84 and
469         * Mercator (EPSG:3857).
470         *
471         * @param way Way to measure, should be closed (first node is the same as last node)
472         * @return area of the closed way.
473         */
474        public static double closedWayArea(Way way) {
475    
476            //http://local.wasp.uwa.edu.au/~pbourke/geometry/polyarea/
477            double area = 0;
478            Node lastN = null;
479            for (Node n : way.getNodes()) {
480                if (lastN != null) {
481                    n.getEastNorth().getX();
482    
483                    area += (calcX(n) * calcY(lastN)) - (calcY(n) * calcX(lastN));
484                }
485                lastN = n;
486            }
487            return Math.abs(area/2);
488        }
489    
490        protected static double calcX(Node p1){
491            double lat1, lon1, lat2, lon2;
492            double dlon, dlat;
493    
494            lat1 = p1.getCoor().lat() * Math.PI / 180.0;
495            lon1 = p1.getCoor().lon() * Math.PI / 180.0;
496            lat2 = lat1;
497            lon2 = 0;
498    
499            dlon = lon2 - lon1;
500            dlat = lat2 - lat1;
501    
502            double a = (Math.pow(Math.sin(dlat/2), 2) + Math.cos(lat1) * Math.cos(lat2) * Math.pow(Math.sin(dlon/2), 2));
503            double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
504            return 6367000 * c;
505        }
506    
507        protected static double calcY(Node p1){
508            double lat1, lon1, lat2, lon2;
509            double dlon, dlat;
510    
511            lat1 = p1.getCoor().lat() * Math.PI / 180.0;
512            lon1 = p1.getCoor().lon() * Math.PI / 180.0;
513            lat2 = 0;
514            lon2 = lon1;
515    
516            dlon = lon2 - lon1;
517            dlat = lat2 - lat1;
518    
519            double a = (Math.pow(Math.sin(dlat/2), 2) + Math.cos(lat1) * Math.cos(lat2) * Math.pow(Math.sin(dlon/2), 2));
520            double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
521            return 6367000 * c;
522        }
523    
524        /**
525         * Determines whether a way is oriented clockwise.
526         *
527         * Internals: Assuming a closed non-looping way, compute twice the area
528         * of the polygon using the formula {@code 2 * area = sum (X[n] * Y[n+1] - X[n+1] * Y[n])}.
529         * If the area is negative the way is ordered in a clockwise direction.
530         *
531         * See http://paulbourke.net/geometry/polyarea/
532         *
533         * @param w the way to be checked.
534         * @return true if and only if way is oriented clockwise.
535         * @throws IllegalArgumentException if way is not closed (see {@link Way#isClosed}).
536         */
537        public static boolean isClockwise(Way w) {
538            if (!w.isClosed()) {
539                throw new IllegalArgumentException("Way must be closed to check orientation.");
540            }
541    
542            double area2 = 0.;
543            int nodesCount = w.getNodesCount();
544    
545            for (int node = 1; node <= /*sic! consider last-first as well*/ nodesCount; node++) {
546                LatLon coorPrev = w.getNode(node - 1).getCoor();
547                LatLon coorCurr = w.getNode(node % nodesCount).getCoor();
548                area2 += coorPrev.lon() * coorCurr.lat();
549                area2 -= coorCurr.lon() * coorPrev.lat();
550            }
551            return area2 < 0;
552        }
553    
554        /**
555         * Returns angle of a segment defined with 2 point coordinates.
556         *
557         * @param p1
558         * @param p2
559         * @return Angle in radians (-pi, pi]
560         */
561        public static double getSegmentAngle(EastNorth p1, EastNorth p2) {
562            return Math.atan2(p2.north() - p1.north(), p2.east() - p1.east());
563        }
564    
565        /**
566         * Returns angle of a corner defined with 3 point coordinates.
567         *
568         * @param p1
569         * @param p2 Common endpoint
570         * @param p3
571         * @return Angle in radians (-pi, pi]
572         */
573        public static double getCornerAngle(EastNorth p1, EastNorth p2, EastNorth p3) {
574            Double result = getSegmentAngle(p2, p1) - getSegmentAngle(p2, p3);
575            if (result <= -Math.PI) {
576                result += 2 * Math.PI;
577            }
578    
579            if (result > Math.PI) {
580                result -= 2 * Math.PI;
581            }
582    
583            return result;
584        }
585        
586        public static EastNorth getCentroid(List<Node> nodes) {
587            // Compute the centroid of nodes
588    
589            BigDecimal area = new BigDecimal(0);
590            BigDecimal north = new BigDecimal(0);
591            BigDecimal east = new BigDecimal(0);
592    
593            // See http://en.wikipedia.org/w/index.php?title=Centroid&oldid=294224857#Centroid_of_polygon for the equation used here
594            for (int i = 0; i < nodes.size(); i++) {
595                EastNorth n0 = nodes.get(i).getEastNorth();
596                EastNorth n1 = nodes.get((i+1) % nodes.size()).getEastNorth();
597    
598                BigDecimal x0 = new BigDecimal(n0.east());
599                BigDecimal y0 = new BigDecimal(n0.north());
600                BigDecimal x1 = new BigDecimal(n1.east());
601                BigDecimal y1 = new BigDecimal(n1.north());
602    
603                BigDecimal k = x0.multiply(y1, MathContext.DECIMAL128).subtract(y0.multiply(x1, MathContext.DECIMAL128));
604    
605                area = area.add(k, MathContext.DECIMAL128);
606                east = east.add(k.multiply(x0.add(x1, MathContext.DECIMAL128), MathContext.DECIMAL128));
607                north = north.add(k.multiply(y0.add(y1, MathContext.DECIMAL128), MathContext.DECIMAL128));
608            }
609    
610            BigDecimal d = new BigDecimal(3, MathContext.DECIMAL128); // 1/2 * 6 = 3
611            area  = area.multiply(d, MathContext.DECIMAL128);
612            if (area.compareTo(BigDecimal.ZERO) != 0) {
613                north = north.divide(area, MathContext.DECIMAL128);
614                east = east.divide(area, MathContext.DECIMAL128);
615            }
616    
617            return new EastNorth(east.doubleValue(), north.doubleValue());
618        }
619    
620        /**
621         * Returns the coordinate of intersection of segment sp1-sp2 and an altitude
622         * to it starting at point ap. If the line defined with sp1-sp2 intersects
623         * its altitude out of sp1-sp2, null is returned.
624         *
625         * @param sp1
626         * @param sp2
627         * @param ap
628         * @return Intersection coordinate or null
629         */
630        public static EastNorth getSegmentAltituteIntersection(EastNorth sp1,
631                EastNorth sp2, EastNorth ap) {
632            Double segmentLenght = sp1.distance(sp2);
633            Double altitudeAngle = getSegmentAngle(sp1, sp2) + Math.PI / 2;
634    
635            // Taking a random point on the altitude line (angle is known).
636            EastNorth ap2 = new EastNorth(ap.east() + 1000
637                    * Math.cos(altitudeAngle), ap.north() + 1000
638                    * Math.sin(altitudeAngle));
639    
640            // Finding the intersection of two lines
641            EastNorth resultCandidate = Geometry.getLineLineIntersection(sp1, sp2,
642                    ap, ap2);
643    
644            // Filtering result
645            if (resultCandidate != null
646                    && resultCandidate.distance(sp1) * .999 < segmentLenght
647                    && resultCandidate.distance(sp2) * .999 < segmentLenght) {
648                return resultCandidate;
649            } else {
650                return null;
651            }
652        }
653    }