Nothing
R/RSpinconv.R
In RSpincalc: Conversion Between Attitude Representations of DCM, Euler Angles, Quaternions, and Euler Vectors
# Q Rotation Quaternions Q - [q1,q2,q3,q4] (Nx4)
# EV Euler Vector and rotation angle (degrees) EV - [m1,m2,m3,MU] (Nx4)
# DCM Orthogonal DCM Rotation Matrix DCM - 3x3xN
# EA Euler angles (12 possible sets) (degrees) EA - [psi,theta,phi] (Nx3)
# DCM2EA DCM2EV DCM2Q
# EA2DCM EA2EV EA2Q EA2EA
# EV2DCM EV2EA EV2Q
# Q2DCM Q2EA Q2EV Q2GL
EA2Q<-function(EA, EulerOrder='zyx', ichk=FALSE, ignoreAllChk=FALSE)
{# EA - [psi,theta,phi] yaw, pitch, roll to EV - [m1,m2,m3,MU]
# EA in radians
# ichk = FALSE disables near-singularity warnings.
# Identify singularities (2nd Euler angle out of range)
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EA)) EA <- matrix(EA,ncol=3,byrow=FALSE)
theta = EA[, 2] # N×1
if (!ignoreAllChk){
if (substr(EulerOrder,1,1) != substr(EulerOrder,3,3)) {# Type 1 rotation about three distinct axes
# Type 1 rotation (rotations about three distinct axes)
if (any(abs(theta) >= pi/2 )) { stop('Second input Euler angle(s) outside -90 to 90 degree range')
} else if (ichk && any(abs(theta)>88 * (pi/180))) warning('Warning: Second input Euler angle(s) near a singularity (-90 or 90 degrees).')
} else {
# Type 2 rotation (1st and 3rd rotation about same axis)
if (any((theta<=0) | (theta >= pi))) { stop('Second input Euler angle(s) outside 0 to 180 degree range')
} else if (ichk && (any(theta < 2 * (pi/180)) | (theta>178 * (pi/180)))) warning('Warning: Second input Euler angle(s) near a singularity (0 or 180 degrees).')
}}
# Half angles in radians
HALF = EA / 2# * (pi/360) # N×3
Hpsi = matrix(HALF[,1],ncol=1) # N×1
Htheta = matrix(HALF[,2],ncol=1) # N×1
Hphi = matrix(HALF[,3],ncol=1) # N×1
# Pre-calculate cosines and sines of the half-angles for conversion.
c1=cos(Hpsi); c2=cos(Htheta); c3=cos(Hphi)
s1=sin(Hpsi); s2=sin(Htheta); s3=sin(Hphi)
c13 =cos(Hpsi+Hphi); s13 =sin(Hpsi+Hphi)
c1_3=cos(Hpsi-Hphi); s1_3=sin(Hpsi-Hphi)
c3_1=cos(Hphi-Hpsi); s3_1=sin(Hphi-Hpsi)
if (EulerOrder=='xyx') Q=cbind(c2*c13, c2*s13,s2*c1_3, s2*s1_3) else
if (EulerOrder=='yzy') Q=cbind(c2*c13, s2*s1_3,c2*s13, s2*c1_3) else
if (EulerOrder=='zxz') Q=cbind(c2*c13, s2*c1_3,s2*s1_3, c2*s13) else
if (EulerOrder=='xzx') Q=cbind(c2*c13, c2*s13,s2*s3_1, s2*c3_1) else
if (EulerOrder=='yxy') Q=cbind(c2*c13, s2*c3_1,c2*s13, s2*s3_1) else
if (EulerOrder=='zyz') Q=cbind(c2*c13, s2*s3_1,s2*c3_1, c2*s13) else
if (EulerOrder=='xyz') Q=cbind(c1*c2*c3-s1*s2*s3, s1*c2*c3+c1*s2*s3,c1*s2*c3-s1*c2*s3, c1*c2*s3+s1*s2*c3) else
if (EulerOrder=='yzx') Q=cbind(c1*c2*c3-s1*s2*s3, c1*c2*s3+s1*s2*c3,s1*c2*c3+c1*s2*s3, c1*s2*c3-s1*c2*s3) else
if (EulerOrder=='zxy') Q=cbind(c1*c2*c3-s1*s2*s3, c1*s2*c3-s1*c2*s3,c1*c2*s3+s1*s2*c3, s1*c2*c3+c1*s2*s3) else
if (EulerOrder=='xzy') Q=cbind(c1*c2*c3+s1*s2*s3, s1*c2*c3-c1*s2*s3,c1*c2*s3-s1*s2*c3, c1*s2*c3+s1*c2*s3) else
if (EulerOrder=='yxz') Q=cbind(c1*c2*c3+s1*s2*s3, c1*s2*c3+s1*c2*s3,s1*c2*c3-c1*s2*s3, c1*c2*s3-s1*s2*c3) else
if (EulerOrder=='zyx') Q=cbind(c1*c2*c3+s1*s2*s3, c1*c2*s3-s1*s2*c3,c1*s2*c3+s1*c2*s3, s1*c2*c3-c1*s2*s3) else
if (!ignoreAllChk) stop('Invalid input Euler angle order')
Q
}
EV2Q <- function(EV,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EV - [m1,m2,m3,MU] to Q - [q1,q2,q3,q4]
# Euler vector (EV) and angle MU in radians
if(is.null(dim(EV))) EV<-matrix(EV,ncol=4,byrow=FALSE)
EVtmp = matrix(EV[,1:3],ncol=3,byrow=FALSE) # N×3
halfMU = matrix(EV[,4] / 2,ncol=1) #* (pi/360) (N×1) MU/2 in radians
# Check that input m's constitute unit vector
delta = sqrt(matrix(apply(EVtmp^2,1,sum),ncol=1)) - 1 # N×1
if (!ignoreAllChk) if (any(abs(delta) > tol)) stop('(At least one of the) input Euler vector(s) is not a unit vector')
# Quaternion
SIN = sin(halfMU) # (N×1)
Q = cbind(EVtmp[,2]*SIN, EVtmp[,3]*SIN, cos(halfMU), EVtmp[,1]*SIN)
Q
}
DCM2Q <- function(DCM,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# DCM - 3x3xN to Q - [q1,q2,q3,q4]
# NOTE: Orthogonal matrixes may have determinant -1 or 1
# DCMs are special orthogonal matrices, with determinant 1
improper = FALSE
DCM_not_1 = FALSE
if(any(is.null(dim(DCM)))) stop('DCM must be a matrix or array.')
if(is.na(dim(DCM)[3])) N =1 else N = dim(DCM)[3]
if (N == 1){
# Computing deviation from orthogonality
delta = DCM %*% t(DCM) - diag(3) # DCM*DCM' - I
delta = matrix(delta,ncol=1) # 9×1 <-- 3×3
# Checking determinant of DCM
DET = det(DCM)
if (DET<0) improper=TRUE
if (ichk && (abs(DET-1)>tol)) DCM_not_1=TRUE
# Permuting DCM
DCM = array( DCM, c(1, 3, 3)) # 1×3×3
} else {
delta <- array(0, dim(DCM))
d=lapply(1:N, function(cntDCM) delta[,,cntDCM] <- t(matrix(DCM[,,cntDCM],3,3) %*% t(matrix(DCM[,,cntDCM],3,3)) - diag(3)))
delta <- unlist(d)
delta <- c(delta)
DET = DCM[1,1,]*DCM[2,2,]*DCM[3,3,] -DCM[1,1,]*DCM[2,3,]*DCM[3,2,]+
DCM[1,2,]*DCM[2,3,]*DCM[3,1,] -DCM[1,2,]*DCM[2,1,]*DCM[3,3,]+
DCM[1,3,]*DCM[2,1,]*DCM[3,2,] -DCM[1,3,]*DCM[2,2,]*DCM[3,1,]
DET = array(DET,c(1,1,N)) # 1×1×N
if (any(DET<0)) improper=TRUE
if (ichk && (any(abs(DET-1)>tol))) DCM_not_1=TRUE
DCMtmp<-lapply(1:N, function(n) t(DCM[,n,]) ) # it works! but must be turned into an array?
DCMtmp<-array(unlist(DCMtmp), dim = c(dim(DCMtmp[[1]]), length(DCMtmp)))
DCM <- DCMtmp
}
# Issuing error messages or warnings
if (!ignoreAllChk) if (ichk && any(abs(delta)>tol)) warning('Warning: Input DCM is not orthogonal.')
if (!ignoreAllChk) if (improper) stop('Improper input DCM')
if (!ignoreAllChk) if (DCM_not_1) warning('Warning: Input DCM determinant off from 1 by more than tolerance.')
# Denominators for 4 distinct types of equivalent Q equations
denom = cbind(1 + DCM[,1,1] - DCM[,2,2] - DCM[,3,3], 1 - DCM[,1,1] + DCM[,2,2] - DCM[,3,3],
1 - DCM[,1,1] - DCM[,2,2] + DCM[,3,3], 1 + DCM[,1,1] + DCM[,2,2] + DCM[,3,3])
#denom[which(is.na(denom))]<-0
denom[which(denom<0)]<-0
denom = 2 * sqrt(denom) # N×4
# Choosing for each DCM the equation which uses largest denominator
maxdenom <- apply(denom,1,max)
index <- apply(denom,1,function(x) which(x==max(x)))
if(is.null(dim(maxdenom))) maxdenom <- matrix(maxdenom,ncol=1)
Q = matrix(NA,N,4) # N×4
if (N==1){
ii=1
if (index==1) Q <- (cbind( (DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom, 0.25 * maxdenom, ( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom,( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom) )
if (index==2) Q <- (cbind( (DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom,( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom,0.25 * maxdenom,( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom) )
if (index==3) Q <- (cbind( (DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom,( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom,( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom,0.25 * maxdenom) )
if (index==4) Q <- (cbind(0.25 * maxdenom,( DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom,( DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom,( DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom) )
} else {
ii = which(index==1)
if (length(ii) !=0) Q[ii,] = (cbind( (DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom[ii], 0.25 * maxdenom[ii], ( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom[ii],( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom[ii]) )
ii = which(index==2)
if (length(ii) !=0) Q[ii,] = (cbind( (DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom[ii],( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom[ii],0.25 * maxdenom[ii],( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom[ii]) )
ii = which(index==3)
if (length(ii) !=0) Q[ii,] = (cbind( (DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom[ii],( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom[ii],( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom[ii],0.25 * maxdenom[ii]) )
ii = which(index==4)
if (length(ii) !=0) Q[ii,] = (cbind(0.25 * maxdenom[ii],( DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom[ii],( DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom[ii],( DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom[ii]) )
}
Q
}
Q2EA <- function(Q, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{
# Implementation of quaternion to Euler angles based on D. M. Henderson 1977
# Shuttle Program. Euler Angles, Quaternions, and Transformation Matrices Working Relationships.
# National Aeronautics and Space Administration (NASA), N77-31234/6
# Q - [q1,q2,q3,q4] to EA - [psi,theta,phi]
# Madgwick (zyx) originaly used Q = [phi, theta, psi]
# Jose Gama 2014
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if(is.null(dim(Q))) N <- 1 else N <- dim(Q)[1]
Q<-matrix(Q,N,4)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
if (EulerOrder=='zyx') {
EA <- cbind(atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)), atan2(-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),sqrt(1-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3]))^2)),atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)))
}
if (EulerOrder=='yxz') {
EA <- cbind(atan2(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]), 1-2*(Q[,2]^2 + Q[,3]^2)), atan2(-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),sqrt(1-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2]))^2)),atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)))
}
if (EulerOrder=='xzy') {
EA <- cbind( - atan2(-(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)), atan2(-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),sqrt(1-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4]))^2)),atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)))
}
if (EulerOrder=='zxy') {
EA <- cbind(atan2(-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)), atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),sqrt(1-(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2]))^2)),atan2(-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)))
}
if (EulerOrder=='yzx') {
EA <- cbind(atan2(-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)), atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),sqrt(1-(2*(Q[,2]*Q[,3] + Q[,1]*Q[,4]))^2)),atan2(-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)))
}
if (EulerOrder=='xyz') {
EA <- cbind(atan2(-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)), atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),sqrt(1-(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]))^2)),atan2(-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)))
}
if (EulerOrder=='zyz') {
EA <- cbind(atan2((2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)),atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3]))))
}
if (EulerOrder=='zxz') {
EA <- cbind(atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)),atan2((2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2]))))
}
if (EulerOrder=='yxy') {
EA <- cbind(atan2((2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)),atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2]))))
}
if (EulerOrder=='yzy') {
EA <- cbind(atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)),atan2((2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(2*(Q[,2]*Q[,3] + Q[,1]*Q[,4]))))
}
if (EulerOrder=='xzx') {
EA <- cbind(atan2((2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(2*(Q[,2]*Q[,3] + Q[,1]*Q[,4]))), atan2(sqrt(1-(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)^2),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)),atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4]))))
}
if (EulerOrder=='xyx') {
EA <- cbind(atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3]))), atan2(sqrt(1-(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)^2),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)),atan2((2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]))))
}
#EA = EA * (180/pi) # (N×3) Euler angles in degrees
theta = EA[,2] # (N×1) Angle THETA in degrees
# Check EA
if (!ignoreAllChk) if (any(is.complex( EA ))) stop('Unreal\nUnreal Euler EA. Input resides too close to singularity.\nPlease choose different EA type.')
# Type 1 rotation (rotations about three distinct axes)
# THETA is computed using ASIN and ranges from -90 to 90 degrees
if (!ignoreAllChk) {
if (substr(EulerOrder,1,1) != substr(EulerOrder,3,3)){
singularities = abs(theta) > 89.9*(pi/180) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)) {
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 1 Euler singularity.\n',
'Type 1 Euler singularity occurs when second angle is -90 or 90 degrees.\n',
'Please choose different EA type.',sep=''))
}
} else {
# Type 2 rotation (1st and 3rd rotation about same axis)
# THETA is computed using ACOS and ranges from 0 to 180 degrees
singularities = (theta<0.1*(pi/180)) | (theta>179.9*(pi/180)) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)){
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 2 Euler singularity.\n',
'Type 2 Euler singularity occurs when second angle is 0 or 180 degrees.\n',
'Please choose different EA type.',sep=''))
}
}
}
EA
}
Q2EA.Xiao <- function(Q, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{
# Implementation of quaternion to Euler angles based on:
# J. Xiao, 2013. Princeton Vision Toolkit. Available from: <http://vision.princeton.edu/code.html>
# http://vision.princeton.edu/pvt/GCBreader/quaternion.m
# Q - [q1,q2,q3,q4] to EA - [psi,theta,phi]
# EA in radians
# Madgwick (zyx) originaly used Q = [phi, theta, psi]
# Converted by Jose Gama 2014
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if(is.null(dim(Q))) Q<-matrix(Q,1,4)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
Q <- Qnormalize(Q)
if (EulerOrder=='zyx') {
EA <- cbind(atan2(2*(Q[4]*Q[1]- Q[2]*Q[3]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)),
asin(2*(Q[2]*Q[4]+ Q[3]*Q[1])),
atan2(2*(Q[2]*Q[1]- Q[3]*Q[4]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)))
}
if (EulerOrder=='yxz') {
EA <- cbind( atan2(2*(Q[3]*Q[1]- Q[4]*Q[2]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)),
asin(2*(Q[2]*Q[1]+ Q[3]*Q[4])),
atan2(2*(Q[4]*Q[1]- Q[2]*Q[3]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='xzy') {
EA <- cbind( atan2(2*(Q[2]*Q[1]- Q[4]*Q[3]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)),
asin(2*(Q[2]*Q[3]+ Q[4]*Q[1])),
atan2(2*(Q[3]*Q[1]- Q[2]*Q[4]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='zxy') {
EA <- cbind( atan2(2*(Q[2]*Q[3]+ Q[4]*Q[1]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)),
asin(2*(Q[2]*Q[1]- Q[3]*Q[4])),
atan2(2*(Q[2]*Q[4]+ Q[3]*Q[1]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)))
}
if (EulerOrder=='yzx') {
EA <- cbind( atan2(2*(Q[2]*Q[4]+ Q[3]*Q[1]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)),
asin(2*(Q[4]*Q[1]- Q[2]*Q[3])),
atan2(2*(Q[2]*Q[1]+ Q[3]*Q[4]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='xyz') {
EA <- cbind( atan2(2*(Q[2]*Q[1]+ Q[4]*Q[3]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)),
asin(2*(Q[3]*Q[1]- Q[2]*Q[4])),
atan2(2*(Q[2]*Q[3]+ Q[4]*Q[1]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='zyz') {
EA <- cbind( atan2((Q[2]*Q[1]+ Q[3]*Q[4]),(Q[3]*Q[1]- Q[2]*Q[4])),
acos(Q[1]^2-Q[2]^2- Q[3]^2+Q[4]^2),
atan2((Q[3]*Q[4]- Q[2]*Q[1]),(Q[2]*Q[4]+ Q[3]*Q[1])))
}
if (EulerOrder=='zxz') {
EA <- cbind( atan2((Q[2]*Q[4]- Q[3]*Q[1]),(Q[2]*Q[1]+ Q[3]*Q[4])),
acos(Q[1]^2-Q[2]^2- Q[3]^2+Q[4]^2),
atan2((Q[2]*Q[4]+ Q[3]*Q[1]),(Q[2]*Q[1]- Q[3]*Q[4])))
}
if (EulerOrder=='yxy') {
EA <- cbind( atan2((Q[2]*Q[3]+ Q[4]*Q[1]),(Q[2]*Q[1]- Q[3]*Q[4])),
acos(Q[1]^2-Q[2]^2+ Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[3]- Q[4]*Q[1]),(Q[2]*Q[1]+ Q[3]*Q[4])))
}
if (EulerOrder=='yzy') {
EA <- cbind( atan2((Q[3]*Q[4]- Q[2]*Q[1]),(Q[2]*Q[3]+ Q[4]*Q[1])),
acos(Q[1]^2-Q[2]^2+ Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[1]+ Q[3]*Q[4]),(Q[4]*Q[1]- Q[2]*Q[3])))
}
if (EulerOrder=='xzx') {
EA <- cbind( atan2((Q[2]*Q[4]+ Q[3]*Q[1]),(Q[4]*Q[1]- Q[2]*Q[3])),
acos(Q[1]^2+Q[2]^2- Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[4]- Q[3]*Q[1]),(Q[2]*Q[3]+ Q[4]*Q[1])))
}
if (EulerOrder=='xyx') {
EA <- cbind( atan2((Q[2]*Q[3]- Q[4]*Q[1]),(Q[2]*Q[4]+ Q[3]*Q[1])),
acos(Q[1]^2+Q[2]^2- Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[3]+ Q[4]*Q[1]),(Q[3]*Q[1]- Q[2]*Q[4])))
}
#EA = - EA # *(180/pi) (N×3) Euler angles in radians
theta = EA[,2] # (N×1) Angle THETA in radians
# Check EA
if (!ignoreAllChk) if (any(is.complex( EA ))) stop('Unreal\nUnreal Euler EA. Input resides too close to singularity.\nPlease choose different EA type.')
# Type 1 rotation (rotations about three distinct axes)
# THETA is computed using ASIN and ranges from -90 to 90 degrees
if (!ignoreAllChk) {
if (substr(EulerOrder,1,1) != substr(EulerOrder,3,3)){
singularities = abs(theta) > 89.9 *(pi/180) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)) {
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 1 Euler singularity.\n',
'Type 1 Euler singularity occurs when second angle is -90 or 90 degrees.\n',
'Please choose different EA type.',sep=''))
}
} else {
# Type 2 rotation (1st and 3rd rotation about same axis)
# THETA is computed using ACOS and ranges from 0 to 180 degrees
singularities = (theta < 0.1*(pi/180)) | (theta > 179.9*(pi/180)) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)){
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 2 Euler singularity.\n',
'Type 2 Euler singularity occurs when second angle is 0 or 180 degrees.\n',
'Please choose different EA type.',sep=''))
}
}
}
EA
}
Q2EV <- function(Q,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# Q - [q1,q2,q3,q4] to EV - [m1,m2,m3,MU]
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
# Normalize quaternion(s) in case of deviation from unity.
# User has already been warned of deviation.
v_length=4; isnot_DCM=TRUE;N=dim(Q)[1]
#Qnorms = sqrt(apply(Q^2, 2,sum))
# Angle MU in radians and sine of MU/2
Q2 <-matrix(Q[,1:3],ncol=3,byrow=FALSE)
halfMUrad = matrix(atan2( sqrt(apply(Q2^2,1,sum)), Q[,4] ),ncol=1) # N×1
#print(halfMUrad)
SIN = sin(halfMUrad) # N×1
index = which(SIN==0) # [N×1] Logical index
if (length(index)>0){
# Initializing
EV = matrix(0,N,4)
# Singular cases [MU is zero degrees]
EV[index, 1] = 1
# Non-singular cases
SIN = SIN[-index, 1]
EV[-index, ] = cbind(Q[-index,1] / SIN,
Q[-index,2] / SIN,
Q[-index,3] / SIN,
halfMUrad * 2 )#* (360/pi)
} else {
# Non-singular cases
EV = cbind(Q[,1]/SIN, Q[,2]/SIN, Q[,3]/SIN, halfMUrad * 2)#*(360/pi)
}
# MU greater than 180 degrees
index = which(EV[,4] > pi/2) # [N×1] Logical index
EV[index, ] = cbind(-matrix(EV[index,1:3],ncol=3,byrow=FALSE), 2*pi-EV[index,4])
EV
}
Q2DCM <- function(Q,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# Q - [q1,q2,q3,q4] to DCM - 3x3xN
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
# Normalize quaternion(s) in case of deviation from unity.
Qn <- Qnormalize(Q)
# User has already been warned of deviation.
N<-dim(Qn)[1]
Qn = array(t(Qn), c(1, 4, N))
DCM= array(0, c(3, 3, N))
DCM[1,1,] = 1-2*(Qn[1,3,]*Qn[1,3,]+Qn[1,4,]*Qn[1,4,])
DCM[2,1,] = 2*(Qn[1,2,]*Qn[1,3,]-Qn[1,1,]*Qn[1,4,])
DCM[3,1,] = 2*(Qn[1,2,]*Qn[1,4,]+Qn[1,1,]*Qn[1,3,])
DCM[1,2,] = 2*(Qn[1,2,]*Qn[1,3,]+Qn[1,1,]*Qn[1,4,])
DCM[2,2,] = 1-2*(Qn[1,2,]*Qn[1,2,]+Qn[1,4,]*Qn[1,4,])
DCM[3,2,] = 2*(Qn[1,3,]*Qn[1,4,]-Qn[1,1,]*Qn[1,2,])
DCM[1,3,] = 2*(Qn[1,2,]*Qn[1,4,]-Qn[1,1,]*Qn[1,3,])
DCM[2,3,] = 2*(Qn[1,3,]*Qn[1,4,]+Qn[1,1,]*Qn[1,2,])
DCM[3,3,] = 1-2*(Qn[1,2,]*Qn[1,2,]+Qn[1,3,]*Qn[1,3,])
if (length(dim(DCM))==3) if(all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
DCM
}
# Q2DCM <- function(Q,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
# {# Q - [q1,q2,q3,q4] to DCM - 3x3xN
# if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
# if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
# # Normalize quaternion(s) in case of deviation from unity.
# # User has already been warned of deviation.
# N<-dim(Q)[1]
# Q = array(t(Q), c(1, 4, N))
# SQ = Q^2
# DCM= array(0, c(3, 3, N))
# DCM[1,1,] = SQ[1,1,]-SQ[1,2,]-SQ[1,3,]+SQ[1,4,]
# DCM[1,2,] = 2*(Q[1,1,]*Q[1,2,] +Q[1,3,]*Q[1,4,])
# DCM[1,3,] = 2*(Q[1,1,]*Q[1,3,] -Q[1,2,]*Q[1,4,])
# DCM[2,1,] = 2*(Q[1,1,]*Q[1,2,] -Q[1,3,]*Q[1,4,])
# DCM[2,2,] = -SQ[1,1,]+SQ[1,2,]-SQ[1,3,]+SQ[1,4,]
# DCM[2,3,] = 2*(Q[1,2,]*Q[1,3,] +Q[1,1,]*Q[1,4,])
# DCM[3,1,] = 2*(Q[1,1,]*Q[1,3,] +Q[1,2,]*Q[1,4,])
# DCM[3,2,] = 2*(Q[1,2,]*Q[1,3,] -Q[1,1,]*Q[1,4,])
# DCM[3,3,] = -SQ[1,1,]-SQ[1,2,]+SQ[1,3,]+SQ[1,4,]
# if (all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
# DCM
# }
Q2GL<-function(Q)
{# Q - [q1,q2,q3,q4] to OpenGL translation matrix 4x4xn
#based on
#http://www.tinkerforge.com/doc/Software/Bricks/IMU_Brick_Python.html
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
GL <- lapply(1:N, function(n)
{
x<-Q[n,1]
y<-Q[n,2]
w<-Q[n,3]
z<-Q[n,4]
tmp <- c(1 - 2*(y*y + z*z), 2*(x*y - w*z), 2*(x*z + w*y), 0,
2*(x*y + w*z), 1 - 2*(x*x + z*z), 2*(y*z - w*x), 0,
2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x*x + y*y), 0,
0, 0, 0, 1)
matrix(tmp,nrow=4, ncol=4)
})
if (N==1) GL=array(unlist(GL), dim = c(dim(GL[[1]]) )) else GL=array(unlist(GL), dim = c(dim(GL[[1]]), length(GL)))
GL
}
EV2EA<-function(EV, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EV - [m1,m2,m3,MU] to EA - [psi,theta,phi]
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EV)) EV <- matrix(unlist(EV),ncol=4,byrow=FALSE)
Q<-EV2Q(EV, tol, ichk, ignoreAllChk)
EA<-Q2EA(Q, EulerOrder, tol, ichk, ignoreAllChk)
EA
}
EA2EV<-function(EA, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EA - [psi,theta,phi] to EV - [m1,m2,m3,MU]
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EA)) EA <- matrix(unlist(EA),ncol=3,byrow=FALSE)
Q<-EA2Q(EA, EulerOrder, ichk, ignoreAllChk)
EV<-Q2EV(Q, tol, ichk, ignoreAllChk)
EV
}
EA2EA<-function(EA, EulerOrder1='zyx',EulerOrder2='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{#EA - [psi,theta,phi] to EA - [psi,theta,phi]
if (!is.character(EulerOrder1)) stop('<<EulerOrder1>> must be a string.')
EulerOrder1 <- tolower(EulerOrder1)
if (!(EulerOrder1 %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.character(EulerOrder2)) stop('<<EulerOrder2>> must be a string.')
EulerOrder2 <- tolower(EulerOrder2)
if (!(EulerOrder2 %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (all(EulerOrder1==EulerOrder2)) return (EA)
Q<- EA2Q(EA, EulerOrder1, ichk, ignoreAllChk)
EA<-Q2EA(Q, EulerOrder2, tol, ichk, ignoreAllChk)
EA
}
EA2DCM<-function(EA, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EA - [psi,theta,phi] to DCM - 3x3xN
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EA)) EA <- matrix(unlist(EA),ncol=3,byrow=FALSE)
Q<-EA2Q(EA, EulerOrder, ichk, ignoreAllChk)
DCM<-Q2DCM(Q, tol, ichk, ignoreAllChk)
if (length(DCM)==3) if (all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
DCM
}
EV2DCM<-function(EV, tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EV - [m1,m2,m3,MU] to DCM - 3x3xN
if (!is.matrix(EV)) EV <- matrix(unlist(EV),ncol=4,byrow=FALSE)
Q<-EV2Q(EV, tol, ichk, ignoreAllChk)
DCM<-Q2DCM(Q, tol, ichk, ignoreAllChk)
if (length(DCM)==3) if (all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
DCM
}
DCM2EV<-function(DCM, tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# DCM - 3x3xN to EV - [m1,m2,m3,MU]
Q<-DCM2Q(DCM, tol, ichk, ignoreAllChk)
EV<-Q2EV(Q, tol, ichk, ignoreAllChk)
EV
}
DCM2EA<-function(DCM, EulerOrder='zyx', tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# DCM - 3x3xN to EA - [psi,theta,phi]
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
Q<-DCM2Q(DCM, tol, ichk, ignoreAllChk)
EA<-Q2EA(Q, EulerOrder, tol, ichk, ignoreAllChk)
EA
}
vectQrot<-function( Q, rr )
# Rotate a vector by a quaternion
{
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
if (!is.matrix(rr)) rr <-matrix(rr,ncol=3,byrow=FALSE)
N <- dim(Q)[1]
if (!is.matrix(rr)) rr <-matrix( rr,ncol=3,nrow=N,byrow=FALSE)
Qr <- (Qconj(Q) %Q*% cbind(0, rr)) %Q*% Q
Qr[,2:4]
}
Qrot <- function(Q,w,dT)
# Updates current attitude quaternion q
# output - current attitude quaternion
# input - wx, wy, wz - angular rate values
# input - dT - inverse of update rate
{
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (!is.matrix(w)) w <-matrix(w,ncol=3,byrow=FALSE)#nrow=N,
Qr <- matrix(0,nrow=N, ncol=4)
Qr<-lapply(1:N, function(n) {
Fx <- w[n,1]*dT
Fy <- w[n,2]*dT
Fz <- w[n,3]*dT
Fm <- sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
sinFm2 <- sin(Fm/2)
cosFm2 <- cos(Fm/2)
if (Fm != 0)
Qr[n,]<-c(cosFm2, Fx/Fm*sinFm2, Fy/Fm*sinFm2, Fz/Fm*sinFm2)#Qr[n,]
else
Qr[n,]<-c(1, 0, 0, 0)#Qr[n,]
Qr[n,]<- Q[n,] %Q*% Qr[n,]#Qr[n,]<- Q[n,] %Q*% Qr[n,]
Qr
})
if (N==1) Qr=array(unlist(Qr), dim = c(dim(Qr[[1]]) )) else Qr=array(unlist(Qr), dim = c(dim(Qr[[1]]), length(Qr)))
if (length(dim(Qr))==3) Qr <- array(Qr,c(N,4))
#if (length(dim(Qr))==3) if (all(dim(Qr)==c(1,4,1))) Qr <- array(Qr,c(1,4))
Qr
}
"%Q+%" <- function(Q1, Q2)
{# quaternion sum
if (!is.matrix(Q1)) Q1 <-matrix(Q1,ncol=4,byrow=FALSE)
if (!is.matrix(Q2)) Q2 <-matrix(Q2,ncol=4,byrow=FALSE)
Q1 + Q2
}
"%Q-%" <- function(Q1, Q2)
{# quaternion difference
if (!is.matrix(Q1)) Q1 <-matrix(Q1,ncol=4,byrow=FALSE)
if (!is.matrix(Q2)) Q2 <-matrix(Q2,ncol=4,byrow=FALSE)
Q1 - Q2
}
Qconj<-function(Q)
{# quaternion conjugate
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
Q[,2:4] <- -Q[,2:4]
Q
}
Qinv<-function(Q)
{# quaternion inverse
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (Qnorm(Q)==1) return (Qconj(Q))
else return (Qconj(Q)/Qnorm(Q))
}
"%Q*%" <- function(Q1, Q2)
{# quaternion product
if (!is.matrix(Q1)) Q1 <-matrix(Q1,ncol=4,byrow=FALSE)
if (!is.matrix(Q2)) Q2 <-matrix(Q2,ncol=4,byrow=FALSE)
N <- dim(Q1)[1]
ab<-matrix(0,ncol=4,nrow=N)
ab<-lapply(1:N, function(n) {
ab[n,1]<-Q1[n,1]*Q2[n,1]-Q1[n,2]*Q2[n,2]-Q1[n,3]*Q2[n,3]-Q1[n,4]*Q2[n,4]
ab[n,2]<-Q1[n,1]*Q2[n,2]+Q1[n,2]*Q2[n,1]+Q1[n,3]*Q2[n,4]-Q1[n,4]*Q2[n,3]
ab[n,3]<-Q1[n,1]*Q2[n,3]-Q1[n,2]*Q2[n,4]+Q1[n,3]*Q2[n,1]+Q1[n,4]*Q2[n,2]
ab[n,4]<-Q1[n,1]*Q2[n,4]+Q1[n,2]*Q2[n,3]-Q1[n,3]*Q2[n,2]+Q1[n,4]*Q2[n,1]
ab
} )
if (N==1) ab=array(unlist(ab), dim = c(dim(ab[[1]]) )) else ab=array(unlist(ab), dim = c(dim(ab[[1]]), length(ab)))
ab<-matrix(ab,ncol=4,nrow=N)
return(ab)
}
"%Q/%" <- function(Q1, Q2)
{# quaternion division
Q1 %Q*% Qinv(Q2)
}
Qnorm<-function(Q)
{ # norm of a quaternion
if (length(Q)==3) Q<-c(0,Q)
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (N==1) sqrt(sum(Q^2))
apply(Q,1,function(n) sqrt(sum(n^2)))
}
Qnormalize<-function(Q)
{ # normalize a quaternion
if (length(Q)==3) Q<-c(0,Q)
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (N==1) Qn <- Q/sqrt(sum(Q^2)) else Qn <- Q/apply(Q,1,function(n) sqrt(sum(n^2)))
Qn[which(is.na(Qn))] <- 0
Qn
}
#############################################################################################
isUnitQuaternion<-function(Q)
{# returns TRUE if q is a unit quaternion
# |q|==1 => q is a unit quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
qN <- Qnorm(Q)
qN == 1
}
isPureQuaternion<-function(Q)
{# returns TRUE if q is a pure quaternion
# q==(0,v) => q is a pure quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
(Q[,2] == 0) & (Q[,3] == 0) & (Q[,4] == 0)
}
isRealQuaternion<-function(Q)
{# returns TRUE if q is a real quaternion
# q==(0,v) => q is a pure quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
Q[,1] == 0
}
Qlog<-function(Q)
{# returns the logarithm of a quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
a <- acos(Q[,1])
sina <- sin(a)
Qret <- matrix(0,nrow=dim(Q)[1],4)
sinaZero <- which(sina>0)
Qret[sinaZero,2:4] <- a[sinaZero] * Q[sinaZero,2:4] / sina[sinaZero]
Qret
}
Qexp<-function(Q)
{# returns the exponential of a quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
a <- exp(Q[,1])
sina <- sin(a)
cosa <- cos(a)
Qret <- cbind(cosa,0,0,0)
sinaZero <- which(sina>0)
Qret[sinaZero,2:4] <- sina[sinaZero] * Q[sinaZero,2:4] / a[sinaZero]
Qret
}
"%Q.%" <- function(Q1, Q2)
{# quaternion dot product
apply(Q1 * Q2,1,sum)
}
Qzero <- function(n=NA){
# generate zero-value quaternions
if (is.na(n)) n <- 1
matrix(0,n,4)
}
Qone <- function(n=NA){
# generate one-value quaternions
if (is.na(n)) n <- 1
matrix(c(1,0,0,0),n,4)
}
Qlerp<-function(Q1, Q2, fracT)
{# linear quaternion interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
N <- dim(Q1)[1]
if (fracT == 0.0) return (Qzero(N))
if (fracT == 1.0) return (Qone(N))
Qnormalize((Q1*(1-fracT) %Q+% Q2*fracT))
}
Qslerp<-function(Q1, Q2, fracT)
{# spherical linear interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
N <- dim(Q1)[1]
if (fracT == 0.0) return (Qzero(N))
if (fracT == 1.0) return (Qone(N))
Q3 <- Q2
Qd <- Q1 %Q.% Q2
QdLtz <- which(Qd < 0)
Qd[QdLtz] <- -Qd[QdLtz]
Q3[QdLtz] <- -Q2[QdLtz]
QdLt.95 <- which(Qd < 0.95)
angleQ = acos(Qd[QdLt.95])
Q3[QdLt.95] <- (Q1[QdLt.95] * sin(angleQ * (1-fracT)) + Q3[QdLt.95] * sin(angleQ * fracT))/sin(angleQ)
Q3[-QdLt.95] <- Qlerp(Q1[QdLt.95], Q3[QdLt.95],fracT)
Q3
}
QslerpNoInvert<-function(Q1, Q2, fracT)
{# This version of slerp, used by squad, does not check for theta > 90.
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
N <- dim(Q1)[1]
if (fracT == 0.0) return (Qzero(N))
if (fracT == 1.0) return (Qone(N))
Q3 <- Q2
Qd <- Q1 %Q.% Q2
QdLt.bt95 <- which((Qd < 0.95) & (Qd > -0.95))
angleQ = acos(Qd[QdLt.bt95])
Q3[QdLt.bt95] <- (Q1[QdLt.bt95] * sin(angleQ * (1-fracT)) + Q2[QdLt.bt95] * sin(angleQ * fracT))/sin(angleQ)
Q3[-QdLt.bt95] <- Qlerp(Q1[QdLt.bt95], Q2[QdLt.bt95],fracT)
Q3
}
Qsquad<-function(Q1, Q2, Qa, Qb, fracT)
{# Spherical and Quadrangle linear interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Qc <- QslerpNoInvert(Q1,Q2,fracT)
Qd <- QslerpNoInvert(Qa,Qb,fracT)
QslerpNoInvert(Qc,Qd,2*fracT*(1-fracT))
}
Qbezier<-function(Q1, Q2, Qa, Qb, fracT)
{# Shoemake-Bezier interpolation using De Castlejau algorithm
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Q11 <- QslerpNoInvert(Q1,Qa,fracT)
Q12 <- QslerpNoInvert(Qa,Qb,fracT)
Q13 <- QslerpNoInvert(Qb,Q2,fracT)
QslerpNoInvert(QslerpNoInvert(Q11,Q12,fracT), QslerpNoInvert(Q12,Q13,fracT), fracT)
}
Qspline<-function(Qnm1, Qn, Qnp1)
{# Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Qni <- Qn
Qni[,2:4] <- -Qni[,2:4]
Qn %Q*% Qexp(( Qlog(Qni %Q*% Qnm1) %Q+% Qlog(Qni %Q*% Qnp1) ) / -4)
}
QangularDifference<-function(Q1, Q2)
{#angular difference between 2 quaternions
acos((Q1 %Q.% Q2)/(Qnorm(Q1)*Qnorm(Q2)))
}
Qrandom <- function(n=NA)
{# generate - uniform random unit quaternions
#
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
# LaValle, Steven M. "Planning algorithms." University of Illinois 2004 (1999).
# pp. 164 - "uniform random unit quaternions leads to uniform random samples over SO(3)"
if (is.na(n)) n <- 1
R <- matrix(stats::runif(n*3),n,3)
r1 <- sqrt(1.0 - R[,1])
r2 <- sqrt(R[,1])
pi2 <- pi * 2.0
t1 <- pi2 * R[,2]
t2 <- pi2 * R[,3]
Q <- cbind(cos(t2)*r2, sin(t1)*r1,cos(t1)*r1, sin(t2)*r2)
Qnormalize(Q)
}
isPureRotationMatrix<-function(DCM, tol=0.01)
{# pure rotation matrix = proper orthogonal matrix = det(m)==1
if (is.matrix(DCM)) return(((det(DCM)-1)<tol) & (det(DCM)>0)) else {
apply(DCM,3,function(x) (((det(x)-1)<tol) & (det(x)>0)) )
}
}
DCMrandom <- function(n=NA, tol = 10 * .Machine$double.eps, ignoreAllChk=FALSE)
{# generate - uniform random DCM
if (is.na(n)) n <- 1
Q <- Qrandom(n)
DCM <- Q2DCM(Q, tol = tol, ignoreAllChk=TRUE)
if (ignoreAllChk) return(DCM)
badQ <- which(isPureRotationMatrix(DCM)==FALSE)
while (length(badQ)>0) {# remove wrong matrices
n <- length(badQ)
Q <- Qrandom(n)
DCM[badQ] <- Q2DCM(Q, tol = tol, ignoreAllChk=TRUE)
badQ <- which(isPureRotationMatrix(DCM)==FALSE)
}
DCM
}
EArandom <- function(n=NA, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ignoreAllChk=FALSE)
{# generate - uniform random Euler Angles
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (is.na(n)) n <- 1
Q <- Qrandom(n)
EA <- Q2EA(Q, EulerOrder, tol = tol, ignoreAllChk=TRUE)
if (ignoreAllChk) return(EA)
badQ <- which((abs(EA[,2]) > 89.9 *(pi/180)) & ((EA[,2] < 0.1*(pi/180)) | (EA[,2] > 179.9*(pi/180))))
while (length(badQ)>0) {# fix singularities
n <- length(badQ)
Q <- Qrandom(n)
EA[badQ] <- Q2EA(Q, EulerOrder, tol = tol, ignoreAllChk=TRUE)
badQ <- which((abs(EA[,2]) > 89.9 *(pi/180)) & ((EA[,2] < 0.1*(pi/180)) | (EA[,2] > 179.9*(pi/180))))
}
EA
}
EVrandom <- function(n=NA, tol = 10 * .Machine$double.eps, ignoreAllChk=FALSE)
{# generate - uniform random Euler Vectors
if (is.na(n)) n <- 1
Q <- Qrandom(n)
EV <- Q2EV(Q, tol = tol, ignoreAllChk=TRUE)
if (ignoreAllChk) return(EV)
EV
}
Try the RSpincalc package in your browser
Any scripts or data that you put into this service are public.
RSpincalc documentation built on May 2, 2019, 10:59 a.m.
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
# Q Rotation Quaternions Q - [q1,q2,q3,q4] (Nx4)
# EV Euler Vector and rotation angle (degrees) EV - [m1,m2,m3,MU] (Nx4)
# DCM Orthogonal DCM Rotation Matrix DCM - 3x3xN
# EA Euler angles (12 possible sets) (degrees) EA - [psi,theta,phi] (Nx3)
# DCM2EA DCM2EV DCM2Q
# EA2DCM EA2EV EA2Q EA2EA
# EV2DCM EV2EA EV2Q
# Q2DCM Q2EA Q2EV Q2GL
EA2Q<-function(EA, EulerOrder='zyx', ichk=FALSE, ignoreAllChk=FALSE)
{# EA - [psi,theta,phi] yaw, pitch, roll to EV - [m1,m2,m3,MU]
# EA in radians
# ichk = FALSE disables near-singularity warnings.
# Identify singularities (2nd Euler angle out of range)
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EA)) EA <- matrix(EA,ncol=3,byrow=FALSE)
theta = EA[, 2] # N×1
if (!ignoreAllChk){
if (substr(EulerOrder,1,1) != substr(EulerOrder,3,3)) {# Type 1 rotation about three distinct axes
# Type 1 rotation (rotations about three distinct axes)
if (any(abs(theta) >= pi/2 )) { stop('Second input Euler angle(s) outside -90 to 90 degree range')
} else if (ichk && any(abs(theta)>88 * (pi/180))) warning('Warning: Second input Euler angle(s) near a singularity (-90 or 90 degrees).')
} else {
# Type 2 rotation (1st and 3rd rotation about same axis)
if (any((theta<=0) | (theta >= pi))) { stop('Second input Euler angle(s) outside 0 to 180 degree range')
} else if (ichk && (any(theta < 2 * (pi/180)) | (theta>178 * (pi/180)))) warning('Warning: Second input Euler angle(s) near a singularity (0 or 180 degrees).')
}}
# Half angles in radians
HALF = EA / 2# * (pi/360) # N×3
Hpsi = matrix(HALF[,1],ncol=1) # N×1
Htheta = matrix(HALF[,2],ncol=1) # N×1
Hphi = matrix(HALF[,3],ncol=1) # N×1
# Pre-calculate cosines and sines of the half-angles for conversion.
c1=cos(Hpsi); c2=cos(Htheta); c3=cos(Hphi)
s1=sin(Hpsi); s2=sin(Htheta); s3=sin(Hphi)
c13 =cos(Hpsi+Hphi); s13 =sin(Hpsi+Hphi)
c1_3=cos(Hpsi-Hphi); s1_3=sin(Hpsi-Hphi)
c3_1=cos(Hphi-Hpsi); s3_1=sin(Hphi-Hpsi)
if (EulerOrder=='xyx') Q=cbind(c2*c13, c2*s13,s2*c1_3, s2*s1_3) else
if (EulerOrder=='yzy') Q=cbind(c2*c13, s2*s1_3,c2*s13, s2*c1_3) else
if (EulerOrder=='zxz') Q=cbind(c2*c13, s2*c1_3,s2*s1_3, c2*s13) else
if (EulerOrder=='xzx') Q=cbind(c2*c13, c2*s13,s2*s3_1, s2*c3_1) else
if (EulerOrder=='yxy') Q=cbind(c2*c13, s2*c3_1,c2*s13, s2*s3_1) else
if (EulerOrder=='zyz') Q=cbind(c2*c13, s2*s3_1,s2*c3_1, c2*s13) else
if (EulerOrder=='xyz') Q=cbind(c1*c2*c3-s1*s2*s3, s1*c2*c3+c1*s2*s3,c1*s2*c3-s1*c2*s3, c1*c2*s3+s1*s2*c3) else
if (EulerOrder=='yzx') Q=cbind(c1*c2*c3-s1*s2*s3, c1*c2*s3+s1*s2*c3,s1*c2*c3+c1*s2*s3, c1*s2*c3-s1*c2*s3) else
if (EulerOrder=='zxy') Q=cbind(c1*c2*c3-s1*s2*s3, c1*s2*c3-s1*c2*s3,c1*c2*s3+s1*s2*c3, s1*c2*c3+c1*s2*s3) else
if (EulerOrder=='xzy') Q=cbind(c1*c2*c3+s1*s2*s3, s1*c2*c3-c1*s2*s3,c1*c2*s3-s1*s2*c3, c1*s2*c3+s1*c2*s3) else
if (EulerOrder=='yxz') Q=cbind(c1*c2*c3+s1*s2*s3, c1*s2*c3+s1*c2*s3,s1*c2*c3-c1*s2*s3, c1*c2*s3-s1*s2*c3) else
if (EulerOrder=='zyx') Q=cbind(c1*c2*c3+s1*s2*s3, c1*c2*s3-s1*s2*c3,c1*s2*c3+s1*c2*s3, s1*c2*c3-c1*s2*s3) else
if (!ignoreAllChk) stop('Invalid input Euler angle order')
Q
}
EV2Q <- function(EV,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EV - [m1,m2,m3,MU] to Q - [q1,q2,q3,q4]
# Euler vector (EV) and angle MU in radians
if(is.null(dim(EV))) EV<-matrix(EV,ncol=4,byrow=FALSE)
EVtmp = matrix(EV[,1:3],ncol=3,byrow=FALSE) # N×3
halfMU = matrix(EV[,4] / 2,ncol=1) #* (pi/360) (N×1) MU/2 in radians
# Check that input m's constitute unit vector
delta = sqrt(matrix(apply(EVtmp^2,1,sum),ncol=1)) - 1 # N×1
if (!ignoreAllChk) if (any(abs(delta) > tol)) stop('(At least one of the) input Euler vector(s) is not a unit vector')
# Quaternion
SIN = sin(halfMU) # (N×1)
Q = cbind(EVtmp[,2]*SIN, EVtmp[,3]*SIN, cos(halfMU), EVtmp[,1]*SIN)
Q
}
DCM2Q <- function(DCM,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# DCM - 3x3xN to Q - [q1,q2,q3,q4]
# NOTE: Orthogonal matrixes may have determinant -1 or 1
# DCMs are special orthogonal matrices, with determinant 1
improper = FALSE
DCM_not_1 = FALSE
if(any(is.null(dim(DCM)))) stop('DCM must be a matrix or array.')
if(is.na(dim(DCM)[3])) N =1 else N = dim(DCM)[3]
if (N == 1){
# Computing deviation from orthogonality
delta = DCM %*% t(DCM) - diag(3) # DCM*DCM' - I
delta = matrix(delta,ncol=1) # 9×1 <-- 3×3
# Checking determinant of DCM
DET = det(DCM)
if (DET<0) improper=TRUE
if (ichk && (abs(DET-1)>tol)) DCM_not_1=TRUE
# Permuting DCM
DCM = array( DCM, c(1, 3, 3)) # 1×3×3
} else {
delta <- array(0, dim(DCM))
d=lapply(1:N, function(cntDCM) delta[,,cntDCM] <- t(matrix(DCM[,,cntDCM],3,3) %*% t(matrix(DCM[,,cntDCM],3,3)) - diag(3)))
delta <- unlist(d)
delta <- c(delta)
DET = DCM[1,1,]*DCM[2,2,]*DCM[3,3,] -DCM[1,1,]*DCM[2,3,]*DCM[3,2,]+
DCM[1,2,]*DCM[2,3,]*DCM[3,1,] -DCM[1,2,]*DCM[2,1,]*DCM[3,3,]+
DCM[1,3,]*DCM[2,1,]*DCM[3,2,] -DCM[1,3,]*DCM[2,2,]*DCM[3,1,]
DET = array(DET,c(1,1,N)) # 1×1×N
if (any(DET<0)) improper=TRUE
if (ichk && (any(abs(DET-1)>tol))) DCM_not_1=TRUE
DCMtmp<-lapply(1:N, function(n) t(DCM[,n,]) ) # it works! but must be turned into an array?
DCMtmp<-array(unlist(DCMtmp), dim = c(dim(DCMtmp[[1]]), length(DCMtmp)))
DCM <- DCMtmp
}
# Issuing error messages or warnings
if (!ignoreAllChk) if (ichk && any(abs(delta)>tol)) warning('Warning: Input DCM is not orthogonal.')
if (!ignoreAllChk) if (improper) stop('Improper input DCM')
if (!ignoreAllChk) if (DCM_not_1) warning('Warning: Input DCM determinant off from 1 by more than tolerance.')
# Denominators for 4 distinct types of equivalent Q equations
denom = cbind(1 + DCM[,1,1] - DCM[,2,2] - DCM[,3,3], 1 - DCM[,1,1] + DCM[,2,2] - DCM[,3,3],
1 - DCM[,1,1] - DCM[,2,2] + DCM[,3,3], 1 + DCM[,1,1] + DCM[,2,2] + DCM[,3,3])
#denom[which(is.na(denom))]<-0
denom[which(denom<0)]<-0
denom = 2 * sqrt(denom) # N×4
# Choosing for each DCM the equation which uses largest denominator
maxdenom <- apply(denom,1,max)
index <- apply(denom,1,function(x) which(x==max(x)))
if(is.null(dim(maxdenom))) maxdenom <- matrix(maxdenom,ncol=1)
Q = matrix(NA,N,4) # N×4
if (N==1){
ii=1
if (index==1) Q <- (cbind( (DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom, 0.25 * maxdenom, ( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom,( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom) )
if (index==2) Q <- (cbind( (DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom,( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom,0.25 * maxdenom,( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom) )
if (index==3) Q <- (cbind( (DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom,( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom,( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom,0.25 * maxdenom) )
if (index==4) Q <- (cbind(0.25 * maxdenom,( DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom,( DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom,( DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom) )
} else {
ii = which(index==1)
if (length(ii) !=0) Q[ii,] = (cbind( (DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom[ii], 0.25 * maxdenom[ii], ( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom[ii],( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom[ii]) )
ii = which(index==2)
if (length(ii) !=0) Q[ii,] = (cbind( (DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom[ii],( DCM[ii,1,2]+ DCM[ii,2,1]) / maxdenom[ii],0.25 * maxdenom[ii],( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom[ii]) )
ii = which(index==3)
if (length(ii) !=0) Q[ii,] = (cbind( (DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom[ii],( DCM[ii,1,3]+ DCM[ii,3,1]) / maxdenom[ii],( DCM[ii,2,3]+ DCM[ii,3,2]) / maxdenom[ii],0.25 * maxdenom[ii]) )
ii = which(index==4)
if (length(ii) !=0) Q[ii,] = (cbind(0.25 * maxdenom[ii],( DCM[ii,2,3]- DCM[ii,3,2]) / maxdenom[ii],( DCM[ii,3,1]- DCM[ii,1,3]) / maxdenom[ii],( DCM[ii,1,2]- DCM[ii,2,1]) / maxdenom[ii]) )
}
Q
}
Q2EA <- function(Q, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{
# Implementation of quaternion to Euler angles based on D. M. Henderson 1977
# Shuttle Program. Euler Angles, Quaternions, and Transformation Matrices Working Relationships.
# National Aeronautics and Space Administration (NASA), N77-31234/6
# Q - [q1,q2,q3,q4] to EA - [psi,theta,phi]
# Madgwick (zyx) originaly used Q = [phi, theta, psi]
# Jose Gama 2014
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if(is.null(dim(Q))) N <- 1 else N <- dim(Q)[1]
Q<-matrix(Q,N,4)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
if (EulerOrder=='zyx') {
EA <- cbind(atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)), atan2(-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),sqrt(1-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3]))^2)),atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)))
}
if (EulerOrder=='yxz') {
EA <- cbind(atan2(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]), 1-2*(Q[,2]^2 + Q[,3]^2)), atan2(-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),sqrt(1-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2]))^2)),atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)))
}
if (EulerOrder=='xzy') {
EA <- cbind( - atan2(-(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)), atan2(-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),sqrt(1-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4]))^2)),atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)))
}
if (EulerOrder=='zxy') {
EA <- cbind(atan2(-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)), atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),sqrt(1-(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2]))^2)),atan2(-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)))
}
if (EulerOrder=='yzx') {
EA <- cbind(atan2(-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)), atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),sqrt(1-(2*(Q[,2]*Q[,3] + Q[,1]*Q[,4]))^2)),atan2(-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)))
}
if (EulerOrder=='xyz') {
EA <- cbind(atan2(-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)), atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),sqrt(1-(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]))^2)),atan2(-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)))
}
if (EulerOrder=='zyz') {
EA <- cbind(atan2((2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)),atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3]))))
}
if (EulerOrder=='zxz') {
EA <- cbind(atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 - Q[,3]^2 + Q[,4]^2)),atan2((2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2]))))
}
if (EulerOrder=='yxy') {
EA <- cbind(atan2((2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(2*(Q[,3]*Q[,4] + Q[,1]*Q[,2]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)),atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),-(2*(Q[,3]*Q[,4] - Q[,1]*Q[,2]))))
}
if (EulerOrder=='yzy') {
EA <- cbind(atan2((2*(Q[,3]*Q[,4] + Q[,1]*Q[,2])),-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4]))), atan2(sqrt(1-(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)^2),(Q[,1]^2 - Q[,2]^2 + Q[,3]^2- Q[,4]^2)),atan2((2*(Q[,3]*Q[,4] - Q[,1]*Q[,2])),(2*(Q[,2]*Q[,3] + Q[,1]*Q[,4]))))
}
if (EulerOrder=='xzx') {
EA <- cbind(atan2((2*(Q[,2]*Q[,4] - Q[,1]*Q[,3])),(2*(Q[,2]*Q[,3] + Q[,1]*Q[,4]))), atan2(sqrt(1-(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)^2),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)),atan2((2*(Q[,2]*Q[,4] + Q[,1]*Q[,3])),-(2*(Q[,2]*Q[,3] - Q[,1]*Q[,4]))))
}
if (EulerOrder=='xyx') {
EA <- cbind(atan2((2*(Q[,2]*Q[,3] + Q[,1]*Q[,4])),-(2*(Q[,2]*Q[,4] - Q[,1]*Q[,3]))), atan2(sqrt(1-(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)^2),(Q[,1]^2 + Q[,2]^2 - Q[,3]^2 - Q[,4]^2)),atan2((2*(Q[,2]*Q[,3] - Q[,1]*Q[,4])),(2*(Q[,2]*Q[,4] + Q[,1]*Q[,3]))))
}
#EA = EA * (180/pi) # (N×3) Euler angles in degrees
theta = EA[,2] # (N×1) Angle THETA in degrees
# Check EA
if (!ignoreAllChk) if (any(is.complex( EA ))) stop('Unreal\nUnreal Euler EA. Input resides too close to singularity.\nPlease choose different EA type.')
# Type 1 rotation (rotations about three distinct axes)
# THETA is computed using ASIN and ranges from -90 to 90 degrees
if (!ignoreAllChk) {
if (substr(EulerOrder,1,1) != substr(EulerOrder,3,3)){
singularities = abs(theta) > 89.9*(pi/180) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)) {
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 1 Euler singularity.\n',
'Type 1 Euler singularity occurs when second angle is -90 or 90 degrees.\n',
'Please choose different EA type.',sep=''))
}
} else {
# Type 2 rotation (1st and 3rd rotation about same axis)
# THETA is computed using ACOS and ranges from 0 to 180 degrees
singularities = (theta<0.1*(pi/180)) | (theta>179.9*(pi/180)) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)){
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 2 Euler singularity.\n',
'Type 2 Euler singularity occurs when second angle is 0 or 180 degrees.\n',
'Please choose different EA type.',sep=''))
}
}
}
EA
}
Q2EA.Xiao <- function(Q, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{
# Implementation of quaternion to Euler angles based on:
# J. Xiao, 2013. Princeton Vision Toolkit. Available from: <http://vision.princeton.edu/code.html>
# http://vision.princeton.edu/pvt/GCBreader/quaternion.m
# Q - [q1,q2,q3,q4] to EA - [psi,theta,phi]
# EA in radians
# Madgwick (zyx) originaly used Q = [phi, theta, psi]
# Converted by Jose Gama 2014
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if(is.null(dim(Q))) Q<-matrix(Q,1,4)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
Q <- Qnormalize(Q)
if (EulerOrder=='zyx') {
EA <- cbind(atan2(2*(Q[4]*Q[1]- Q[2]*Q[3]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)),
asin(2*(Q[2]*Q[4]+ Q[3]*Q[1])),
atan2(2*(Q[2]*Q[1]- Q[3]*Q[4]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)))
}
if (EulerOrder=='yxz') {
EA <- cbind( atan2(2*(Q[3]*Q[1]- Q[4]*Q[2]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)),
asin(2*(Q[2]*Q[1]+ Q[3]*Q[4])),
atan2(2*(Q[4]*Q[1]- Q[2]*Q[3]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='xzy') {
EA <- cbind( atan2(2*(Q[2]*Q[1]- Q[4]*Q[3]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)),
asin(2*(Q[2]*Q[3]+ Q[4]*Q[1])),
atan2(2*(Q[3]*Q[1]- Q[2]*Q[4]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='zxy') {
EA <- cbind( atan2(2*(Q[2]*Q[3]+ Q[4]*Q[1]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)),
asin(2*(Q[2]*Q[1]- Q[3]*Q[4])),
atan2(2*(Q[2]*Q[4]+ Q[3]*Q[1]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)))
}
if (EulerOrder=='yzx') {
EA <- cbind( atan2(2*(Q[2]*Q[4]+ Q[3]*Q[1]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)),
asin(2*(Q[4]*Q[1]- Q[2]*Q[3])),
atan2(2*(Q[2]*Q[1]+ Q[3]*Q[4]),(Q[1]^2- Q[2]^2+Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='xyz') {
EA <- cbind( atan2(2*(Q[2]*Q[1]+ Q[4]*Q[3]),(Q[1]^2- Q[2]^2-Q[3]^2+Q[4]^2)),
asin(2*(Q[3]*Q[1]- Q[2]*Q[4])),
atan2(2*(Q[2]*Q[3]+ Q[4]*Q[1]),(Q[1]^2+ Q[2]^2-Q[3]^2-Q[4]^2)))
}
if (EulerOrder=='zyz') {
EA <- cbind( atan2((Q[2]*Q[1]+ Q[3]*Q[4]),(Q[3]*Q[1]- Q[2]*Q[4])),
acos(Q[1]^2-Q[2]^2- Q[3]^2+Q[4]^2),
atan2((Q[3]*Q[4]- Q[2]*Q[1]),(Q[2]*Q[4]+ Q[3]*Q[1])))
}
if (EulerOrder=='zxz') {
EA <- cbind( atan2((Q[2]*Q[4]- Q[3]*Q[1]),(Q[2]*Q[1]+ Q[3]*Q[4])),
acos(Q[1]^2-Q[2]^2- Q[3]^2+Q[4]^2),
atan2((Q[2]*Q[4]+ Q[3]*Q[1]),(Q[2]*Q[1]- Q[3]*Q[4])))
}
if (EulerOrder=='yxy') {
EA <- cbind( atan2((Q[2]*Q[3]+ Q[4]*Q[1]),(Q[2]*Q[1]- Q[3]*Q[4])),
acos(Q[1]^2-Q[2]^2+ Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[3]- Q[4]*Q[1]),(Q[2]*Q[1]+ Q[3]*Q[4])))
}
if (EulerOrder=='yzy') {
EA <- cbind( atan2((Q[3]*Q[4]- Q[2]*Q[1]),(Q[2]*Q[3]+ Q[4]*Q[1])),
acos(Q[1]^2-Q[2]^2+ Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[1]+ Q[3]*Q[4]),(Q[4]*Q[1]- Q[2]*Q[3])))
}
if (EulerOrder=='xzx') {
EA <- cbind( atan2((Q[2]*Q[4]+ Q[3]*Q[1]),(Q[4]*Q[1]- Q[2]*Q[3])),
acos(Q[1]^2+Q[2]^2- Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[4]- Q[3]*Q[1]),(Q[2]*Q[3]+ Q[4]*Q[1])))
}
if (EulerOrder=='xyx') {
EA <- cbind( atan2((Q[2]*Q[3]- Q[4]*Q[1]),(Q[2]*Q[4]+ Q[3]*Q[1])),
acos(Q[1]^2+Q[2]^2- Q[3]^2-Q[4]^2),
atan2((Q[2]*Q[3]+ Q[4]*Q[1]),(Q[3]*Q[1]- Q[2]*Q[4])))
}
#EA = - EA # *(180/pi) (N×3) Euler angles in radians
theta = EA[,2] # (N×1) Angle THETA in radians
# Check EA
if (!ignoreAllChk) if (any(is.complex( EA ))) stop('Unreal\nUnreal Euler EA. Input resides too close to singularity.\nPlease choose different EA type.')
# Type 1 rotation (rotations about three distinct axes)
# THETA is computed using ASIN and ranges from -90 to 90 degrees
if (!ignoreAllChk) {
if (substr(EulerOrder,1,1) != substr(EulerOrder,3,3)){
singularities = abs(theta) > 89.9 *(pi/180) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)) {
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 1 Euler singularity.\n',
'Type 1 Euler singularity occurs when second angle is -90 or 90 degrees.\n',
'Please choose different EA type.',sep=''))
}
} else {
# Type 2 rotation (1st and 3rd rotation about same axis)
# THETA is computed using ACOS and ranges from 0 to 180 degrees
singularities = (theta < 0.1*(pi/180)) | (theta > 179.9*(pi/180)) # (N×1) Logical index
singularities[is.na(singularities)]<-FALSE
if (length(singularities)>0) if (any(singularities)){
firstsing = which(singularities)[1] # (1×1)
stop(paste('Input rotation ', firstsing, ' resides too close to Type 2 Euler singularity.\n',
'Type 2 Euler singularity occurs when second angle is 0 or 180 degrees.\n',
'Please choose different EA type.',sep=''))
}
}
}
EA
}
Q2EV <- function(Q,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# Q - [q1,q2,q3,q4] to EV - [m1,m2,m3,MU]
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
# Normalize quaternion(s) in case of deviation from unity.
# User has already been warned of deviation.
v_length=4; isnot_DCM=TRUE;N=dim(Q)[1]
#Qnorms = sqrt(apply(Q^2, 2,sum))
# Angle MU in radians and sine of MU/2
Q2 <-matrix(Q[,1:3],ncol=3,byrow=FALSE)
halfMUrad = matrix(atan2( sqrt(apply(Q2^2,1,sum)), Q[,4] ),ncol=1) # N×1
#print(halfMUrad)
SIN = sin(halfMUrad) # N×1
index = which(SIN==0) # [N×1] Logical index
if (length(index)>0){
# Initializing
EV = matrix(0,N,4)
# Singular cases [MU is zero degrees]
EV[index, 1] = 1
# Non-singular cases
SIN = SIN[-index, 1]
EV[-index, ] = cbind(Q[-index,1] / SIN,
Q[-index,2] / SIN,
Q[-index,3] / SIN,
halfMUrad * 2 )#* (360/pi)
} else {
# Non-singular cases
EV = cbind(Q[,1]/SIN, Q[,2]/SIN, Q[,3]/SIN, halfMUrad * 2)#*(360/pi)
}
# MU greater than 180 degrees
index = which(EV[,4] > pi/2) # [N×1] Logical index
EV[index, ] = cbind(-matrix(EV[index,1:3],ncol=3,byrow=FALSE), 2*pi-EV[index,4])
EV
}
Q2DCM <- function(Q,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# Q - [q1,q2,q3,q4] to DCM - 3x3xN
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
# Normalize quaternion(s) in case of deviation from unity.
Qn <- Qnormalize(Q)
# User has already been warned of deviation.
N<-dim(Qn)[1]
Qn = array(t(Qn), c(1, 4, N))
DCM= array(0, c(3, 3, N))
DCM[1,1,] = 1-2*(Qn[1,3,]*Qn[1,3,]+Qn[1,4,]*Qn[1,4,])
DCM[2,1,] = 2*(Qn[1,2,]*Qn[1,3,]-Qn[1,1,]*Qn[1,4,])
DCM[3,1,] = 2*(Qn[1,2,]*Qn[1,4,]+Qn[1,1,]*Qn[1,3,])
DCM[1,2,] = 2*(Qn[1,2,]*Qn[1,3,]+Qn[1,1,]*Qn[1,4,])
DCM[2,2,] = 1-2*(Qn[1,2,]*Qn[1,2,]+Qn[1,4,]*Qn[1,4,])
DCM[3,2,] = 2*(Qn[1,3,]*Qn[1,4,]-Qn[1,1,]*Qn[1,2,])
DCM[1,3,] = 2*(Qn[1,2,]*Qn[1,4,]-Qn[1,1,]*Qn[1,3,])
DCM[2,3,] = 2*(Qn[1,3,]*Qn[1,4,]+Qn[1,1,]*Qn[1,2,])
DCM[3,3,] = 1-2*(Qn[1,2,]*Qn[1,2,]+Qn[1,3,]*Qn[1,3,])
if (length(dim(DCM))==3) if(all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
DCM
}
# Q2DCM <- function(Q,tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
# {# Q - [q1,q2,q3,q4] to DCM - 3x3xN
# if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
# if (!ignoreAllChk) if (ichk && any(abs(sqrt(apply(Q^2, 2,sum)) - 1) > tol)) warning('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
# # Normalize quaternion(s) in case of deviation from unity.
# # User has already been warned of deviation.
# N<-dim(Q)[1]
# Q = array(t(Q), c(1, 4, N))
# SQ = Q^2
# DCM= array(0, c(3, 3, N))
# DCM[1,1,] = SQ[1,1,]-SQ[1,2,]-SQ[1,3,]+SQ[1,4,]
# DCM[1,2,] = 2*(Q[1,1,]*Q[1,2,] +Q[1,3,]*Q[1,4,])
# DCM[1,3,] = 2*(Q[1,1,]*Q[1,3,] -Q[1,2,]*Q[1,4,])
# DCM[2,1,] = 2*(Q[1,1,]*Q[1,2,] -Q[1,3,]*Q[1,4,])
# DCM[2,2,] = -SQ[1,1,]+SQ[1,2,]-SQ[1,3,]+SQ[1,4,]
# DCM[2,3,] = 2*(Q[1,2,]*Q[1,3,] +Q[1,1,]*Q[1,4,])
# DCM[3,1,] = 2*(Q[1,1,]*Q[1,3,] +Q[1,2,]*Q[1,4,])
# DCM[3,2,] = 2*(Q[1,2,]*Q[1,3,] -Q[1,1,]*Q[1,4,])
# DCM[3,3,] = -SQ[1,1,]-SQ[1,2,]+SQ[1,3,]+SQ[1,4,]
# if (all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
# DCM
# }
Q2GL<-function(Q)
{# Q - [q1,q2,q3,q4] to OpenGL translation matrix 4x4xn
#based on
#http://www.tinkerforge.com/doc/Software/Bricks/IMU_Brick_Python.html
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
GL <- lapply(1:N, function(n)
{
x<-Q[n,1]
y<-Q[n,2]
w<-Q[n,3]
z<-Q[n,4]
tmp <- c(1 - 2*(y*y + z*z), 2*(x*y - w*z), 2*(x*z + w*y), 0,
2*(x*y + w*z), 1 - 2*(x*x + z*z), 2*(y*z - w*x), 0,
2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x*x + y*y), 0,
0, 0, 0, 1)
matrix(tmp,nrow=4, ncol=4)
})
if (N==1) GL=array(unlist(GL), dim = c(dim(GL[[1]]) )) else GL=array(unlist(GL), dim = c(dim(GL[[1]]), length(GL)))
GL
}
EV2EA<-function(EV, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EV - [m1,m2,m3,MU] to EA - [psi,theta,phi]
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EV)) EV <- matrix(unlist(EV),ncol=4,byrow=FALSE)
Q<-EV2Q(EV, tol, ichk, ignoreAllChk)
EA<-Q2EA(Q, EulerOrder, tol, ichk, ignoreAllChk)
EA
}
EA2EV<-function(EA, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EA - [psi,theta,phi] to EV - [m1,m2,m3,MU]
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EA)) EA <- matrix(unlist(EA),ncol=3,byrow=FALSE)
Q<-EA2Q(EA, EulerOrder, ichk, ignoreAllChk)
EV<-Q2EV(Q, tol, ichk, ignoreAllChk)
EV
}
EA2EA<-function(EA, EulerOrder1='zyx',EulerOrder2='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{#EA - [psi,theta,phi] to EA - [psi,theta,phi]
if (!is.character(EulerOrder1)) stop('<<EulerOrder1>> must be a string.')
EulerOrder1 <- tolower(EulerOrder1)
if (!(EulerOrder1 %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.character(EulerOrder2)) stop('<<EulerOrder2>> must be a string.')
EulerOrder2 <- tolower(EulerOrder2)
if (!(EulerOrder2 %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (all(EulerOrder1==EulerOrder2)) return (EA)
Q<- EA2Q(EA, EulerOrder1, ichk, ignoreAllChk)
EA<-Q2EA(Q, EulerOrder2, tol, ichk, ignoreAllChk)
EA
}
EA2DCM<-function(EA, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EA - [psi,theta,phi] to DCM - 3x3xN
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (!is.matrix(EA)) EA <- matrix(unlist(EA),ncol=3,byrow=FALSE)
Q<-EA2Q(EA, EulerOrder, ichk, ignoreAllChk)
DCM<-Q2DCM(Q, tol, ichk, ignoreAllChk)
if (length(DCM)==3) if (all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
DCM
}
EV2DCM<-function(EV, tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# EV - [m1,m2,m3,MU] to DCM - 3x3xN
if (!is.matrix(EV)) EV <- matrix(unlist(EV),ncol=4,byrow=FALSE)
Q<-EV2Q(EV, tol, ichk, ignoreAllChk)
DCM<-Q2DCM(Q, tol, ichk, ignoreAllChk)
if (length(DCM)==3) if (all(dim(DCM)==c(3,3,1))) DCM <- matrix(DCM,3)
DCM
}
DCM2EV<-function(DCM, tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# DCM - 3x3xN to EV - [m1,m2,m3,MU]
Q<-DCM2Q(DCM, tol, ichk, ignoreAllChk)
EV<-Q2EV(Q, tol, ichk, ignoreAllChk)
EV
}
DCM2EA<-function(DCM, EulerOrder='zyx', tol = 10 * .Machine$double.eps, ichk=FALSE, ignoreAllChk=FALSE)
{# DCM - 3x3xN to EA - [psi,theta,phi]
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
Q<-DCM2Q(DCM, tol, ichk, ignoreAllChk)
EA<-Q2EA(Q, EulerOrder, tol, ichk, ignoreAllChk)
EA
}
vectQrot<-function( Q, rr )
# Rotate a vector by a quaternion
{
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
if (!is.matrix(rr)) rr <-matrix(rr,ncol=3,byrow=FALSE)
N <- dim(Q)[1]
if (!is.matrix(rr)) rr <-matrix( rr,ncol=3,nrow=N,byrow=FALSE)
Qr <- (Qconj(Q) %Q*% cbind(0, rr)) %Q*% Q
Qr[,2:4]
}
Qrot <- function(Q,w,dT)
# Updates current attitude quaternion q
# output - current attitude quaternion
# input - wx, wy, wz - angular rate values
# input - dT - inverse of update rate
{
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (!is.matrix(w)) w <-matrix(w,ncol=3,byrow=FALSE)#nrow=N,
Qr <- matrix(0,nrow=N, ncol=4)
Qr<-lapply(1:N, function(n) {
Fx <- w[n,1]*dT
Fy <- w[n,2]*dT
Fz <- w[n,3]*dT
Fm <- sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
sinFm2 <- sin(Fm/2)
cosFm2 <- cos(Fm/2)
if (Fm != 0)
Qr[n,]<-c(cosFm2, Fx/Fm*sinFm2, Fy/Fm*sinFm2, Fz/Fm*sinFm2)#Qr[n,]
else
Qr[n,]<-c(1, 0, 0, 0)#Qr[n,]
Qr[n,]<- Q[n,] %Q*% Qr[n,]#Qr[n,]<- Q[n,] %Q*% Qr[n,]
Qr
})
if (N==1) Qr=array(unlist(Qr), dim = c(dim(Qr[[1]]) )) else Qr=array(unlist(Qr), dim = c(dim(Qr[[1]]), length(Qr)))
if (length(dim(Qr))==3) Qr <- array(Qr,c(N,4))
#if (length(dim(Qr))==3) if (all(dim(Qr)==c(1,4,1))) Qr <- array(Qr,c(1,4))
Qr
}
"%Q+%" <- function(Q1, Q2)
{# quaternion sum
if (!is.matrix(Q1)) Q1 <-matrix(Q1,ncol=4,byrow=FALSE)
if (!is.matrix(Q2)) Q2 <-matrix(Q2,ncol=4,byrow=FALSE)
Q1 + Q2
}
"%Q-%" <- function(Q1, Q2)
{# quaternion difference
if (!is.matrix(Q1)) Q1 <-matrix(Q1,ncol=4,byrow=FALSE)
if (!is.matrix(Q2)) Q2 <-matrix(Q2,ncol=4,byrow=FALSE)
Q1 - Q2
}
Qconj<-function(Q)
{# quaternion conjugate
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
Q[,2:4] <- -Q[,2:4]
Q
}
Qinv<-function(Q)
{# quaternion inverse
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (Qnorm(Q)==1) return (Qconj(Q))
else return (Qconj(Q)/Qnorm(Q))
}
"%Q*%" <- function(Q1, Q2)
{# quaternion product
if (!is.matrix(Q1)) Q1 <-matrix(Q1,ncol=4,byrow=FALSE)
if (!is.matrix(Q2)) Q2 <-matrix(Q2,ncol=4,byrow=FALSE)
N <- dim(Q1)[1]
ab<-matrix(0,ncol=4,nrow=N)
ab<-lapply(1:N, function(n) {
ab[n,1]<-Q1[n,1]*Q2[n,1]-Q1[n,2]*Q2[n,2]-Q1[n,3]*Q2[n,3]-Q1[n,4]*Q2[n,4]
ab[n,2]<-Q1[n,1]*Q2[n,2]+Q1[n,2]*Q2[n,1]+Q1[n,3]*Q2[n,4]-Q1[n,4]*Q2[n,3]
ab[n,3]<-Q1[n,1]*Q2[n,3]-Q1[n,2]*Q2[n,4]+Q1[n,3]*Q2[n,1]+Q1[n,4]*Q2[n,2]
ab[n,4]<-Q1[n,1]*Q2[n,4]+Q1[n,2]*Q2[n,3]-Q1[n,3]*Q2[n,2]+Q1[n,4]*Q2[n,1]
ab
} )
if (N==1) ab=array(unlist(ab), dim = c(dim(ab[[1]]) )) else ab=array(unlist(ab), dim = c(dim(ab[[1]]), length(ab)))
ab<-matrix(ab,ncol=4,nrow=N)
return(ab)
}
"%Q/%" <- function(Q1, Q2)
{# quaternion division
Q1 %Q*% Qinv(Q2)
}
Qnorm<-function(Q)
{ # norm of a quaternion
if (length(Q)==3) Q<-c(0,Q)
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (N==1) sqrt(sum(Q^2))
apply(Q,1,function(n) sqrt(sum(n^2)))
}
Qnormalize<-function(Q)
{ # normalize a quaternion
if (length(Q)==3) Q<-c(0,Q)
if (!is.matrix(Q)) Q <-matrix(Q,ncol=4,byrow=FALSE)
N <- dim(Q)[1]
if (N==1) Qn <- Q/sqrt(sum(Q^2)) else Qn <- Q/apply(Q,1,function(n) sqrt(sum(n^2)))
Qn[which(is.na(Qn))] <- 0
Qn
}
#############################################################################################
isUnitQuaternion<-function(Q)
{# returns TRUE if q is a unit quaternion
# |q|==1 => q is a unit quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
qN <- Qnorm(Q)
qN == 1
}
isPureQuaternion<-function(Q)
{# returns TRUE if q is a pure quaternion
# q==(0,v) => q is a pure quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
(Q[,2] == 0) & (Q[,3] == 0) & (Q[,4] == 0)
}
isRealQuaternion<-function(Q)
{# returns TRUE if q is a real quaternion
# q==(0,v) => q is a pure quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
Q[,1] == 0
}
Qlog<-function(Q)
{# returns the logarithm of a quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
a <- acos(Q[,1])
sina <- sin(a)
Qret <- matrix(0,nrow=dim(Q)[1],4)
sinaZero <- which(sina>0)
Qret[sinaZero,2:4] <- a[sinaZero] * Q[sinaZero,2:4] / sina[sinaZero]
Qret
}
Qexp<-function(Q)
{# returns the exponential of a quaternion
# source: Quaternion C++ class by Will Perone, 2003
# http://willperone.net/Code/quaternion.php
if (!is.matrix(Q)) Q <- matrix(Q,ncol=4,byrow=FALSE)
a <- exp(Q[,1])
sina <- sin(a)
cosa <- cos(a)
Qret <- cbind(cosa,0,0,0)
sinaZero <- which(sina>0)
Qret[sinaZero,2:4] <- sina[sinaZero] * Q[sinaZero,2:4] / a[sinaZero]
Qret
}
"%Q.%" <- function(Q1, Q2)
{# quaternion dot product
apply(Q1 * Q2,1,sum)
}
Qzero <- function(n=NA){
# generate zero-value quaternions
if (is.na(n)) n <- 1
matrix(0,n,4)
}
Qone <- function(n=NA){
# generate one-value quaternions
if (is.na(n)) n <- 1
matrix(c(1,0,0,0),n,4)
}
Qlerp<-function(Q1, Q2, fracT)
{# linear quaternion interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
N <- dim(Q1)[1]
if (fracT == 0.0) return (Qzero(N))
if (fracT == 1.0) return (Qone(N))
Qnormalize((Q1*(1-fracT) %Q+% Q2*fracT))
}
Qslerp<-function(Q1, Q2, fracT)
{# spherical linear interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
N <- dim(Q1)[1]
if (fracT == 0.0) return (Qzero(N))
if (fracT == 1.0) return (Qone(N))
Q3 <- Q2
Qd <- Q1 %Q.% Q2
QdLtz <- which(Qd < 0)
Qd[QdLtz] <- -Qd[QdLtz]
Q3[QdLtz] <- -Q2[QdLtz]
QdLt.95 <- which(Qd < 0.95)
angleQ = acos(Qd[QdLt.95])
Q3[QdLt.95] <- (Q1[QdLt.95] * sin(angleQ * (1-fracT)) + Q3[QdLt.95] * sin(angleQ * fracT))/sin(angleQ)
Q3[-QdLt.95] <- Qlerp(Q1[QdLt.95], Q3[QdLt.95],fracT)
Q3
}
QslerpNoInvert<-function(Q1, Q2, fracT)
{# This version of slerp, used by squad, does not check for theta > 90.
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
N <- dim(Q1)[1]
if (fracT == 0.0) return (Qzero(N))
if (fracT == 1.0) return (Qone(N))
Q3 <- Q2
Qd <- Q1 %Q.% Q2
QdLt.bt95 <- which((Qd < 0.95) & (Qd > -0.95))
angleQ = acos(Qd[QdLt.bt95])
Q3[QdLt.bt95] <- (Q1[QdLt.bt95] * sin(angleQ * (1-fracT)) + Q2[QdLt.bt95] * sin(angleQ * fracT))/sin(angleQ)
Q3[-QdLt.bt95] <- Qlerp(Q1[QdLt.bt95], Q2[QdLt.bt95],fracT)
Q3
}
Qsquad<-function(Q1, Q2, Qa, Qb, fracT)
{# Spherical and Quadrangle linear interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Qc <- QslerpNoInvert(Q1,Q2,fracT)
Qd <- QslerpNoInvert(Qa,Qb,fracT)
QslerpNoInvert(Qc,Qd,2*fracT*(1-fracT))
}
Qbezier<-function(Q1, Q2, Qa, Qb, fracT)
{# Shoemake-Bezier interpolation using De Castlejau algorithm
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Q11 <- QslerpNoInvert(Q1,Qa,fracT)
Q12 <- QslerpNoInvert(Qa,Qb,fracT)
Q13 <- QslerpNoInvert(Qb,Q2,fracT)
QslerpNoInvert(QslerpNoInvert(Q11,Q12,fracT), QslerpNoInvert(Q12,Q13,fracT), fracT)
}
Qspline<-function(Qnm1, Qn, Qnp1)
{# Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Qni <- Qn
Qni[,2:4] <- -Qni[,2:4]
Qn %Q*% Qexp(( Qlog(Qni %Q*% Qnm1) %Q+% Qlog(Qni %Q*% Qnp1) ) / -4)
}
QangularDifference<-function(Q1, Q2)
{#angular difference between 2 quaternions
acos((Q1 %Q.% Q2)/(Qnorm(Q1)*Qnorm(Q2)))
}
Qrandom <- function(n=NA)
{# generate - uniform random unit quaternions
#
# http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
# LaValle, Steven M. "Planning algorithms." University of Illinois 2004 (1999).
# pp. 164 - "uniform random unit quaternions leads to uniform random samples over SO(3)"
if (is.na(n)) n <- 1
R <- matrix(stats::runif(n*3),n,3)
r1 <- sqrt(1.0 - R[,1])
r2 <- sqrt(R[,1])
pi2 <- pi * 2.0
t1 <- pi2 * R[,2]
t2 <- pi2 * R[,3]
Q <- cbind(cos(t2)*r2, sin(t1)*r1,cos(t1)*r1, sin(t2)*r2)
Qnormalize(Q)
}
isPureRotationMatrix<-function(DCM, tol=0.01)
{# pure rotation matrix = proper orthogonal matrix = det(m)==1
if (is.matrix(DCM)) return(((det(DCM)-1)<tol) & (det(DCM)>0)) else {
apply(DCM,3,function(x) (((det(x)-1)<tol) & (det(x)>0)) )
}
}
DCMrandom <- function(n=NA, tol = 10 * .Machine$double.eps, ignoreAllChk=FALSE)
{# generate - uniform random DCM
if (is.na(n)) n <- 1
Q <- Qrandom(n)
DCM <- Q2DCM(Q, tol = tol, ignoreAllChk=TRUE)
if (ignoreAllChk) return(DCM)
badQ <- which(isPureRotationMatrix(DCM)==FALSE)
while (length(badQ)>0) {# remove wrong matrices
n <- length(badQ)
Q <- Qrandom(n)
DCM[badQ] <- Q2DCM(Q, tol = tol, ignoreAllChk=TRUE)
badQ <- which(isPureRotationMatrix(DCM)==FALSE)
}
DCM
}
EArandom <- function(n=NA, EulerOrder='zyx',tol = 10 * .Machine$double.eps, ignoreAllChk=FALSE)
{# generate - uniform random Euler Angles
if (!is.character(EulerOrder)) stop('<<EulerOrder>> must be a string.')
EulerOrder <- tolower(EulerOrder)
if (!(EulerOrder %in% c('zyx','zxy','yxz','xzy','xyz','yzx','zyz','zxz','yxy','yzy','xyx','xzx'))) stop('Invalid input Euler angle order')
if (is.na(n)) n <- 1
Q <- Qrandom(n)
EA <- Q2EA(Q, EulerOrder, tol = tol, ignoreAllChk=TRUE)
if (ignoreAllChk) return(EA)
badQ <- which((abs(EA[,2]) > 89.9 *(pi/180)) & ((EA[,2] < 0.1*(pi/180)) | (EA[,2] > 179.9*(pi/180))))
while (length(badQ)>0) {# fix singularities
n <- length(badQ)
Q <- Qrandom(n)
EA[badQ] <- Q2EA(Q, EulerOrder, tol = tol, ignoreAllChk=TRUE)
badQ <- which((abs(EA[,2]) > 89.9 *(pi/180)) & ((EA[,2] < 0.1*(pi/180)) | (EA[,2] > 179.9*(pi/180))))
}
EA
}
EVrandom <- function(n=NA, tol = 10 * .Machine$double.eps, ignoreAllChk=FALSE)
{# generate - uniform random Euler Vectors
if (is.na(n)) n <- 1
Q <- Qrandom(n)
EV <- Q2EV(Q, tol = tol, ignoreAllChk=TRUE)
if (ignoreAllChk) return(EV)
EV
}
Try the RSpincalc package in your browser
Any scripts or data that you put into this service are public.
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
Embedding an R snippet on your website
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.