R/R_to_AA.r

In affiner: A Finer Way to Render 3D Illustrated Objects in 'grid' Using Affine Transformations

# Rotation matrix to Axis-angle representation
# https://en.wikipedia.org/wiki/Axis-angle_representation

#' Convert from 3D rotation matrix to axis-angle representation.
#'
#' `rotate3d_to_AA()` converts from (post-multiplied) rotation matrix
#' to an axis-angle representation of 3D rotations.
#'
#' @examples
#'  # axis-angle representation of 90 degree rotation about the x-axis
#'  rotate3d_to_AA(rotate3d("x-axis", 90, unit = "degrees"))
#'
#'  # find Axis-Angle representation of first rotating about x-axis 180 degrees
#'  # and then rotating about z-axis 45 degrees
#'  R <- rotate3d("x-axis", 180, unit = "degrees") %*%
#'         rotate3d("z-axis", 45, unit = "degrees")
#'  AA <- rotate3d_to_AA(R)
#'
#'  # Can use `rotate3d()` to convert back to rotation matrix representation
#'  all.equal(R, do.call(rotate3d, AA))
#'
#' @seealso \url{https://en.wikipedia.org/wiki/Axis-angle_representation} for more details
#'   about the Axis-angle representation of 3D rotations.
#'   [rotate3d()] can be used to convert from an axis-angle representation to a rotation matrix.
#' @param mat 3D rotation matrix (post-multiplied).
#'            If you have a pre-multiplied rotation matrix
#'            simply transpose it with [t()] to get a post-multiplied rotation matrix.
#' @inheritParams angle
#' @export
rotate3d_to_AA <- function(mat = diag(4), unit = getOption("affiner_angular_unit", "degrees")) {
    unit <- standardize_angular_unit(unit)
    if (!all(dim(mat) == c(3, 3)) && !is_transform3d(mat))
        mat <- transform3d(mat)
    R <- mat[1:3, 1:3]

    theta <- arccosine(0.5 * (trace(R) - 1), unit = unit)
    if (all(is_congruent(R, diag(3)))) { # no rotation
        theta <- angle(0, unit)
        e <- c(0, 0, 1)
    } else if (is_congruent(theta, angle(180, "degrees"))) { # 180 degree rotation
        theta <- angle(from_piradians(1, unit), unit)
        B <- 0.5 * (R + diag(3))
        e <- sqrt(diag(B))
        sB <- sign(B)
        if (all(is_congruent(sB, ppn))) {
            e[3] <- -e[3]
            theta <- angle(from_piradians(-1, unit), unit)
        } else if (all(is_congruent(sB, pnp))) {
            e[2] <- -e[2]
        } else if (all(is_congruent(sB, npp))) {
            e[1] <- -e[1]
        }
    } else {
        e <- numeric(3)
        e[1] <- R[3,2] - R[2,3]
        e[2] <- R[1,3] - R[3,1]
        e[3] <- R[2,1] - R[1,2]
        e <- e / (2 * sin(-theta))
    }
    if (e[3] < 0) { # Force z-axis element positive
        e <- -e
        theta <- -theta
    }
    list(axis = as_coord3d(e[1], e[2], e[3]),
         theta = theta)
}

# Sign matrices for "B" matrix used to tell signs for axis unit vector (up to sign ambiguity) when angle = 180 degrees
# signs for ppp and nnn don't need to be changed
# ppn and npp
ppn <- matrix(c(1, 1, -1,
                1, 1, -1,
                -1, -1, 1), ncol = 3, byrow = TRUE)
# pnp and npn
pnp <- matrix(c(1, -1, 1,
                -1, 1, -1,
                1, -1, 1), ncol = 3, byrow = TRUE)
# pnn and npp
npp <- matrix(c(1, -1, -1,
                -1, 1, 1,
                -1, 1, 1), ncol = 3, byrow = TRUE)

# trace of a (square) matrix
trace <- function(m) sum(diag(m))