stla
commented
2 years ago I did it. It works!
R code for Barry-Goldman quaternions spline
library(onion)
.canonicalized <- function(quaternions){
l <- length(quaternions)
out <- quaternion(length.out = l)
p <- H1
for(i in seq_len(l)){
q <- quaternions[i]
if(dotprod(p, q) < 0){
q = -q
}
out[i] = q
p <- q
}
out
}
.check_quaternions <- function(quaternions, closed){
if(length(quaternions) < 2L){
stop("At least two quaternions are required.")
}
if(closed){
quaternions <- c(quaternions, quaternions[1L])
}
.canonicalized(quaternions)
}
.check_keyTimes <- function(keyTimes, n_quaternions){
if(is.null(keyTimes)){
return(seq_len(n_quaternions))
}
if(any(diff(keyTimes) <= 0)){
stop("`keyTimes` must be an increasing vector of numbers.")
}
keyTimes
}
.check_time <- function(t, keyTimes){
l <- length(keyTimes)
lastKeyTime <- keyTimes[l]
if(t < keyTimes[1L] || t > lastKeyTime){
stop("The interpolating times must be within the range of `keyTimes`.")
}
if(t < lastKeyTime){
idx <- findInterval(trunc(t), keyTimes, left.open = TRUE, rightmost.closed = FALSE)
}else{ # t = lastKeyTime
idx <- l - 2L
}
idx
}
.slerp <- function(q1, q2, t){
(q2 * onion_inverse(q1))^t *q1
}
BarryGoldman <- function(quaternions, keyTimes = NULL, times){
quaternions <- .check_quaternions(quaternions, closed = TRUE)
n_quaternions <- length(quaternions)
keyTimes <- .check_keyTimes(keyTimes, n_quaternions)
n_keyTimes <- length(keyTimes)
evaluate <- function(t){
if((l <- length(t)) > 1L){
out <- quaternion(l)
for(j in seq_len(l)){
out[j] <- evaluate(t[j])
}
return(out)
}
idx <- .check_time(t, keyTimes) + 1L
q0 <- quaternions[idx]
q1 <- quaternions[idx + 1L]
t0 <- keyTimes[idx]
t1 <- keyTimes[idx + 1L]
if(idx == 1L){
q_1 <- quaternions[n_quaternions - 1L]
if(dotprod(q_1, q0) < 0){
q_1 <- -q_1
}
t_1 <- t0 - (keyTimes[n_keyTimes] - keyTimes[n_keyTimes - 1L])
}else{
q_1 <- quaternions[idx-1]
t_1 <- keyTimes[idx-1L]
}
if(idx + 1L == n_quaternions){
q2 <- quaternions[2L]
if(dotprod(q1, q2) < 0){
q2 <- -q2
}
t2 <- t1 + (keyTimes[2L] - keyTimes[1L])
}else{
q2 <- quaternions[idx+2]
t2 <- keyTimes[idx+2]
}
slerp_0_1 <- .slerp(q0, q1, (t - t0) / (t1 - t0))
.slerp(
.slerp(
.slerp(q_1, q0, (t - t_1) / (t0 - t_1)),
slerp_0_1,
(t - t_1) / (t1 - t_1)
),
.slerp(
slerp_0_1,
.slerp(q1, q2, (t - t1) / (t2 - t1)),
(t - t0) / (t2 - t0)
),
(t - t0) / (t1 - t0)
)
}
evaluate(times)
}Application
#' @description Spherical coordinates to Cartesian coordinates.
sph2cart <- function(rho, theta, phi){
return(c(
rho * sin(theta) * sin(phi),
rho * cos(theta) * sin(phi),
rho * cos(phi)
))
}
#' @description Get the unit quaternion whose corresponding rotation
#' sends U to V; the vectors U and V must be normalized.
getQuaternion <- function(U, V){
d <- c(tcrossprod(U, t(V)))
c <- sqrt(1 + d)
r <- 1 / sqrt(2) / c
W <- r * rgl:::xprod(U, V)
quaternion(Re = c/sqrt(2), i = W[1L], j = W[2L], k = W[3L])
}
################################################################################
keyPoints <- matrix(nrow = 0L, ncol = 3L)
theta_ <- seq(0, 2*pi, length.out = 9L)[-1L]
phi <- 1
for(theta in theta_){
keyPoints <- rbind(keyPoints, sph2cart(5, theta, phi))
phi = pi - phi
}
keyRotors <- quaternion(length.out = nrow(keyPoints))
rotor <- H1
keyRotors[1L] <- rotor
for(i in seq_len(nrow(keyPoints) - 1L)){
keyRotors[i+1L] <- rotor <-
getQuaternion(keyPoints[i,]/5, keyPoints[i+1L,]/5) * rotor
}
nkr <- length(keyRotors)
n <- 10 # number of interpolations for each segment
times = seq(1, nkr+1, length.out = n*(nkr-1)-nkr+2)
rotors <- BarryGoldman(keyRotors, keyTimes = seq_len(nkr+1), times = times)
points <- matrix(nrow = 0L, ncol = 3L)
point1 <- rbind(keyPoints[1L, ])
for(i in seq_along(rotors)){
points <- rbind(
points,
rotate(point1, rotors[i])
)
}
library(rgl)
spheres3d(0, 0, 0, radius = 5, color = "lightgreen")
spheres3d(points, radius = 0.2, color = "midnightblue")
RobinHankin
Hello Robin,
Would you be interested in adding some quaternions splines in onion? The slerp allows quaternions interpolation, but it interpolates along a "straight spherical line". Here is an example of slerp to interpolate between the red balls (and the blue one):
Here are the same points interpolated with a Kochanek-Bartels algorithm:
I did that with the Python library splines. The Barry-Goldman quaternions spline does not seem complicated to implement, and gives something similar to Kochanek-Bartels (which seems complicated to implement).
I will try too implement Barry-Goldman in R. Would you be interested in including it in your package?