calculateTCPA.R (1535B)
1 #' Calculate the time to closest point of approach (TCPA) between two aircraft 2 #' at each time point of their their trajectories. 3 #' 4 #' @param trajectory1 A \code{flighttrajectory} object corresponding to the 5 #' first aircraft. 6 #' @param trajectory2 A \code{flighttrajectory} object corresponding to the 7 #' second aircraft. 8 #' @return The numeric vector giving the TCPA. Values in seconds. 9 #' 10 #' @details This code is based on the SLoWC formulation by Ethan Pratt and Jacob 11 #' Kay as implemented in a MATLAB script by Ethan Pratt dated 2016-04-18. This 12 #' code calculates horizontal CPA only. 13 #' 14 #' @export 15 calculateTCPA <- function(trajectory1, trajectory2) { 16 checkTrajectories(trajectory1, trajectory2) 17 18 if (!is.flattrajectory(trajectory1)) { 19 lon0 <- mean(c(trajectory1$longitude, trajectory2$longitude)) 20 lat0 <- mean(c(trajectory1$latitude, trajectory2$latitude)) 21 22 trajectory1 <- trajectoryToXYZ(trajectory1, c(lon0, lat0)) 23 trajectory2 <- trajectoryToXYZ(trajectory2, c(lon0, lat0)) 24 } 25 26 # Distance between aircraft 27 dXYZ <- trajectory2$position - trajectory1$position 28 dXYZ[, 3] <- abs(dXYZ[, 3]) 29 relativeVelocity <- trajectory2$velocity - trajectory1$velocity 30 31 # Calculate time to CPA and projected HMD 32 tCPADividend <- -apply(dXYZ[, 1:2, drop = FALSE] * relativeVelocity, 1, sum) 33 tCPADivisor <- apply(relativeVelocity^2, 1, sum) 34 tCPA <- tCPADividend / tCPADivisor 35 # Safegaurd against singularity 36 tCPA[tCPADivisor == 0 | tCPADividend < 0] <- 0 37 38 return(tCPA) 39 }