#' Sample unit sphere uniformly
#'
#' @param n sample size
#' @param spherical Return azimuth-inclination? (Def: FALSE)
#' @details
#' Default is to return unit vectors.
#' @export
runifsphere <- function(n, spherical=FALSE){
u <- runif(n)
v <- runif(n)
a <- u * 2 * pi
i <- acos(2*v - 1)
ai<- cbind(azi=a, inc=i)
if(spherical) ai
else ai2xyz(ai)
}
# This from sphere-package, file coords.R
#' OLD for plotting on the globe, we need to trasform as the texture is fuckd up
#'
xyz2globe <- function(xyz) {
xyzrotate(xyz, ax=pi/2, ay=-pi/2)
}
#' latlon to 3d coordinates
#'
#' @param latlon latitude-longitude
#'
#' @export
ll2xyz <- function(latlon) {
latlon <- rbind(latlon)
azi <- lon2azi(latlon[,2])
inc <- lat2inc(latlon[,1])
ai2xyz(cbind(azi,inc))
}
#' (azi,incl) to 3d coordinates
#'
#' @param aziinc azi, directions
#'
#' @export
ai2xyz <- function(aziinc) {
aziinc <- rbind(aziinc)
azi <- aziinc[,1]
inc <- aziinc[,2]
wx<-sin(inc)*cos(azi)
wy<-sin(inc)*sin(azi)
wz<-cos(inc)
cbind(x=wx,y=wy,z=wz)
}
#' latitude to inclination
#'
#' @param lat latitude
#'
#' @export
lat2inc <- function(lat) {
pi/2 - lat
}
#' inclination to latitude
#'
#' @param inc inclination
#'
#' @export
inc2lat <- function(inc){
pi/2 - inc
}
#' azimuth to longitude
#'
#' @param azi azimuth
#'
#' @export
azi2lon <- function(azi){
lon <- azi
lon[lon>pi] <- -(2*pi-azi[azi>pi])
lon
}
#' longitude to azimuth
#'
#' @param lon longitude
#'
#' @export
lon2azi <- function(lon){
azi <- lon
azi[azi < 0] <- 2*pi + azi[azi < 0]
azi
}
#' antipode lat and lon
#'
#' @param latlon lat lon
#'
#' @export
antipode <- function(latlon){
latlon<-rbind(latlon)
cbind(-latlon[,1], latlon[,2]-sign(latlon[,2])*pi)
}
#' xyz to lat lon
#'
#' @param xyz unit vector
#'
#' @export
xyz2ll <- function(xyz) {
ai <- xyz2ai(xyz)
inc <- ai[,2]
azi <- ai[,1]
cbind(lat=inc2lat(inc), lon=azi2lon(azi))
}
#' azi-inc to lat lon
#'
#' @param aziinc azi incl
#'
#' @export
ai2ll <- function(aziinc){
aziinc <- rbind(aziinc)
cbind(lat=inc2lat(aziinc[,2]), lon=azi2lon(aziinc[,1]))
}
#'latlon to aziinc
#'
#' @param latlon lat lon
#'
#' @export
ll2ai <- function(latlon){
latlon <- rbind(latlon)
cbind(azi=lon2azi(latlon[,2]), inc=lat2inc(latlon[,1]))
}
#' xyz to azi inc
#'
#' @param xyz unit vector 3D
#'
#' @export
xyz2ai <- function(xyz) {
xyz <- rbind(xyz)
r <- apply(xyz, 1, function(a)sqrt(sum(a^2)))
inc <- acos(xyz[,3]/r)
azi <- atan2(xyz[,2],xyz[,1])
cbind(azi=azi, inc=inc)
}
#' rotate xyz coordinates
#'
#' @param xyz coordinates
#' @param ... passed to rotationMatrix3
#'
#' @export
xyzrotate <- function(xyz, ...) {
R <- rotationMatrix3(...)
xyz %*% R
}
#' Product of elementary rotation matrices in 3D
#' @param ax,ay,az radians around each axis
#' @param order Order in which to to the product. Aimed for pre-product
#' Default: 3-2-1 i.e. around z, then around y, then around x.
#'
#' @export
rotationMatrix3 <- function(ax=0, ay=0, az=0, order=c(3,2,1)) {
R<-list()
R[[1]] <- cbind(c(1,0,0), c(0, cos(ax), sin(ax)), c(0, -sin(ax), cos(ax)) )
R[[2]] <- cbind(c(cos(ay), 0, -sin(ay)), c(0, 1, 0), c(sin(ay), 0, cos(ay)) )
R[[3]] <- cbind(c(cos(az), sin(az), 0), c(-sin(az), cos(az), 0), c(0,0,1) )
R[[order[3]]]%*%R[[order[2]]]%*%R[[order[1]]]
}
#' Rotation using azimuth-inclination.
#'
#' @param rot_ai angles
#'
#' @export
aziinc2rotationMatrix <- function(rot_ai=c(0,0)){
rotationMatrix3(az=rot_ai[1])%*%(rotationMatrix3(ay=rot_ai[2]))
}
#' Rotation matrix to quaternion
#'
#' @param R matrix
#'
#' @export
rotationMatrix2quaternion <- function(R){
q4 <- 0.5 * sqrt(1 + R[1,1] + R[2,2] + R[3,3])
if(is.na(q4)) q4<-0 # improper rotation
if(q4 < 1e-5){ # try other way
q1 <- 0.5 * sqrt(1+R[1,1]-R[2,2]-R[3,3])
if(q1 < 1e-5){ # try other way
q2 <- 0.5 * sqrt(1-R[1,1]+R[2,2]-R[3,3])
if(q2 < 1e-5){
q3 <- 0.5 * sqrt(1-R[1,1]-R[2,2]+R[3,3])
if(q3 < 1e-5) stop("Can't convert matrix to quaternion.")
m <- 1/(4*q3) # q3 ok
q1 <- m * (R[3,1]+R[1,3])
q2 <- m * (R[3,2]+R[2,3])
q4 <- m * (R[2,1]-R[1,2])
}else{ # q2 ok
m <- 1/(4*q2)
q1 <- m * (R[2,1]+R[1,2])
q3 <- m * (R[3,2]+R[2,3])
q4 <- m * (R[1,3]-R[3,1])
}
}else{ # q1 ok
m <- 1/(4*q1)
q2 <- m * (R[1,2]+R[2,1])
q3 <- m * (R[1,3]+R[3,1])
q4 <- m * (R[3,2]-R[2,3])
}
}else{ # q4 ok
m <- 1/(4*q4)
q1 <- m * (R[3,2]-R[2,3])
q2 <- m * (R[1,3]-R[3,1])
q3 <- m * (R[2,1]-R[1,2])
}
c(q1,q2,q3,q4)
}
#' Quaternion to rotation matrix
#'
#' @param q quartenion vector
#'
#' @export
quaternion2rotationMatrix <- function(q){
q3 <- q[-4]
Q <- matrix(c(0,q[3],-q[2],-q[3],0,q[1],q[2],-q[1],0), 3)
c(q[4]^2-t(q3)%*%q3) * diag(1,3) + 2*q3%*%t(q3) + 2*q[4]*Q
}
#' Quaternion to Euler axis/angle
#'
#' @param q quartenion vector
#'
#' @export
quaternion2Euler <- function(q){
e <- q[-4]/sqrt(sum(q[-4]^2))
theta <- 2 * acos(q[4])
c(e=e, theta=theta)
}
#' Euler axis/angle to quaternion
#'
#' @param euler c(e1,e2,e3,angle) where e* is the unit axis of rotation
#'
#' @export
Euler2quaternion <- function(euler){
theta <- euler[4]
m <- sin(theta/2)
c(q=euler[1:3]*m, q4=cos(theta/2))
}
#' Quaternion to Euler angles
#'
#' from euclideanspace.com
#'
#'
#' @param q quartenion vector
#'
#' @details
#'
#' Convention: Euler is (Heading, Attitude, Bank) so that
#' Heading = rotation around y, Attitude = rotation around z,
#' Bank = rotation around x, in that order.
#'
#' @export
quaternion2EulerAngles <- function(q){
# if(q[2]*q[3]+q[3]*q[1]==0.5){
# phi <- 2*atan2(q[2],q[1])
#
# }
phi <- atan2(2*(q[1]*q[3]-q[2]*q[4]), 1-2*(q[3]^2+q[4]^2))
theta <- asin(2*(q[2]*q[3]+q[1]*q[4]))
psi <- atan2(2*(q[1]*q[2]-q[3]*q[4]), 1-2*(q[2]^2+q[4]^2))
c(phi=phi, theta=theta, psi=psi)
}
#' Euler angles to quaternions
#'
#' @param angle Euler angle vector
#'
#' @export
EulerAngles2quaternion <- function(angle){
cc <- cos(angle/2)
ss <- sin(angle/2)
q1 <- cc[1]*cc[2]*cc[3] - ss[1]*ss[2]*ss[3]
q2 <- ss[1]*ss[2]*cc[3] + cc[1]*cc[2]*ss[3]
q3 <- ss[1]*cc[2]*cc[3] + cc[1]*ss[2]*ss[3]
q4 <- cc[1]*ss[2]*cc[3] - ss[1]*cc[2]*ss[3]
c(q1,q2,q3,q4)
}
#' Rotation matrix (prop or improp) to Euler axis-angle
#'
#' @param R Rotation matrix
#'
#' @export
rotationMatrix2Euler <- function(R){
e <- det(R)
Tr <- sum(diag(R))
a <- 3-e*Tr
b <- 1+e*Tr
if(a==0){
theta <- acos(e)
n <- c(1,0,0)
}
else if(b==0){
theta <- acos(-e)
eR <- e*R
e1 <- e2 <- e3 <- 1
n1 <- sqrt(1+eR[1,1])
n2 <- sqrt(1+eR[2,2])
n3 <- sqrt(1+eR[3,3])
if(n1){
if(n2) e2 <- eR[1,2]/(n1*n2)
if(n3) e3 <- eR[1,3]/(n1*n3)
}else{
if(n2 & n3) e2 <- e3 <- eR[2,3]/(n2*n3)
}
n <- c(n1,n2,n3)*c(e1,e2,e3)
n <- n/sum(n)
}
else{
theta <- acos(0.5 * (Tr-e))
n <- c(R[3,2]-R[2,3], R[1,3]-R[3,1], R[2,1]-R[1,2]) /sqrt(a*b)
}
v <- c(axis=n, angle=theta)
if(e < 0) attr(v, "proper") <- FALSE
v
}
#' Euler axis-angle to Rotation matrix
#'
#' @param v euler vector
#' @param proper proper
#'
#' @export
Euler2rotationMatrix <- function(v, proper=TRUE){
n <- v[1:3]
a <- v[4]
co <- cos(a)
si <- sin(a)
if(proper){
r1 <- c(co + n[1]^2*(1-co), n[1]*n[2]*(1-co)-n[3]*si, n[1]*n[3]*(1-co)+n[2]*si)
r2 <- c(n[1]*n[2]*(1-co)+n[3]*si, co+n[2]^2*(1-co), n[2]*n[3]*(1-co)-n[1]*si)
r3 <- c(n[1]*n[3]*(1-co)-n[2]*si, n[2]*n[3]*(1-co)+n[1]*si, co+n[3]^2*(1-co))
} else{
r1 <- c(co - n[1]^2*(1+co), -n[1]*n[2]*(1+co)-n[3]*si, -n[1]*n[3]*(1+co)+n[2]*si)
r2 <- c(-n[1]*n[2]*(1+co)+n[3]*si, co-n[2]^2*(1+co), -n[2]*n[3]*(1+co)-n[1]*si)
r3 <- c(-n[1]*n[3]*(1+co)-n[2]*si, -n[2]*n[3]*(1+co)+n[1]*si, co-n[3]^2*(1+co))
}
R <- rbind(r1, r2, r3)
unname(R)
}
#' Rotation matrix to Euler angles
#'
#' @param R rotation matrix
#'
#' @return
#' (Heading, Attitude, Bank)
#'
#' @export
rotationMatrix2EulerAngles <- function(R){
quaternion2EulerAngles(rotationMatrix2quaternion(R))
}
#' Euler angles to Rotation matrix
#'
#' @param angle Euler angle vector
#'
#' @export
EulerAngles2rotationMatrix <- function(angle){
quaternion2rotationMatrix(EulerAngles2quaternion(angle))
}
# perkele
# xyz<-u <- unit.vecs(3)
# e<-xyz2ll(u)
# f <- ll2xyz(e)
# all.equal(u,f)
#
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.