@ Pure public static double distanceSquaredSegmentSegment ( double ax1 , double ay1 , double az1 , double ax2 , double ay2 , double az2 , double bx1 , double by1 , double bz1 , double bx2 , double by2 , double bz2 ) { Vector3f u = new Vector3f ( ax2 - ax1 , ay2 - ay1 , az2 - az1 ) ; Vector3f v = new Vector3f ( bx2 - bx1 , by2 - by1 , bz2 - bz1 ) ; Vector3f w = new Vector3f ( ax1 - bx1 , ay1 - by1 , az1 - bz1 ) ; double a = u . dot ( u ) ; double b = u . dot ( v ) ; double c = v . dot ( v ) ; double d = u . dot ( w ) ; double e = v . dot ( w ) ; double D = a * c - b * b ; double sc , sN , tc , tN ; double sD = D ; double tD = D ; // compute the line parameters of the two closest points if ( MathUtil . isEpsilonZero ( D ) ) { // the lines are almost parallel // force using point P0 on segment S1 // to prevent possible division by 0.0 later sN = 0. ; sD = 1. ; tN = e ; tD = c ; } else { // get the closest points on the infinite lines sN = b * e - c * d ; tN = a * e - b * d ; if ( sN < 0. ) { // sc < 0 => the s=0 edge is visible sN = 0. ; tN = e ; tD = c ; } else if ( sN > sD ) { // sc > 1 => the s=1 edge is visible sN = sD ; tN = e + b ; tD = c ; } } if ( tN < 0. ) { // tc < 0 => the t=0 edge is visible tN = 0. ; // recompute sc for this edge if ( - d < 0. ) sN = 0. ; else if ( - d > a ) sN = sD ; else { sN = - d ; sD = a ; } } else if ( tN > tD ) { // tc > 1 => the t=1 edge is visible tN = tD ; // recompute sc for this edge if ( ( - d + b ) < 0. ) sN = 0 ; else if ( ( - d + b ) > a ) sN = sD ; else { sN = ( - d + b ) ; sD = a ; } } // finally do the division to get sc and tc sc = ( MathUtil . isEpsilonZero ( sN ) ? 0. : sN / sD ) ; tc = ( MathUtil . isEpsilonZero ( tN ) ? 0. : tN / tD ) ; // get the difference of the two closest points // = S1(sc) - S2(tc) // reuse u, v, w u . scale ( sc ) ; w . add ( u ) ; v . scale ( tc ) ; w . sub ( v ) ; return w . lengthSquared ( ) ; }
Compute and replies the squared distance between two segments .