Line3: Add method for computing closest squared distance between line segments.

src/math/Line3.js

@@ -4,6 +4,12 @@ import { clamp } from './MathUtils.js';
 const _startP = /*@__PURE__*/ new Vector3();
 const _startEnd = /*@__PURE__*/ new Vector3();
 
+const _d1 = /*@__PURE__*/ new Vector3();
+const _d2 = /*@__PURE__*/ new Vector3();
+const _r = /*@__PURE__*/ new Vector3();
+const _c1 = /*@__PURE__*/ new Vector3();
+const _c2 = /*@__PURE__*/ new Vector3();
+
 /**
  * An analytical line segment in 3D space represented by a start and end point.
  */
@@ -166,6 +172,127 @@ class Line3 {
 
 	}
 
+	/**
+	 * Returns the closest squared distance between this line segment and the given one.
+	 *
+	 * @param {Line3} line - The line segment to compute the closest squared distance to.
+	 * @param {?Vector3} c1 - The closest point on this line segment.
+	 * @param {?Vector3} c2 - The closest point on the given line segment.
+	 * @return {number} The squared distance between this line segment and the given one.
+	 */
+	distanceSqToLine3( line, c1 = _c1, c2 = _c2 ) {
+
+		// from Real-Time Collision Detection by Christer Ericson, chapter 5.1.9
+
+		// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
+		// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
+		// distance between between S1(s) and S2(t)
+
+		const EPSILON = 1e-8 * 1e-8; // must be squared since we compare squared length
+		let s, t;
+
+		const p1 = this.start;
+		const p2 = line.start;
+		const q1 = this.end;
+		const q2 = line.end;
+
+		_d1.subVectors( q1, p1 ); // Direction vector of segment S1
+		_d2.subVectors( q2, p2 ); // Direction vector of segment S2
+		_r.subVectors( p1, p2 );
+
+		const a = _d1.dot( _d1 ); // Squared length of segment S1, always nonnegative
+		const e = _d2.dot( _d2 ); // Squared length of segment S2, always nonnegative
+		const f = _d2.dot( _r );
+
+		// Check if either or both segments degenerate into points
+
+		if ( a <= EPSILON && e <= EPSILON ) {
+
+			// Both segments degenerate into points
+
+			c1.copy( p1 );
+			c2.copy( p2 );
+
+			c1.sub( c2 );
+
+			return c1.dot( c1 );
+
+		}
+
+		if ( a <= EPSILON ) {
+
+			// First segment degenerates into a point
+
+			s = 0;
+			t = f / e; // s = 0 => t = (b*s + f) / e = f / e
+			t = clamp( t, 0, 1 );
+
+
+		} else {
+
+			const c = _d1.dot( _r );
+
+			if ( e <= EPSILON ) {
+
+				// Second segment degenerates into a point
+
+				t = 0;
+				s = clamp( - c / a, 0, 1 ); // t = 0 => s = (b*t - c) / a = -c / a
+
+			} else {
+
+				// The general nondegenerate case starts here
+
+				const b = _d1.dot( _d2 );
+				const denom = a * e - b * b; // Always nonnegative
+
+				// If segments not parallel, compute closest point on L1 to L2 and
+				// clamp to segment S1. Else pick arbitrary s (here 0)
+
+				if ( denom !== 0 ) {
+
+					s = clamp( ( b * f - c * e ) / denom, 0, 1 );
+
+				} else {
+
+					s = 0;
+
+				}
+
+				// Compute point on L2 closest to S1(s) using
+				// t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
+
+				t = ( b * s + f ) / e;
+
+				// If t in [0,1] done. Else clamp t, recompute s for the new value
+				// of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
+				// and clamp s to [0, 1]
+
+				if ( t < 0 ) {
+
+					t = 0.;
+					s = clamp( - c / a, 0, 1 );
+
+				} else if ( t > 1 ) {
+
+					t = 1;
+					s = clamp( ( b - c ) / a, 0, 1 );
+
+				}
+
+			}
+
+		}
+
+		c1.copy( p1 ).add( _d1.multiplyScalar( s ) );
+		c2.copy( p2 ).add( _d2.multiplyScalar( t ) );
+
+		c1.sub( c2 );
+
+		return c1.dot( c1 );
+
+	}
+
 	/**
 	 * Applies a 4x4 transformation matrix to this line segment.
 	 *

test/unit/src/math/Line3.tests.js

@@ -211,6 +211,62 @@ export default QUnit.module( 'Maths', () => {
 
 		} );
 
+		QUnit.test( 'distanceSqToLine3', ( assert ) => {
+
+			const line1 = new Line3();
+			line1.start.set( 0, 0, 0 );
+			line1.end.set( 2, 0, 0 );
+
+			const line2 = new Line3();
+			line2.start.set( 1, 10, 0 );
+			line2.end.set( 1, - 2, 0 );
+
+			assert.numEqual( line1.distanceSqToLine3( line2 ), 0 );
+
+			// Parallel lines case
+			line2.start.set( - 2, 0, 2 );
+			line2.end.set( 20, 0, 2 );
+
+			assert.numEqual( line1.distanceSqToLine3( line2 ), 4 );
+
+			// Closest point on lines from one side is out of segment
+			line1.start.set( 0, 4, 0 );
+			line1.end.set( 2, 2, 0 );
+
+			line2.start.set( 0, 0, 0 );
+			line2.end.set( 4, 0, 0 );
+
+			assert.numEqual( line1.distanceSqToLine3( line2 ), 4 );
+
+			// Closest point on lines from another side is out of segment
+			line1.start.set( 0, 4, 0 );
+			line1.end.set( 3, 1, 0 );
+
+			line2.start.set( 0, 0, 0 );
+			line2.end.set( 1, 0, 0 );
+
+			assert.numEqual( line1.distanceSqToLine3( line2 ), 4.5 );
+
+			// Closest point on lines from both sides is out of the segment
+			line1.start.set( 0, 4, 0 );
+			line1.end.set( 2, 2, 0 );
+
+			line2.start.set( 0, 0, 0 );
+			line2.end.set( 1, 0, 0 );
+
+			assert.numEqual( line1.distanceSqToLine3( line2 ), 5 );
+
+			// General case with skew lines
+			line1.start.set( 4, 0, 0 );
+			line1.end.set( - 4, 0, 0 );
+
+			line2.start.set( 0, 4, 0 );
+			line2.end.set( 0, 0, 4 );
+
+			assert.numEqual( line1.distanceSqToLine3( line2 ), 8 );
+
+		} );
+
 	} );
 
 } );