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