private int findBestCompatible ( LineSegment2D_F32 target , List < LineSegment2D_F32 > candidates , int start ) { int bestIndex = - 1 ; double bestDistance = Double . MAX_VALUE ; int bestFarthest = 0 ; float targetAngle = UtilAngle . atanSafe ( target . slopeY ( ) , target . slopeX ( ) ) ; float cos = ( float ) Math . cos ( targetAngle ) ; float sin = ( float ) Math . sin ( targetAngle ) ; for ( int i = start ; i < candidates . size ( ) ; i ++ ) { LineSegment2D_F32 c = candidates . get ( i ) ; float angle = UtilAngle . atanSafe ( c . slopeY ( ) , c . slopeX ( ) ) ; // see if the two lines have the same slope if ( UtilAngle . distHalf ( targetAngle , angle ) > lineSlopeAngleTol ) continue ; // see the distance the two lines are apart and if it could be the best line closestFarthestPoints ( target , c ) ; // two closest end points Point2D_F32 pt0 = closestIndex < 2 ? target . a : target . b ; Point2D_F32 pt1 = ( closestIndex % 2 ) == 0 ? c . a : c . b ; float xx = pt1 . x - pt0 . x ; float yy = pt1 . y - pt0 . y ; float distX = Math . abs ( cos * xx - sin * yy ) ; float distY = Math . abs ( cos * yy + sin * xx ) ; if ( distX >= bestDistance || distX > parallelTol || distY > tangentTol ) continue ; // check the angle of the combined line pt0 = farthestIndex < 2 ? target . a : target . b ; pt1 = ( farthestIndex % 2 ) == 0 ? c . a : c . b ; float angleCombined = UtilAngle . atanSafe ( pt1 . y - pt0 . y , pt1 . x - pt0 . x ) ; if ( UtilAngle . distHalf ( targetAngle , angleCombined ) <= lineSlopeAngleTol ) { bestDistance = distX ; bestIndex = i ; bestFarthest = farthestIndex ; } } if ( bestDistance < parallelTol ) { farthestIndex = bestFarthest ; return bestIndex ; } return - 1 ; }
Searches for a line in the list which the target is compatible with and can be connected to .