# R/alignBall.R In ftelschow/KneeAnally: Statistical analysis of gait data modeled as curves in SO(3)

#### Defines functions KreuzProdRot2AxisAngleAxisAngle2Rotspat.aveBallAlignment

```################################################################################
#
#   Ball's functional alignment
#
################################################################################
KreuzProd <- function(a,b){
c( a[2]*b[3] - a[3]*b[2], a[3]*b[1] - a[1]*b[3], a[1]*b[2] - a[2]*b[1] )
}

Rot2AxisAngle <-  function(R){
#   v = c( R[3,2] - R[2,3], R[1,3]-R[3,1], R[2,1]-R[1,2])
#   norm <- sqrt(sum(v^2))
#   theta <- atan(norm / (R[1,1]+R[2,2]+R[3,3]-1))
v <- screwM2Vec(Log.SO3(R))
theta <- sqrt(sum(v^2))
list(u = v / theta, ang = theta)
#    list(u = v / norm, ang = theta)
}

AxisAngle2Rot <- function(AxisAngle){
u <- AxisAngle\$u
ang <- AxisAngle\$ang
rbind(
c( u[1]*u[1]*(1-cos(ang)) + cos(ang),      u[1]*u[2]*(1-cos(ang)) - u[3]*sin(ang), u[1]*u[3]*(1-cos(ang)) + u[2]*sin(ang) ),
c( u[2]*u[1]*(1-cos(ang)) + u[3]*sin(ang), u[2]*u[2]*(1-cos(ang)) + cos(ang), u[2]*u[3]*(1-cos(ang)) - u[1]*sin(ang) ),
c( u[3]*u[1]*(1-cos(ang)) - u[2]*sin(ang), u[3]*u[2]*(1-cos(ang)) + u[1]*sin(ang), u[3]*u[3]*(1-cos(ang)) + cos(ang) )
)
}

spat.ave <- function(A){
S = Quat2Rot(A[,1])
for(i in 2:dim(A)[2]){
G <- t(S) %*% Quat2Rot(A[,i])
u <- Rot2AxisAngle(G)
u\$ang <- u\$ang / i
P <- AxisAngle2Rot( u )
S <- S %*% P
}
S
}

BallAlignment <- function(A, r=c(0,1,0), ave = "Ball"){
## Compute spatial average
if( ave == "Ball" ){
A.ave <- spat.ave(A)
}else{
A.ave <- Quat2Rot(ProjectiveMean(A)\$mean)
}

ind.pos <- NULL
for(i in 1:dim(A)[2]){
R <- t(A.ave) %*% Quat2Rot(A[,i])
if( sum(Rot2AxisAngle(R)\$u * r) > 0 ){
ind.pos <- c(ind.pos,i)
}
}

if( ave == "Ball" ){
A.p <- spat.ave( A[,ind.pos] )
A.n <- spat.ave( A[,setdiff(1:dim(A)[2], ind.pos)] )
}else{
A.p <- Quat2Rot(ProjectiveMean( A[,ind.pos] )\$mean)
A.n <- Quat2Rot(ProjectiveMean( A[,setdiff(1:dim(A)[2], ind.pos)] )\$mean)
}

## Get M.A
J.M <- t(A.n) %*% A.p
m   <- Rot2AxisAngle(J.M)\$u
theta.m <- acos( sum(m * r) )
if(theta.m > pi/2){
theta.m = theta.m - pi
m <- -m
}
u.m   <- KreuzProd(m,r)
u.m   <- u.m / sqrt(sum(u.m^2))
M.A <- AxisAngle2Rot( list(u=u.m, ang=theta.m) )

## Get B.A
J.B <- A.n %*% t(A.p)
b   <- Rot2AxisAngle(J.B)\$u
theta.b <- acos( sum(b * r) )
if(theta.b > pi/2){
theta.b = theta.b - pi
b <- -b
}
u.b   <- KreuzProd(b,r)
u.b   <- u.b / sqrt(sum(u.b^2))
B.A <- AxisAngle2Rot( list(u=u.b, ang=theta.b) )

list(rR = M.A, lR = t(B.A))
}
```
ftelschow/KneeAnally documentation built on Nov. 4, 2019, 12:58 p.m.