geo: implement ST_3DDWithin and ST_3DDistance

docs/generated/sql/functions.md

@@ -1721,6 +1721,8 @@ the locality flag on node startup. Returns an error if no region is set.</p>
 <table>
 <thead><tr><th>Function &rarr; Returns</th><th>Description</th><th>Volatility</th></tr></thead>
 <tbody>
+<tr><td><a name="_st_3ddwithin"></a><code>_st_3ddwithin(geometry_a: geometry, geometry_b: geometry, distance: <a href="float.html">float</a>) &rarr; <a href="bool.html">bool</a></code></td><td><span class="funcdesc"><p>Returns true if any of geometry_a is within distance units of geometry_b, using 3D Euclidean distance. This variant does not utilize any spatial index.</p>
+</span></td><td>Immutable</td></tr>
 <tr><td><a name="_st_contains"></a><code>_st_contains(geometry_a: geometry, geometry_b: geometry) &rarr; <a href="bool.html">bool</a></code></td><td><span class="funcdesc"><p>Returns true if no points of geometry_b lie in the exterior of geometry_a, and there is at least one point in the interior of geometry_b that lies in the interior of geometry_a.</p>
 <p>This function utilizes the GEOS module.</p>
 <p>This function variant does not utilize any spatial index.</p>
@@ -1867,6 +1869,10 @@ the locality flag on node startup. Returns an error if no region is set.</p>
 </span></td><td>Immutable</td></tr>
 <tr><td><a name="postgis_wagyu_version"></a><code>postgis_wagyu_version() &rarr; <a href="string.html">string</a></code></td><td><span class="funcdesc"><p>Compatibility placeholder function with PostGIS. Returns a fixed string based on PostGIS 3.0.1, with minor edits.</p>
 </span></td><td>Immutable</td></tr>
+<tr><td><a name="st_3ddistance"></a><code>st_3ddistance(geometry_a: geometry, geometry_b: geometry) &rarr; <a href="float.html">float</a></code></td><td><span class="funcdesc"><p>Returns the 3-dimensional minimum Cartesian distance between two geometries. If either geometry has no Z component, this is equivalent to ST_Distance.</p>
+</span></td><td>Immutable</td></tr>
+<tr><td><a name="st_3ddwithin"></a><code>st_3ddwithin(geometry_a: geometry, geometry_b: geometry, distance: <a href="float.html">float</a>) &rarr; <a href="bool.html">bool</a></code></td><td><span class="funcdesc"><p>Returns true if any of geometry_a is within distance units of geometry_b, using 3D Euclidean distance.</p>
+</span></td><td>Immutable</td></tr>
 <tr><td><a name="st_3dlength"></a><code>st_3dlength(geometry: geometry) &rarr; <a href="float.html">float</a></code></td><td><span class="funcdesc"><p>Returns the 3-dimensional or 2-dimensional length of the geometry.</p>
 <p>Note ST_3DLength is only valid for LineString or MultiLineString.
 For 2-D lines it will return the 2-D length (same as ST_Length and ST_Length2D)</p>

pkg/geo/geomfn/coord.go

@@ -60,3 +60,104 @@ func coordEqual(a geom.Coord, b geom.Coord) bool {
 func coordMag2(c geom.Coord) float64 {
 	return coordDot(c, c)
 }
+
+// coordGetZ returns the Z value from a coordinate, or 0 if no Z is present.
+func coordGetZ(c geom.Coord) float64 {
+	if len(c) > 2 {
+		return c[2]
+	}
+	return 0
+}
+
+// coordAdd3D adds two coordinates in 3D and returns a new result.
+func coordAdd3D(a geom.Coord, b geom.Coord) geom.Coord {
+	return geom.Coord{a.X() + b.X(), a.Y() + b.Y(), coordGetZ(a) + coordGetZ(b)}
+}
+
+// coordSub3D subtracts two coordinates in 3D and returns a new result.
+func coordSub3D(a geom.Coord, b geom.Coord) geom.Coord {
+	return geom.Coord{a.X() - b.X(), a.Y() - b.Y(), coordGetZ(a) - coordGetZ(b)}
+}
+
+// coordMul3D multiplies a 3D coord by a scalar and returns the new result.
+func coordMul3D(a geom.Coord, s float64) geom.Coord {
+	return geom.Coord{a.X() * s, a.Y() * s, coordGetZ(a) * s}
+}
+
+// coordDot3D returns the dot product of two 3D coord vectors.
+func coordDot3D(a geom.Coord, b geom.Coord) float64 {
+	return a.X()*b.X() + a.Y()*b.Y() + coordGetZ(a)*coordGetZ(b)
+}
+
+// coordNorm2_3D returns the squared norm of a 3D coordinate vector.
+func coordNorm2_3D(c geom.Coord) float64 {
+	return coordDot3D(c, c)
+}
+
+// coordNorm3D returns the Euclidean norm of a 3D coordinate vector.
+func coordNorm3D(c geom.Coord) float64 {
+	return math.Sqrt(coordNorm2_3D(c))
+}
+
+// coordEqual3D returns whether two coordinates are equal in 3D.
+func coordEqual3D(a geom.Coord, b geom.Coord) bool {
+	return a.X() == b.X() && a.Y() == b.Y() && coordGetZ(a) == coordGetZ(b)
+}
+
+// closest3DSegmentSegment finds the closest pair of points on 3D
+// segments [a0, a1] and [b0, b1], using the standard parameterized
+// approach (see Ericson, "Real-Time Collision Detection", §5.1.9).
+// Returns the closest point on segment A and the closest point on
+// segment B as XYZ coordinates.
+func closest3DSegmentSegment(a0, a1, b0, b1 geom.Coord) (geom.Coord, geom.Coord) {
+	const epsilon = 1e-30
+	d1 := coordSub3D(a1, a0)
+	d2 := coordSub3D(b1, b0)
+	r := coordSub3D(a0, b0)
+	a := coordDot3D(d1, d1)
+	e := coordDot3D(d2, d2)
+	f := coordDot3D(d2, r)
+
+	var s, t float64
+	switch {
+	case a <= epsilon && e <= epsilon:
+		// Both segments are degenerate points.
+		return a0, b0
+	case a <= epsilon:
+		// Segment A is a degenerate point; project onto B.
+		t = clamp01(f / e)
+	default:
+		c := coordDot3D(d1, r)
+		if e <= epsilon {
+			// Segment B is a degenerate point; project onto A.
+			s = clamp01(-c / a)
+		} else {
+			// General nondegenerate case.
+			b := coordDot3D(d1, d2)
+			denom := a*e - b*b
+			if denom != 0 {
+				s = clamp01((b*f - c*e) / denom)
+			}
+			t = (b*s + f) / e
+			if t < 0 {
+				t = 0
+				s = clamp01(-c / a)
+			} else if t > 1 {
+				t = 1
+				s = clamp01((b - c) / a)
+			}
+		}
+	}
+	return coordAdd3D(a0, coordMul3D(d1, s)), coordAdd3D(b0, coordMul3D(d2, t))
+}
+
+// clamp01 clamps v to [0, 1].
+func clamp01(v float64) float64 {
+	if v < 0 {
+		return 0
+	}
+	if v > 1 {
+		return 1
+	}
+	return v
+}