R/echoIBM.fishReaction.R

In arnejohannesholmin/echoIBM: Simulation of multibeam sonar data

#*********************************************
#*********************************************
#' This function adds randomness in the fish positions and to the fish velocities (given by the polarization).
#'
#' @param data		A list containing the dynamic fish data, as generated by e.g. \code{\link{echoIBM.generate_dynschool}}.
#' @param grsd_plHS	A list or data frame linking standard deviations and polarization values. If not given, this is read by grsd_plHS <- read.TSD(system.file("extdata", "grsd_plHS.TSD", package="echoIBM")).
#'
#' @return
#'
#' @examples
#' \dontrun{}
#'
#' @importFrom TSD read.TSD sph2car
#' @importFrom stats approx rnorm
#'
#' @export
#' @rdname echoIBM.addRandomness
#'
echoIBM.fishReaction <- function(data){
	
	### magR # Fish reaction magnitude (funciton)
	### mxrR # Fish reaction maximum range (inside which fish react by a magnitude given by magR)
	### mxdR # Fish reaction maximum displacement (the maximum displacement of the reacting fish)
	### facR # Factors for each of the three dimensions x, y and z, determining the direction of the maximum reaction. This could be c(1, 1, 1) to represent reaction away from the vessel in all directions, or c(1, 1, 0) for a strictly horizontal reaction.
	if(!all(sapply(data[c("mg1R", "mxrR", "fcxR", "fcyR", "fczR")], function(x) length(x)>0))){
		return(data)
	}
	if(all(data$mg1R==0, data$mg2R==0)){
		return(data)
	}
	if(length(data$rtzf)==0){
		return(data)
	}
	
	
	if(length(data$mxdR)==0){
		data$mxdR <- data$mxrR
	}
	
	# mg1R <- 1
	# mg2R <- 0.5
	# mxrR <- 150
	# mxdR <- 100
	# fcxR <- 1
	# fcyR <- 1
	# fczR <- 0
	
	#data$magR <- c(1, 1)
	#data$mxrR <- 150
	#data$mxdR <- 100
	#data$facR <- c(1, 1, 0)
	
	
	# Save the original positions and rotations:
	saved <- data[c("psxf", "psyf", "pszf", "rtxf", "rtzf")]
	
	
	# Get the direction of the fish (add pi/2 since rtzf = 0 corresponds to along the y axis of the global coordinate system, which is at pi/2):
	fish_direction <- cbind(1, data$rtzf + pi/2, pi/2 - data$rtxf)
	
	# Get the direction from the vessel to the fish:
	vessel2fish_direction <- cbind( data$psxf - data$psxv, data$psyf - data$psyv, data$pszf - (data$pszv + data$psze) )
	
	# Get the range to the vessel:
	#rangeToVessel <- sqrt((data$psxf - data$psxv)^2 + (data$psyf - data$psyv)^2 + (data$pszf - data$pszv)^2)
	rangeToVessel <- sqrt(rowSums(vessel2fish_direction^2))
	vessel2fish_unitVector <- vessel2fish_direction / rangeToVessel
	###vessel2fish_direction <- car2sph(vessel2fish_unitVector)
	
	
	# Apply the reaction magnitude function, resulting in a vector in the interval [0, 1]:
	#if(is.function(data$magR)){
	#	data$magR <- data$magR(rangeToVessel)
	#}
	#else if(length(data$magR)){
		# Declare a vector of all zeros:
		temp <- double(length(rangeToVessel))
		# Fill in with lineary decreasing values inside the reaction range:
		inside <- rangeToVessel <= data$mxrR
		temp[inside] <- data$mg1R + (rangeToVessel[inside] / data$mxrR) * diff(c(data$mg1R, data$mg2R))
		data$magR <- temp
		#}
	
	# Apply the reaction factors for each dimension, possibly resticting only to horizontal reaction:
	displacement_vector <- vessel2fish_unitVector * matrix(c(data$fcxR, data$fcyR, data$fczR), byrow=TRUE, ncol=3, nrow=nrow(vessel2fish_unitVector))
	# Normalize to length 1:
	displacement_vector <- displacement_vector / sqrt(rowSums(displacement_vector^2))
	
	# And the reaction direction in spherical coordinates:
	displacement_direction <- car2sph(displacement_vector)
	
	
	
	
	##############################################
	# Get the displacement reaction in the meters:
	displacement <- displacement_vector * data$magR * data$mxdR
	#displacement <- displacement_vector * data$magR * data$mxrR
	# Get the new positions:
	data$psxf <- data$psxf + displacement[,1]
	data$psyf <- data$psyf + displacement[,2]
	data$pszf <- data$pszf + displacement[,3]
	##############################################
	
	
	# Get the difference in the heading of the fish and the direction of the reaction:
	diffDirection <- displacement_direction - fish_direction
	# Assure that the angles are in [-pi, pi], so that the change in direction takes the shortest angle between the two direcitons:
	outside <- diffDirection[,2] < -pi | diffDirection[,2] > pi
	diffDirection[,2] <- (diffDirection[,2] + pi) %% (2 * pi) - pi
	
	# Get the new direction, added the fraction of the difference in direction specified by data$magR:
	new_fish_direction <- fish_direction + diffDirection * data$magR
	
	# Convert back to rotation angles:
	data$rtzf <- new_fish_direction[,2] - pi/2
	data$rtxf <- pi/2 - new_fish_direction[,3]
	
	#sel <- sample(1:2181270, 1000)
	#rgl::open3d()
	#rgl::plot3d(data$psxf[sel], data$psyf[sel], data$pszf[sel], size=(1 + sin(displacement_direction[sel,2]))/2, col=2, type="s")
	#rgl::open3d()
	#rgl::plot3d(data$psxf[sel], data$psyf[sel], data$pszf[sel], size=(1 + sin(new_fish_direction[sel,2]))/2, col=2, type="s")
	
	
	# Add the saved data:
	names(saved) <- paste0(substr(names(saved), 1, 3), 0)
	
	data <- c(data, saved)
	
	
	return(data)
}