public static double crossTrackDistanceRad(double lat1,double lon1,double lat2,double lon2,double latQ,double lonQ){
  final double dlon12=lon2 - lon1;
  final double dlon1Q=lonQ - lon1;
  final double dlat1Q=latQ - lat1;
  final double clat1=Math.cos(lat1), slat1=MathUtil.cosToSin(lat1,clat1);
  final double clatQ=Math.cos(latQ), slatQ=MathUtil.cosToSin(latQ,clatQ);
  final double clat2=Math.cos(lat2), slat2=MathUtil.cosToSin(lat2,clat2);
  final double slat=Math.sin(dlat1Q * .5);
  final double slon=Math.sin(dlon1Q * .5);
  final double a=slat * slat + slon * slon * clat1* clatQ;
  final double angDist1Q=2 * Math.atan2(Math.sqrt(a),Math.sqrt(1 - a));
  final double sdlon12=Math.sin(dlon12), cdlon12=MathUtil.sinToCos(dlon12,sdlon12);
  final double sdlon1Q=Math.sin(dlon1Q), cdlon1Q=MathUtil.sinToCos(dlon1Q,sdlon1Q);
  final double yE=sdlon12 * clat2;
  final double yQ=sdlon1Q * clatQ;
  final double xE=clat1 * slat2 - slat1 * clat2 * cdlon12;
  final double xQ=clat1 * slatQ - slat1 * clatQ * cdlon1Q;
  final double crs12=Math.atan2(yE,xE);
  final double crs1Q=Math.atan2(yQ,xQ);
  return Math.asin(Math.sin(angDist1Q) * Math.sin(crs1Q - crs12));
}

public static double crossTrackDistanceRad ( double lat1 , double lon1 , double lat2 , double lon2 , double latQ , double lonQ ) { final double dlon12 = lon2 - lon1 ; final double dlon1Q = lonQ - lon1 ; final double dlat1Q = latQ - lat1 ; final double clat1 = Math . cos ( lat1 ) , slat1 = MathUtil . cosToSin ( lat1 , clat1 ) ; final double clatQ = Math . cos ( latQ ) , slatQ = MathUtil . cosToSin ( latQ , clatQ ) ; final double clat2 = Math . cos ( lat2 ) , slat2 = MathUtil . cosToSin ( lat2 , clat2 ) ; final double slat = Math . sin ( dlat1Q * .5 ) ; final double slon = Math . sin ( dlon1Q * .5 ) ; final double a = slat * slat + slon * slon * clat1 * clatQ ; final double angDist1Q = 2 * Math . atan2 ( Math . sqrt ( a ) , Math . sqrt ( 1 - a ) ) ; final double sdlon12 = Math . sin ( dlon12 ) , cdlon12 = MathUtil . sinToCos ( dlon12 , sdlon12 ) ; final double sdlon1Q = Math . sin ( dlon1Q ) , cdlon1Q = MathUtil . sinToCos ( dlon1Q , sdlon1Q ) ; final double yE = sdlon12 * clat2 ; final double yQ = sdlon1Q * clatQ ; final double xE = clat1 * slat2 - slat1 * clat2 * cdlon12 ; final double xQ = clat1 * slatQ - slat1 * clatQ * cdlon1Q ; final double crs12 = Math . atan2 ( yE , xE ) ; final double crs1Q = Math . atan2 ( yQ , xQ ) ; return Math . asin ( Math . sin ( angDist1Q ) * Math . sin ( crs1Q - crs12 ) ) ; }
compute the cross - track distance . xtd = asin ( sin ( dist_sq ) * sin ( crs_sq - crs_se ) )
