Torus whose centerline passes through three given points (R)

torusThreePoints.R

library(rgl) 

#~~ Torus passing by three points ~~####

## cross-product ###
crossProduct <- function(u, v) {
  c(
    u[2L]*v[3L] - u[3L]*v[2L],
    u[3L]*v[1L] - u[1L]*v[3L],
    u[1L]*v[2L] - u[2L]*v[1L]
  )
}

## plane passing by points p1, p2, p3 ###
plane3pts <- function(p1, p2, p3){ 
  w <- crossProduct(p1 - p2, p2 - p3)
  offset <- sum(p1 * w) 
  c(w, offset)
}

## center, radius and normal of the circle passing by three points ###
circleCenterAndRadius <- function(p1, p2, p3){
  p12 <- (p1 + p2)/2
  p23 <- (p2 + p3)/2
  v12 <- p2 - p1
  v23 <- p3 - p2
  plane <- plane3pts(p1, p2, p3)
  normal <- plane[1L:3L]
  A <- rbind(normal, v12, v23)
  b <- c(plane[4L], sum(p12*v12), sum(p23*v23))
  center <- c(solve(A) %*% b)
  r <- sqrt(c(crossprod(p1 - center)))
  list("center" = center, "radius" = r, "normal" = normal)
}

## transformation matrix ###
transfoMatrix <- function(p1, p2, p3){
  crn <- circleCenterAndRadius(p1, p2, p3)
  center <- crn[["center"]]
  radius <- crn[["radius"]]
  normal <- crn[["normal"]]
  measure <- sqrt(c(crossprod(normal)))
  normal <- normal/measure
  s <- sqrt(normal[1L]^2 + normal[2L]^2)
  if(s == 0) {
    M <- cbind(rbind(diag(3L), center), c(0, 0, 0, 1))
  } else {
    u <- c(normal[2L]/s, -normal[1L]/s, 0)
    v <- crossProduct(normal, u)
    M <- cbind(rbind(u, v, normal, center), c(0, 0, 0, 1))
  }
  list("matrix" = M, "radius" = radius)
}

## torus whose centerline passes through p1, p2, p3 ###
torusThreePoints <- function(p1, p2, p3, radius, sides, n, ...) {
  TandR <- transfoMatrix(p1, p2, p3)
  transfo <- TandR[["matrix"]]
  R <- TandR[["radius"]]
  theta_ <- seq(0, 2*pi, length.out = n)
  circle <- cbind(R*cos(theta_), R*sin(theta_), 0)
  tube <- addNormals(
    cylinder3d(circle, radius = radius, sides = sides, closed = TRUE, ...)
  )
  transform3d(tube, transfo)
}


# test ####
p1 <- c(0, 0, 0)
p2 <- c(1, 1, 1)
p3 <- c(3, 2, 1)
torus <- torusThreePoints(p1, p2, p3, radius = 1, sides = 38L, n = 120L)

shade3d(torus, color = "red", alpha = 0.5)
points3d(rbind(p1, p2, p3), size = 10)