Add Z handling to LineSegment and Densifier

modules/core/src/main/java/org/locationtech/jts/geom/Coordinate.java

@@ -62,15 +62,17 @@ public class Coordinate implements Comparable<Coordinate>, Cloneable, Serializab
    * Standard ordinate index value for, where Z is 2.
    *
    * <p>This constant assumes XYZM coordinate sequence definition, please check this assumption
-   * using {@link #getDimension()} and {@link #getMeasures()} before use.
+   * using {@link CoordinateSequence#getDimension()} and
+   * {@link CoordinateSequence#getMeasures()} before use.
    */
   public static final int Z = 2;
 
   /**
    * Standard ordinate index value for, where M is 3.
    *
    * <p>This constant assumes XYZM coordinate sequence definition, please check this assumption
-   * using {@link #getDimension()} and {@link #getMeasures()} before use.
+   * using {@link CoordinateSequence#getDimension()} and
+   * {@link CoordinateSequence#getMeasures()} before use.
    */
   public static final int M = 3;
 
@@ -215,12 +217,20 @@ public double getM() {
   public void setM(double m) {
     throw new IllegalArgumentException("Invalid ordinate index: " + M);
   }
-
+
+  /**
+   * Returns true if this Coordinate has a valid z (different from NaN)
+   * @return true if this Coordinate has a valid z (different from NaN)
+   */
+  public boolean hasZ() {
+    return !Double.isNaN(z);
+  }
+
   /**
    * Gets the ordinate value for the given index.
    * 
    * The base implementation supports values for the index are 
-   * {@link X}, {@link Y}, and {@link Z}.
+   * {@link #X}, {@link #Y}, and {@link #Z}.
    * 
    * @param ordinateIndex the ordinate index
    * @return the value of the ordinate
@@ -241,7 +251,7 @@ public double getOrdinate(int ordinateIndex)
    * to a given value.
    * 
    * The base implementation supported values for the index are 
-   * {@link X}, {@link Y}, and {@link Z}.
+   * {@link #X}, {@link #Y}, and {@link #Z}.
    * 
    * @param ordinateIndex the ordinate index
    * @param value the value to set

modules/core/src/main/java/org/locationtech/jts/geom/CoordinateXY.java

@@ -121,6 +121,14 @@ public void setOrdinate(int ordinateIndex, double value) {
         throw new IllegalArgumentException("Invalid ordinate index: " + ordinateIndex);
     }
   }
+
+  /**
+   * Returns false because CoordinateXY has no z
+   * @return false
+   */
+  public boolean hasZ() {
+    return false;
+  }
 
   public String toString() {
     return "(" + x + ", " + y + ")";

modules/core/src/main/java/org/locationtech/jts/geom/CoordinateXYM.java

@@ -146,6 +146,14 @@ public void setOrdinate(int ordinateIndex, double value) {
         throw new IllegalArgumentException("Invalid ordinate index: " + ordinateIndex);
     }
   }
+
+  /**
+   * Returns false because CoordinateXYM has no z
+   * @return false
+   */
+  public boolean hasZ() {
+    return false;
+  }
 
   public String toString() {
     return "(" + x + ", " + y + " m=" + getM() + ")";

modules/core/src/main/java/org/locationtech/jts/geom/CoordinateXYZM.java

@@ -120,6 +120,14 @@ public void setOrdinate(int ordinateIndex, double value) {
         throw new IllegalArgumentException("Invalid ordinate index: " + ordinateIndex);
     }
   }
+
+  /**
+   * Returns true if this Coordinate has a valid z (different from NaN)
+   * @return true if this Coordinate has a valid z (different from NaN)
+   */
+  public boolean hasZ() {
+    return !Double.isNaN(z);
+  }
 
   public String toString() {
     return "(" + x + ", " + y + ", " + getZ() + " m="+getM()+")";

modules/core/src/main/java/org/locationtech/jts/geom/LineSegment.java

@@ -51,6 +51,10 @@ public LineSegment(double x0, double y0, double x1, double y1) {
     this(new Coordinate(x0, y0), new Coordinate(x1, y1));
   }
 
+  public LineSegment(double x0, double y0, double z0, double x1, double y1, double z1) {
+    this(new Coordinate(x0, y0, z0), new Coordinate(x1, y1, z1));
+  }
+
   public LineSegment(LineSegment ls) {
     this(ls.p0, ls.p1);
   }
@@ -74,8 +78,10 @@ public void setCoordinates(Coordinate p0, Coordinate p1)
   {
     this.p0.x = p0.x;
     this.p0.y = p0.y;
+    this.p0.setZ(p0.getZ());
     this.p1.x = p1.x;
     this.p1.y = p1.y;
+    this.p1.setZ(p1.getZ());
   }
 
   /**
@@ -168,15 +174,15 @@ public int orientationIndex(LineSegment seg)
 
   /**
    * Determines the orientation index of a {@link Coordinate} relative to this segment.
-   * The orientation index is as defined in {@link Orientation#computeOrientation}.
+   * The orientation index is as defined in {@link Orientation#index}.
    *
    * @param p the coordinate to compare
    *
    * @return 1 (LEFT) if <code>p</code> is to the left of this segment
    * @return -1 (RIGHT) if <code>p</code> is to the right of this segment
    * @return 0 (COLLINEAR) if <code>p</code> is collinear with this segment
    * 
-   * @see Orientation#computeOrientation(Coordinate, Coordinate, Coordinate)
+   * @see Orientation#index(Coordinate, Coordinate, Coordinate)
    */
   public int orientationIndex(Coordinate p)
   {
@@ -234,8 +240,10 @@ public Coordinate midPoint()
    */
   public static Coordinate midPoint(Coordinate p0, Coordinate p1)
   {
-    return new Coordinate( (p0.x + p1.x) / 2,
-                           (p0.y + p1.y) / 2);
+      return new Coordinate(
+              (p0.x + p1.x) / 2,
+              (p0.y + p1.y) / 2,
+              (p0.getZ() + p1.getZ()) / 2);
   }
 
   /**
@@ -285,17 +293,22 @@ public Coordinate pointAlong(double segmentLengthFraction)
     Coordinate coord = new Coordinate();
     coord.x = p0.x + segmentLengthFraction * (p1.x - p0.x);
     coord.y = p0.y + segmentLengthFraction * (p1.y - p0.y);
+    coord.z = p0.z + segmentLengthFraction * (p1.z - p0.z);
     return coord;
   }
 
   /**
    * Computes the {@link Coordinate} that lies a given
    * fraction along the line defined by this segment and offset from 
    * the segment by a given distance.
+   * <p>
    * A fraction of <code>0.0</code> offsets from the start point of the segment;
    * a fraction of <code>1.0</code> offsets from the end point of the segment.
    * The computed point is offset to the left of the line if the offset distance is
    * positive, to the right if negative.
+   * </p>
+   * <p>If this LineSegment has valid z values, the z value of the returned
+   * Coordinate is interpolated along the LineSegment</p>
    *
    * @param segmentLengthFraction the fraction of the segment length along the line
    * @param offsetDistance the distance the point is offset from the segment
@@ -306,12 +319,14 @@ public Coordinate pointAlong(double segmentLengthFraction)
    */
   public Coordinate pointAlongOffset(double segmentLengthFraction, double offsetDistance)
   {
-  	// the point on the segment line
-    double segx = p0.x + segmentLengthFraction * (p1.x - p0.x);
-    double segy = p0.y + segmentLengthFraction * (p1.y - p0.y);
-
     double dx = p1.x - p0.x;
     double dy = p1.y - p0.y;
+    double dz = p1.z - p0.z;
+  	// the point on the segment line
+    double segx = p0.x + segmentLengthFraction * dx;
+    double segy = p0.y + segmentLengthFraction * dy;
+    double segz = p0.z + segmentLengthFraction * dz;
+
     double len = Math.sqrt(dx * dx + dy * dy);
     double ux = 0.0;
     double uy = 0.0;
@@ -328,7 +343,7 @@ public Coordinate pointAlongOffset(double segmentLengthFraction, double offsetDi
     double offsetx = segx - uy;
     double offsety = segy + ux;
 
-    Coordinate coord = new Coordinate(offsetx, offsety);
+    Coordinate coord = new Coordinate(offsetx, offsety, segz);
     return coord;
   }
 
@@ -412,6 +427,11 @@ public Coordinate project(Coordinate p)
     Coordinate coord = new Coordinate();
     coord.x = p0.x + r * (p1.x - p0.x);
     coord.y = p0.y + r * (p1.y - p0.y);
+    if (p0.hasZ() && p1.hasZ()) {
+      coord.z = p0.z + r * (p1.z - p0.z);
+    } else {
+      coord.z = p.z;
+    }
     return coord;
   }
   /**
@@ -422,6 +442,10 @@ public Coordinate project(Coordinate p)
    * <p>
    * Note that the returned line may have zero length (i.e. the same endpoints).
    * This can happen for instance if the lines are perpendicular to one another.
+   * </p>
+   * <p>
+   * If seg has significative z values, they are kept as is during projection
+   * </p>
    *
    * @param seg the line segment to project
    * @return the projected line segment, or <code>null</code> if there is no overlap
@@ -448,6 +472,7 @@ public LineSegment project(LineSegment seg)
   /**
    * Computes the reflection of a point in the line defined
    * by this line segment.
+   * <p>If p has a significative z value, it is kept as is</p>
    * 
    * @param p the point to reflect
    * @return the reflected point
@@ -466,8 +491,17 @@ public Coordinate reflect(Coordinate p) {
     double y = p.getY();
     double rx = ( -A2subB2*x - 2*A*B*y - 2*A*C ) / A2plusB2;
     double ry = ( A2subB2*y - 2*A*B*x - 2*B*C ) / A2plusB2;
-
-    return new Coordinate(rx, ry);
+
+    if (p.hasZ()) {
+      if (this.p0.hasZ() && this.p1.hasZ()) {
+        Coordinate projection = this.project(p);
+        return new Coordinate(rx, ry, projection.z - (p.z-projection.z));
+      } else {
+        return new Coordinate(rx, ry, p.z);
+      }
+    } else {
+      return new Coordinate(rx, ry);
+    }
   }
 
   /**
@@ -484,9 +518,10 @@ public Coordinate closestPoint(Coordinate p)
     double dist0 = p0.distance(p);
     double dist1 = p1.distance(p);
     if (dist0 < dist1)
-      return p0;
-    return p1;
+      return new Coordinate(p0.x, p0.y, p0.z);
+    return new Coordinate(p1.x, p1.y, p1.z);
   }
+
   /**
    * Computes the closest points on two line segments.
    * 

modules/core/src/test/java/org/locationtech/jts/densify/DensifierTest.java

@@ -11,6 +11,7 @@
  */
 package org.locationtech.jts.densify;
 
+import org.locationtech.jts.geom.Coordinate;
 import org.locationtech.jts.geom.Geometry;
 
 import junit.textui.TestRunner;
@@ -40,6 +41,20 @@ public void testBoxNoValidate() {
         10, "POLYGON ((10 30, 16.666666666666668 30, 23.333333333333336 30, 30 30, 30 23.333333333333332, 30 16.666666666666664, 30 10, 23.333333333333332 10, 16.666666666666664 10, 10 10, 10 16.666666666666668, 10 23.333333333333336, 10 30))");
   }
 
+  public void testDensify3D() {
+    Geometry line = read("LINESTRING (0 0 0, 30 40 60, 35 35 120)");
+    double distanceTolerance = 10.0;
+    Geometry densified = Densifier.densify(line, distanceTolerance);
+    Coordinate c0 = line.getCoordinates()[0];
+    Coordinate c1 = line.getCoordinates()[1];
+    Coordinate c2 = line.getCoordinates()[2];
+    // Number of segments in original first and second segments
+    int frac01 = (int) (c0.distance(c1)/distanceTolerance) + 1;
+    int frac12 = (int) (c1.distance(c2)/distanceTolerance) + 1;
+    assertEquals(densified.getCoordinates()[1].z, c0.z + (c1.z-c0.z)/frac01,1E-10);
+    assertEquals(densified.getCoordinates()[frac01+1].z, c1.z + (c2.z-c1.z)/frac12,1E-10);
+  }
+
   private void checkDensify(String wkt, double distanceTolerance, String wktExpected) {
     Geometry geom = read(wkt);
     Geometry expected = read(wktExpected);

modules/core/src/test/java/org/locationtech/jts/geom/LineSegmentTest.java

@@ -74,6 +74,69 @@ private void checkLineIntersection(double p1x, double p1y, double p2x, double p2
     assertTrue(dist <= MAX_ABS_ERROR_INTERSECTION);
   }
 
+  public void testMidPoint3D() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    assertTrue(segment.midPoint().getZ() == 1.5);
+  }
+
+  public void testPointAlong3D() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    assertTrue(segment.pointAlong(0.5).getZ() == 1.5);
+  }
+
+  public void testPointAlongOffset3D() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    assertTrue(segment.pointAlongOffset(0.5,1.0).getZ() == 1.5);
+  }
+
+  public void testClosestPoint3D() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    Coordinate p = new Coordinate(5.0, 2.0, 3.14);
+    assertTrue(segment.closestPoint(p).equals3D(new Coordinate(5.0, 0.0, 1.5)));
+  }
+
+  public void testClosestPoint3Dext() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    Coordinate p = new Coordinate(-10.0, 2.0, 3.14);
+    assertTrue(segment.closestPoint(p).equals3D(new Coordinate(0.0, 0.0, 1.0)));
+  }
+
+  public void testProject3D() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    Coordinate p = new Coordinate(5.0, 2.0, 3.14);
+    assertTrue(segment.project(p).equals3D(new Coordinate(5.0, 0.0, 1.5)));
+  }
+
+  public void testProject3Da() {
+    LineSegment segment = new LineSegment(0.0,0.0,3.14, 10.0,0.0, Double.NaN);
+    Coordinate p = new Coordinate(5.0, 2.0, 3.14);
+    assertTrue(segment.project(p).equals3D(new Coordinate(5.0, 0.0, 3.14)));
+  }
+
+  public void testProject3Db() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    Coordinate p = new Coordinate(5.0, 2.0);
+    assertTrue(segment.project(p).equals3D(new Coordinate(5.0, 0.0, 1.5)));
+  }
+
+  public void testReflect3D() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    Coordinate p = new Coordinate(5.0, 2.0, 2.0);
+    assertTrue(segment.reflect(p).equals3D(new Coordinate(5.0, -2.0, 1.0)));
+  }
+
+  public void testReflect3Da() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,Double.NaN);
+    Coordinate p = new Coordinate(5.0, 2.0, 3.14);
+    assertTrue(segment.reflect(p).equals3D(new Coordinate(5.0, -2.0, 3.14)));
+  }
+
+  public void testReflect3Db() {
+    LineSegment segment = new LineSegment(0.0,0.0,1.0,10.0,0.0,2.0);
+    Coordinate p = new Coordinate(5.0, 2.0);
+    assertTrue(segment.reflect(p).equals3D(new Coordinate(5.0, -2.0, Double.NaN)));
+  }
+
   public void testOffset() throws Exception
   {
     checkOffset(0, 0, 10, 10, 0.0, ROOT2, -1, 1);