FC:Script Orientation

library(rgl)

Nearest point calculating Function

calculate_nearest_point <- function(disk_center, disk_radius, disk_orientation,  
                                   plane_point, plane_normal, plane_orientation) {
  
  disk_rot_mat <- create_rotation_matrix(disk_orientation)
  plane_rot_mat <- create_rotation_matrix(plane_orientation)
  
  plane_point_rot <- rotate_point(plane_point, plane_rot_mat)
  plane_normal_rot <- rotate_vector(plane_normal, plane_rot_mat)

  disk_center_plane <- transform_point(disk_center, plane_rot_mat, plane_point_rot)

  nearest_pt <- disk_center_plane + 
               disk_radius * (plane_normal_rot / norm(plane_normal_rot))
               
  transform_point(nearest_pt, plane_rot_mat, plane_point_rot)
}

Nearest points

calculate_nearest_points <- function(disk_center, disk_radius, disk_orientation,  
                                     plane_point, plane_normal, plane_orientation) {

  disk_rot_mat <- create_rotation_matrix(disk_orientation)
  plane_rot_mat <- create_rotation_matrix(plane_orientation)
  
  plane_point_rot <- rotate_point(plane_point, plane_rot_mat)
  plane_normal_rot <- rotate_vector(plane_normal, plane_rot_mat)  

  disk_center_plane <- transform_point(disk_center, plane_rot_mat, plane_point_rot)

  # Grid search
  min_dist <- Inf
  
  for (x in seq(-5, 5, 0.1)) {
    for (y in seq(-5, 5, 0.1)) {
    
      plane_pt <- c(x, y, 0)
      
      disk_pt <- disk_center_plane - 
                (disk_radius * (plane_normal_rot / norm(plane_normal_rot)))
                
      plane_pt <- disk_center_plane + 
                (disk_radius * (plane_normal_rot / norm(plane_normal_rot)))  
    
      dist <- sqrt(sum((disk_pt - plane_pt)^2))
      
      if (dist < min_dist) {
        min_dist <- dist
        disk_nearest_pt <- disk_pt
        plane_nearest_pt <- plane_pt
      } 
    }
  }
  
  return(c(disk_nearest_pt, plane_nearest_pt))
}

Function for creating rotation matrix from angles

create_rotation_matrix <- function(angles) {
  roll <- angles[1]
  pitch <- angles[2]
  yaw <- angles[3]
  
  R_roll <- matrix(c(1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll)), nrow = 3)
  R_pitch <- matrix(c(cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch)), nrow = 3)
  R_yaw <- matrix(c(cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1), nrow = 3)
  
  rot_matrix <- R_yaw %*% R_pitch %*% R_roll
  
  return(rot_matrix)
}

Function to rotate point by rotation matrix

rotate_point <- function(point, rot_mat) {
  return(rot_mat %*% point)
}

Function to rotate vector by rotation matrix

rotate_vector <- function(vec, rot_mat) {
  return(rot_mat %*% vec)
}

Function to transform point between frames

transform_point <- function(point, rot_mat, origin) {

  point <- point - origin  

  rotated_pt <- rot_mat %*% point

  transformed_pt <- rotated_pt + origin

  return(transformed_pt)
}

Getting inputs

disk_roll <- 10 * pi/180  
disk_pitch <- 10* pi/180
disk_yaw <- 10 * pi/180


plane_roll <- 20 * pi/180
plane_pitch <- 30 * pi/180  
plane_yaw <- 40 * pi/180

disk_radius <- 5
disk_center <- c(0, 0, 0)
plane_normal <- c(0, 0, 1) 
plane_point <- c(0, 0, 10)

orientations

disk_orientation <- c(disk_yaw, disk_pitch, disk_roll) 
plane_orientation <- c(plane_yaw, plane_pitch, plane_roll)

Getting nearest point

nearest_pts <- calculate_nearest_points(disk_center, disk_radius, disk_orientation,
                                        plane_point, plane_normal, plane_orientation)

nearest point

cat("Nearest point on disk: ", nearest_pts[1], "\n")  

## Nearest point on disk:  -3.021942

cat("Nearest point on plane: ", nearest_pts[2], "\n")

## Nearest point on plane:  -4.812744

Visualization

create_plane_quad <- function(vertices, normal, color) {

  # Repeat normal to match vertices
  normals <- matrix(rep(normal, nrow(vertices)), ncol = 3, byrow = TRUE)
  
  vertices_with_normals <- rbind(vertices, normals)

  quads <- matrix(c(1,2,3,4), ncol=4)

  shade3d(as.mesh3d(vertices_with_normals), quads=quads, col=color)
  
}

visualize_disk_and_plane <- function(disk_center, disk_radius, disk_orientation,
                                     plane_point, plane_normal, plane_orientation) {

  open3d()
  
  nearest_pts <- calculate_nearest_points(disk_center, disk_radius, disk_orientation,
                                          plane_point, plane_normal, plane_orientation)

  disk_nearest <- nearest_pts[1]
  plane_nearest <- nearest_pts[2]

  theta <- seq(0, 2*pi, length.out=100)
  phi <- seq(0, pi, length.out=100)
  
  x_disk <- disk_center[1] + disk_radius * outer(cos(theta), sin(phi))
  y_disk <- disk_center[2] + disk_radius * outer(sin(theta), sin(phi))
  z_disk <- disk_center[3] + disk_radius * outer(rep(1, 100), cos(phi))
  
  plot3d(x_disk, y_disk, z_disk, col="blue", size=0.5)

  plane_vertices <- rbind(c(-10, -10, 0), c(10, -10, 0),  
                          c(10, 10, 0), c(-10, 10, 0))
                          
  plane_normal <- c(0, 0, 1)
  
  create_plane_quad(plane_vertices, plane_normal, "green")  

  points3d(disk_nearest, col="red", size=0.5)
  points3d(plane_nearest, col="green", size=0.5)

view3d(theta = 30, phi = 30, zoom = 0.8)

# close3d()
}

visualize_disk_and_plane(disk_center, disk_radius, disk_orientation,
                         plane_point, plane_normal, plane_orientation)