R/rotate.towards.R

In eggs: Distances and areas on the surface of an egg

rotate.towards = function(V, D, theta) {
  # Returns the 3d vector obtained by rotating V by theta radians towards D
  V = norm(cbind(V))
  D = norm(cbind(D))
  Q = norm(cross(V,D)) # Axis of rotation
  if(angle.between(V, D) != 0) {
    if(angle.between(Q, c(0,0,1)) < 1e-6) {
      R1 = diag(3)
    } else {
      R1 = get.rotation.matrix(Q, c(0,0,1))
    }
    V = R1%*%V
    R2 = rbind(c(cos(theta),-sin(theta),0), c(sin(theta),cos(theta),0), c(0,0,1))
    V = R2%*%cbind(V)
    if(angle.between(c(0,0,1), Q) < 1e-6) {
      R3 = diag(3)
    } else {
      R3 = get.rotation.matrix(c(0,0,1), Q)
    }
    V = R3%*%cbind(V)
  }
  return (norm(V))
}