Java Code Examples for com.vividsolutions.jts.geom.Coordinate#distance()

private PoseSteering[] resamplePath(PoseSteering[] path, double distanceBetweenPathPoints) {
	if (distanceBetweenPathPoints < 0) return path;
	ArrayList<PoseSteering> ret = new ArrayList<PoseSteering>();
	PoseSteering lastAdded = path[0];
	ret.add(lastAdded);
	for (int i = 1; i < path.length; i++) {
		Coordinate p1 = lastAdded.getPose().getPosition();
		Coordinate p2 = path[i].getPose().getPosition();
		//FIXME The function does not consider the rotation!!
		if (p2.distance(p1) > distanceBetweenPathPoints) {
			lastAdded = path[i];
			ret.add(path[i]);
		}
	}
	//Add the goal if not already added
	if (!ret.get(ret.size()-1).equals(path[path.length-1])) ret.add(path[path.length-1]);
	return ret.toArray(new PoseSteering[ret.size()]);
}
 

public static Coordinate[] createBezierSpline(Coordinate p1, Coordinate p2, Coordinate p3, Coordinate p4, double dist) {
	ArrayList<Coordinate> ret = new ArrayList<Coordinate>();
	double t = 0.0;
	Coordinate prevCoord = p1;
	ret.add(prevCoord);
	do {
		Coordinate coord = cubicBezier(p1, p2, p3, p4, t);
		if (prevCoord.distance(coord) >= dist) {
			ret.add(coord);
			prevCoord = coord;
		}
		t += 0.01;
	}
	while (t < 1.0);
	if (ret.get(ret.size()-1).distance(p4) > dist/4.0) ret.add(p4);
	return ret.toArray(new Coordinate[ret.size()]);
}
 

protected void runOffsetTest(String inputWKT,
      String testPtWKT, double offsetDistance, String expectedPtWKT)
//        throws Exception
    {
      Geometry input = read(inputWKT);
      Geometry testPoint = read(testPtWKT);
      Geometry expectedPoint = read(expectedPtWKT);
      Coordinate testPt = testPoint.getCoordinate();
      Coordinate expectedPt = expectedPoint.getCoordinate();
      Coordinate offsetPt = extractOffsetAt(input, testPt, offsetDistance);
      
      boolean isOk = offsetPt.distance(expectedPt) < TOLERANCE_DIST;
      if (! isOk)
        System.out.println("Expected = " + expectedPoint + "  Actual = " + offsetPt);
      assertTrue(isOk);
    }
 

/**
 * Creates an AffineTransformation defined by a maping between two baselines. 
 * The computed transformation consists of:
 * <ul>
 * <li>a translation 
 * from the start point of the source baseline to the start point of the destination baseline,
 * <li>a rotation through the angle between the baselines about the destination start point,
 * <li>and a scaling equal to the ratio of the baseline lengths.
 * </ul>
 * If the source baseline has zero length, an identity transformation is returned.
 * 
 * @param src0 the start point of the source baseline
 * @param src1 the end point of the source baseline
 * @param dest0 the start point of the destination baseline
 * @param dest1 the end point of the destination baseline
 * @return the computed transformation
 */
public static AffineTransformation createFromBaseLines(
		Coordinate src0, Coordinate src1, 
		Coordinate dest0, Coordinate dest1) 
{
	Coordinate rotPt = new Coordinate(src0.x + dest1.x - dest0.x, src0.y + dest1.y - dest0.y);

	double ang = Angle.angleBetweenOriented(src1, src0, rotPt);

	double srcDist = src1.distance(src0);
	double destDist = dest1.distance(dest0);

	// return identity if transformation would be degenerate
	if (srcDist == 0.0)
		return new AffineTransformation();

	double scale = destDist / srcDist;

	AffineTransformation trans = AffineTransformation.translationInstance(
			-src0.x, -src0.y);
	trans.rotate(ang);
	trans.scale(scale, scale);
	trans.translate(dest0.x, dest0.y);
	return trans;
}
 

protected void mouseLocationChanged(MouseEvent e) {
  try {
    if ((e.getModifiersEx() & InputEvent.BUTTON1_DOWN_MASK) 
        == InputEvent.BUTTON1_DOWN_MASK) {
      Coordinate newCoord = toModelCoordinate(e.getPoint());
      if (newCoord.distance(lastCoordinate()) < gridSize())
        return;
      //add(toModelSnapped(e.getPoint()));
      add(newCoord);
    }

    tentativeCoordinate = toModelSnapped(e.getPoint());
    redrawIndicator();
  } catch (Throwable t) {
  }
}
 

/**
 * Creates an AffineTransformation defined by a pair of control vectors. A
 * control vector consists of a source point and a destination point, which is
 * the image of the source point under the desired transformation. The
 * computed transformation is a combination of one or more of a uniform scale,
 * a rotation, and a translation (i.e. there is no shear component and no
 * reflection)
 * 
 * @param src0
 * @param src1
 * @param dest0
 * @param dest1
 * @return the computed transformation
  * @return null if the control vectors do not determine a well-defined transformation
 */
public static AffineTransformation createFromControlVectors(Coordinate src0,
		Coordinate src1, Coordinate dest0, Coordinate dest1) {
	Coordinate rotPt = new Coordinate(dest1.x - dest0.x, dest1.y - dest0.y);

	double ang = Angle.angleBetweenOriented(src1, src0, rotPt);

	double srcDist = src1.distance(src0);
	double destDist = dest1.distance(dest0);

	if (srcDist == 0.0)
		return null;

	double scale = destDist / srcDist;

	AffineTransformation trans = AffineTransformation.translationInstance(
			-src0.x, -src0.y);
	trans.rotate(ang);
	trans.scale(scale, scale);
	trans.translate(dest0.x, dest0.y);
	return trans;
}
 

/**
 * Computes the distance from a point to a sequence of line segments.
 * 
 * @param p
 *          a point
 * @param line
 *          a sequence of contiguous line segments defined by their vertices
 * @return the minimum distance between the point and the line segments
 */
public static double distancePointLine(Coordinate p, Coordinate[] line)
{
  if (line.length == 0)
    throw new IllegalArgumentException(
        "Line array must contain at least one vertex");
  // this handles the case of length = 1
  double minDistance = p.distance(line[0]);
  for (int i = 0; i < line.length - 1; i++) {
    double dist = CGAlgorithms.distancePointLine(p, line[i], line[i + 1]);
    if (dist < minDistance) {
      minDistance = dist;
    }
  }
  return minDistance;
}
 

/**
 * A basic strategy for finding split points when nothing extra is known about the geometry of
 * the situation.
 * 
 * @param seg the encroached segment
 * @param encroachPt the encroaching point
 * @return the point at which to split the encroached segment
 */
public Coordinate findSplitPoint(Segment seg, Coordinate encroachPt) {
    LineSegment lineSeg = seg.getLineSegment();
    double segLen = lineSeg.getLength();
    double midPtLen = segLen / 2;
    SplitSegment splitSeg = new SplitSegment(lineSeg);

    Coordinate projPt = projectedSplitPoint(seg, encroachPt);
    /**
     * Compute the largest diameter (length) that will produce a split segment which is not
     * still encroached upon by the encroaching point (The length is reduced slightly by a
     * safety factor)
     */
    double nonEncroachDiam = projPt.distance(encroachPt) * 2 * 0.8; // .99;
    double maxSplitLen = nonEncroachDiam;
    if (maxSplitLen > midPtLen) {
        maxSplitLen = midPtLen;
    }
    splitSeg.setMinimumLength(maxSplitLen);

    splitSeg.splitAt(projPt);

    return splitSeg.getSplitPoint();
}
 

private static PoseSteering[] resamplePath(PoseSteering[] path) {
	if (minPathDistance < 0) return path;
	ArrayList<PoseSteering> ret = new ArrayList<PoseSteering>();
	PoseSteering lastAdded = path[0];
	ret.add(lastAdded);
	for (int i = 1; i < path.length; i++) {
		Coordinate p1 = lastAdded.getPose().getPosition();
		Coordinate p2 = path[i].getPose().getPosition();
		if (p2.distance(p1) > minPathDistance) {
			lastAdded = path[i];
			ret.add(path[i]);
		}
	}
	return ret.toArray(new PoseSteering[ret.size()]);
}
 

/**
 * Determines a point closest to the given point.
 * 
 * @param p the point to compare against
 * @param p1 a potential result point
 * @param p2 a potential result point
 * @param q1 a potential result point
 * @param q2 a potential result point
 * @return the point closest to the input point p
 */
private Coordinate findNearestPoint(Coordinate p, Coordinate[] pts)
{
	double minDist = Double.MAX_VALUE;
	Coordinate result = null;
	for (int i = 0; i < pts.length; i++) {
		double dist = p.distance(pts[i]);
		// always initialize the result
		if (i == 0 || dist < minDist) {
			minDist = dist;
			result = pts[i];
		}
	}
	return result;
}
 

public static double distance(Coordinate p0, Coordinate p1)
{
	// default to 2D distance if either Z is not set
	if (Double.isNaN(p0.z) || Double.isNaN(p1.z))
		return p0.distance(p1);
	
    double dx = p0.x - p1.x;
    double dy = p0.y - p1.y;
    double dz = p0.z - p1.z;
    return Math.sqrt(dx * dx + dy * dy + dz * dz);
}
 

public void splitAt(Coordinate pt) {
    // check that given pt doesn't violate min length
    double minFrac = minimumLen / segLen;
    if (pt.distance(seg.p0) < minimumLen) {
        splitPt = seg.pointAlong(minFrac);
        return;
    }
    if (pt.distance(seg.p1) < minimumLen) {
        splitPt = pointAlongReverse(seg, minFrac);
        return;
    }
    // passes minimum distance check - use provided point as split pt
    splitPt = pt;
}
 

/**
 * Given a set of points stored in the kd-tree and a line segment defined by
 * two points in this set, finds a {@link Coordinate} in the circumcircle of
 * the line segment, if one exists. This is called the Gabriel point - if none
 * exists then the segment is said to have the Gabriel condition. Uses the
 * heuristic of finding the non-Gabriel point closest to the midpoint of the
 * segment.
 * 
 * @param p
 *          start of the line segment
 * @param q
 *          end of the line segment
 * @return a point which is non-Gabriel
 * or null if no point is non-Gabriel
 */
private Coordinate findNonGabrielPoint(Segment seg) {
	Coordinate p = seg.getStart();
	Coordinate q = seg.getEnd();
	// Find the mid point on the line and compute the radius of enclosing circle
	Coordinate midPt = new Coordinate((p.x + q.x) / 2.0, (p.y + q.y) / 2.0);
	double segRadius = p.distance(midPt);

	// compute envelope of circumcircle
	Envelope env = new Envelope(midPt);
	env.expandBy(segRadius);
	// Find all points in envelope
	List result = kdt.query(env);

	// For each point found, test if it falls strictly in the circle
	// find closest point
	Coordinate closestNonGabriel = null;
	double minDist = Double.MAX_VALUE;
	for (Iterator i = result.iterator(); i.hasNext();) {
		KdNode nextNode = (KdNode) i.next();
		Coordinate testPt = nextNode.getCoordinate();
		// ignore segment endpoints
		if (testPt.equals2D(p) || testPt.equals2D(q))
			continue;

		double testRadius = midPt.distance(testPt);
		if (testRadius < segRadius) {
			// double testDist = seg.distance(testPt);
			double testDist = testRadius;
			if (closestNonGabriel == null || testDist < minDist) {
				closestNonGabriel = testPt;
				minDist = testDist;
			}
		}
	}
	return closestNonGabriel;
}
 

private void checkVertexDistance(Coordinate vertex)
{
  double vertexDist = vertex.distance(queryPt);
  if (vertexDist > 0) {
    updateClearance(vertexDist, queryPt, vertex);
  }
}
 

protected Coordinate toModelSnappedIfCloseToViewGrid(Point2D p)
{
	// snap to view grid if close to view grid point
	Coordinate pModel = getViewport().toModelCoordinate(p);
	Coordinate pSnappedModel = new Coordinate(pModel);
	gridPM.makePrecise(pSnappedModel);
	
	double tol = getModelSnapTolerance();
	if (pModel.distance(pSnappedModel) <= tol)
		return pSnappedModel;
	return pModel;
}
 

/**
 * Computes the distance from a point p to a line segment AB
 * 
 * Note: NON-ROBUST!
 * 
 * @param p
 *          the point to compute the distance for
 * @param A
 *          one point of the line
 * @param B
 *          another point of the line (must be different to A)
 * @return the distance from p to line segment AB
 */
public static double distancePointLine(Coordinate p, Coordinate A,
    Coordinate B)
{
  // if start = end, then just compute distance to one of the endpoints
  if (A.x == B.x && A.y == B.y)
    return p.distance(A);

  // otherwise use comp.graphics.algorithms Frequently Asked Questions method
  /*
   * (1) r = AC dot AB 
   *         --------- 
   *         ||AB||^2 
   *         
   * r has the following meaning: 
   *   r=0 P = A 
   *   r=1 P = B 
   *   r<0 P is on the backward extension of AB 
   *   r>1 P is on the forward extension of AB 
   *   0<r<1 P is interior to AB
   */

  double len2 = (B.x - A.x) * (B.x - A.x) + (B.y - A.y) * (B.y - A.y);
  double r = ((p.x - A.x) * (B.x - A.x) + (p.y - A.y) * (B.y - A.y))
      / len2;

  if (r <= 0.0)
    return p.distance(A);
  if (r >= 1.0)
    return p.distance(B);

  /*
   * (2) s = (Ay-Cy)(Bx-Ax)-(Ax-Cx)(By-Ay) 
   *         ----------------------------- 
   *                    L^2
   * 
   * Then the distance from C to P = |s|*L.
   * 
   * This is the same calculation as {@link #distancePointLinePerpendicular}.
   * Unrolled here for performance.
   */
  double s = ((A.y - p.y) * (B.x - A.x) - (A.x - p.x) * (B.y - A.y))
      / len2;
  return Math.abs(s) * Math.sqrt(len2);
}
 

/**
 * Inserts a point known to be beyond the distance tolerance of any existing node.
 * The point is inserted at the bottom of the exact splitting path, 
 * so that tree shape is deterministic.
 * 
 * @param p the point to insert
 * @param data the data for the point
 * @return the created node
 */
private KdNode insertExact(Coordinate p, Object data) {
  KdNode currentNode = root;
  KdNode leafNode = root;
  boolean isOddLevel = true;
  boolean isLessThan = true;

  /**
   * Traverse the tree, first cutting the plane left-right (by X ordinate)
   * then top-bottom (by Y ordinate)
   */
  while (currentNode != null) {
    // test if point is already a node (not strictly necessary)
    if (currentNode != null) {
      boolean isInTolerance = p.distance(currentNode.getCoordinate()) <= tolerance;

      // check if point is already in tree (up to tolerance) and if so simply
      // return existing node
      if (isInTolerance) {
        currentNode.increment();
        return currentNode;
      }
    }

    if (isOddLevel) {
      isLessThan = p.x < currentNode.getX();
    } else {
      isLessThan = p.y < currentNode.getY();
    }
    leafNode = currentNode;
    if (isLessThan) {
      currentNode = currentNode.getLeft();
    } else {
      currentNode = currentNode.getRight();
    }

    isOddLevel = ! isOddLevel;
  }

  // no node found, add new leaf node to tree
  numberOfNodes = numberOfNodes + 1;
  KdNode node = new KdNode(p, data);
  if (isLessThan) {
    leafNode.setLeft(node);
  } else {
    leafNode.setRight(node);
  }
  return node;
}
 

private boolean validateMinSegmentLength(final Coordinate coordinate1, final Coordinate coordinate2) {
	return (coordinate1.distance(coordinate2) >= MIN_SEGMENT_LENGTH);
}
 

/**
 * Computes the inCircle test using distance from the circumcentre. 
 * Uses standard double-precision arithmetic.
 * <p>
 * In general this doesn't
 * appear to be any more robust than the standard calculation. However, there
 * is at least one case where the test point is far enough from the
 * circumcircle that this test gives the correct answer. 
 * <pre>
 * LINESTRING
 * (1507029.9878 518325.7547, 1507022.1120341457 518332.8225183258,
 * 1507029.9833 518325.7458, 1507029.9896965567 518325.744909031)
 * </pre>
 * 
 * @param a a vertex of the triangle
 * @param b a vertex of the triangle
 * @param c a vertex of the triangle
 * @param p the point to test
 * @return true if this point is inside the circle defined by the points a, b, c
 */
public static boolean isInCircleCC(Coordinate a, Coordinate b, Coordinate c,
    Coordinate p) {
  Coordinate cc = Triangle.circumcentre(a, b, c);
  double ccRadius = a.distance(cc);
  double pRadiusDiff = p.distance(cc) - ccRadius;
  return pRadiusDiff <= 0;
}
 

/**
 * Computes the interpolated Z-value for a point p lying on the segment p0-p1
 * 
 * @param p
 * @param p0
 * @param p1
 * @return the interpolated Z value
 */
public static double interpolateZ(Coordinate p, Coordinate p0, Coordinate p1) {
    double segLen = p0.distance(p1);
    double ptLen = p.distance(p0);
    double dz = p1.z - p0.z;
    double pz = p0.z + dz * (ptLen / segLen);
    return pz;
}