trajr: Animal Trajectory Analysis

context("3D tests")

# Hack together a random 3D path
gen3dPts <- function(n = 100) {
  pts <- TrajGenerate(n)
  dz <- rnorm(n + 1)
  pts$z[1] <- 10
  for (i in 2:(nrow(pts))) {
    pts$z[i] <- pts$z[i - 1] + dz[i]
  }
  pts
}

#library(rgl)
plot3dTrj <- function(t3, add = FALSE, col = 1, points = FALSE, acc = FALSE) {
  plot3d(t3$x, t3$y, t3$z, add = add, col = col, type = 'l', lwd = 2)
  if (points)
    plot3d(t3$x, t3$y, t3$z, add = T, col = col, type = 's', size = 0.6)
  else
    plot3d(t3$x[1], t3$y[1], t3$z[1], add = T, col = col, type = 's', size = 0.8)
  if (!isFALSE(acc)) {
    sapply(seq_len(nrow(t3) - 2) + 1, function(r) {
      arrow3d(t3[r, c("x", "y", "z")], t3[r, c("x", "y", "z")] + acc[r, ] * 0.0001, col = 2, type = "extrusion")
    })
  }
}

test_that("3D basic", {
  track <- data.frame(x = c(0, 1, 1, 1),
                      y = c(0, 0, 1, 1),
                      z = c(0, 0, 0, 1))
  t3 <- Traj3DFromCoords(track)
  expect_equal(Traj3DLength(t3), 3)
  expect_equal(Traj3DDistance(t3), sqrt(3))
  expect_equal(Traj3DStraightness(t3), sqrt(3) / 3)
  expect_equal(Traj3DStepLengths(t3), rep(1, nrow(track) - 1))

  t32 <- Traj3DFromCoords(track + 0.6)
  expect_equal(Traj3DLength(t32), 3)
  expect_equal(Traj3DDistance(t32), sqrt(3))
  expect_equal(Traj3DStraightness(t32), sqrt(3) / 3)
  expect_equal(Traj3DStepLengths(t32), rep(1, nrow(track) - 1))

  set.seed(1)
  pts <- gen3dPts(100)
  tr <- Traj3DFromCoords(pts, zCol = "z", timeCol = "time")
  expect_gt(Traj3DDistance(tr), TrajDistance(tr))
  expect_gt(Traj3DLength(tr), TrajLength(tr))
  expect_gt(Traj3DLength(tr), Traj3DDistance(tr))

  sm <- Traj3DSmoothSG(tr)
  # A smoothed trajectory should be shorter
  expect_lt(Traj3DLength(sm), Traj3DLength(tr))
  # Expect start and end points to be close to each other, but not identical
  expect_equal(Traj3DDistance(sm), Traj3DDistance(tr), tolerance = .05)

  rs <- Traj3DResampleTime(sm, 0.05)
  # A resampled trajectory (with longer step times) should be even shorter
  expect_lt(Traj3DLength(rs), Traj3DLength(sm))
})

test_that("3d rediscretization", {
  track <- data.frame(x = c(0, 1, 1, 1),
                      y = c(0, 0, 1, 1),
                      z = c(0, 0, 0, 1))
  t3 <- Traj3DFromCoords(track)
  r <- Traj3DRediscretize(t3, 1)
  expect_true(inherits(r, .TRAJ_3D_CLASS))
  expect_equal(r[, c("x", "y", "z")], t3[, c("x", "y", "z")])

  r <- Traj3DRediscretize(t3, 0.5)
  expect_true(inherits(r, .TRAJ_3D_CLASS))
  expect_equal(nrow(r), 7)
  expect_equal(TrajGetUnits(r), TrajGetUnits(t3))
  expect_equal(Traj3DLength(r), Traj3DLength(t3))
  expect_equal(Traj3DDistance(r), Traj3DDistance(t3))
  expect_equal(Traj3DStepLengths(r), rep(0.5, 6))

  r <- Traj3DRediscretize(t3, 0.8)
  expect_true(inherits(r, .TRAJ_3D_CLASS))
  expect_equal(nrow(r), 4)
  expect_equal(TrajGetUnits(r), TrajGetUnits(t3))
  expect_lt(Traj3DLength(r), Traj3DLength(t3))
  expect_lt(Traj3DDistance(r), Traj3DDistance(t3))
  expect_equal(Traj3DStepLengths(r), rep(0.8, 3))

  r <- Traj3DRediscretize(t3, 1.1)
  expect_true(inherits(r, .TRAJ_3D_CLASS))
  expect_equal(nrow(r), 3)
  expect_equal(TrajGetUnits(r), TrajGetUnits(t3))
  expect_lt(Traj3DLength(r), Traj3DLength(t3))
  expect_lt(Traj3DDistance(r), Traj3DDistance(t3))
  expect_equal(Traj3DStepLengths(r), rep(1.1, 2))

  set.seed(1)
  tr <- gen3dPts(100)
  r <- Traj3DRediscretize(tr, 1.1)
  expect_equal(Traj3DStepLengths(r), rep(1.1, nrow(r) - 1))
  expect_lt(Traj3DLength(r), Traj3DLength(tr))
  # Can't say much about distance because it depends on which direction the two
  # ends are facing, but it shouldn't change by more than 1 step length
  expect_lt(abs(Traj3DDistance(r) - Traj3DDistance(tr)), 1.1)
  #plot3dTrj(tr); plot3dTrj(r, add = T, col = 2, points = T)
})

test_that("rediscretize with speed", {
  pts <- gen3dPts(100)
  tr <- Traj3DFromCoords(pts, zCol = "z", timeCol = "time")

  rd <- Traj3DRediscretize(tr, 2, simConstantSpeed = TRUE)
  #plot3dTrj(tr); plot3dTrj(rd, add = T, col = 2, points = T)
  # Start times should be equal
  expect_equal(rd$time[1], tr$time[1])
  # Average speed should be similar
  rdSp <- Traj3DLength(rd) / TrajDuration(rd)
  trjSp <- Traj3DLength(tr) / TrajDuration(tr)
  expect_lt(abs(log(rdSp / trjSp)), log(1.02))

  # Test that simulation without time throws exception
  rd2 <- Traj3DRediscretize(tr, 2, simConstantSpeed = FALSE)
  expect_error(Traj3DRediscretize(rd2, 4, simConstantSpeed = TRUE))
})

test_that("Velocity", {
  track <- data.frame(x = c(0, 1, 1, 1),
                      y = c(0, 0, 1, 1),
                      z = c(0, 0, 0, 1))
  t3 <- Traj3DFromCoords(track)
  expect_equal(c(Traj3DVelocity(t3, "forward")), c(50, 0, 0, NA, 0, 50, 0, NA, 0, 0, 50, NA))
  expect_equal(c(Traj3DVelocity(t3, "backward")), c(NA, 50, 0, 0, NA, 0, 50, 0, NA, 0, 0, 50))
  expect_equal(c(Traj3DVelocity(t3)), c(NA, 25, 0, NA, NA, 25, 25, NA, NA, 0, 25, NA))

  expect_equal(Traj3DSpeed(Traj3DVelocity(t3, "backward")), c(NA, 50, 50, 50))
  expect_equal(Traj3DSpeed(Traj3DVelocity(t3, "forward")), c(50, 50, 50, NA))
  expect_equal(Traj3DSpeed(Traj3DVelocity(t3, "central")), c(NA, sqrt(25^2 + 25^2), sqrt(25^2 + 25^2), NA))

  # Some random trajectories
  set.seed(1)
  tr <- Traj3DFromCoords(gen3dPts(1000), zCol = "z", timeCol = "time")
  diffs <- Traj3DSpeed(Traj3DVelocity(tr)) - Mod(TrajVelocity(tr))
  # All speeds should be >= the speed ignoring z
  expect_true(all(is.na(diffs) | diffs >= 0))
  # Some speeds should be strictly >
  expect_true(any(diffs > 0))
  tr <- Traj3DFromCoords(gen3dPts(1000), zCol = "z", timeCol = "time")
  diffs <- Traj3DSpeed(Traj3DVelocity(tr)) - Mod(TrajVelocity(tr))
  # All speeds should be >= the speed ignoring z
  expect_true(all(is.na(diffs) | diffs >= 0))
  # Some speeds should be strictly >
  expect_true(any(diffs > 0))

  # Acceleration
  acc <- Traj3DAcceleration(t3)
  expect_equal(nrow(acc), nrow(t3))
  expect_true(all(is.na(acc[1,])))
  expect_true(all(is.na(acc[nrow(t3),])))

  tr <- Traj3DFromCoords(gen3dPts(20), zCol = "z", timeCol = "time")

  # plot3dTrj(tr, acc = Traj3DAcceleration(tr))
})

##### TODO
# test_that("Mean vector", {
#   track <- data.frame(x = c(0, 1, 1),
#                       y = c(0, 0, 1),
#                       z = c(0, 0, 0))
#   t3 <- Traj3DFromCoords(track)
#   mv <- Traj3DMeanVectorOfTurningAngles(t3)
#   mv2 <- TrajMeanVectorOfTurningAngles(t3)
#   expect_equal(norm(as.matrix(mv), type = "2"), Mod(mv2))
#
#   track <- data.frame(x = c(0, 1, 1, 1, 3),
#                       y = c(0, 0, 1, 2, 4),
#                       z = c(0, 0, 0, 0, 0))
#   t3 <- Traj3DFromCoords(track)
#   mv <- Traj3DMeanVectorOfTurningAngles(t3)
#   mv2 <- TrajMeanVectorOfTurningAngles(t3)
#   expect_equal(norm(as.matrix(mv), type = "2"), Mod(mv2))
#
#   track <- data.frame(x = c(0, 1, 1, 1),
#                       y = c(0, 0, 1, 1),
#                       z = c(0, 0, 0, 1))
#   t3 <- Traj3DFromCoords(track)
#   mv <- Traj3DMeanVectorOfTurningAngles(t3)
# })