Find the distance between any point and a Bézier curve (find the parameter t that is the closest point on the Bézier curve). reference: https://shikitenkai.blogspot.com/2024/11/bezier.html

.r

# %%
colors <- rainbow(length(1:5))
bezier_poly <- function(a, b, c, d, t) {
  return((1 - t)^3 * a
    + 3 * (1 - t)^2 * t * b
    + 3 * (1 - t) * t^2 * c
    + t^3 * d)
}
bezier_point <- function(ps, t) {
  x <- bezier_poly(ps[1, 1], ps[2, 1], ps[3, 1], ps[4, 1], t)
  y <- bezier_poly(ps[1, 2], ps[2, 2], ps[3, 2], ps[4, 2], t)
  return(c(x, y))
}
bezier_curve <- function(ps, n = 1000) {
  t <- seq(0, 1, length.out = n)
  b <- matrix(0, nrow = n, ncol = 3)
  for (i in 1:n) {
    p <- bezier_point(ps, t[i])
    b[i, 1] <- p[1]
    b[i, 2] <- p[2]
    b[i, 3] <- t[i]
  }
  return(b)
}
coeffs <- function(ps, qs) {
  # reference https://shikitenkai.blogspot.com/2024/11/bezier.html
  # reference https://shikitenkai.blogspot.com/2024/11/bezierqzp0t.html
  p0 <- ps[1, ] - qs[1, ]
  c0 <- ps[2, ] - qs[1, ]
  c1 <- ps[3, ] - qs[1, ]
  p1 <- ps[4, ] - qs[1, ]

  a <- p0[1]
  b <- -p0[1] + c0[1]
  c <- p0[1] - 2 * c0[1] + c1[1]
  d <- -p0[1] + 3 * c0[1] - 3 * c1[1] + p1[1]

  p <- p0[2]
  q <- -p0[2] + c0[2]
  r <- p0[2] - 2 * c0[2] + c1[2]
  s <- -p0[2] + 3 * c0[2] - 3 * c1[2] + p1[2]

  coeff0 <- a * b + p * q
  coeff1 <- 2 * a * c + 3 * b * b + 2 * p * r + 3 * q * q
  coeff2 <- a * d + 9 * b * c + p * s + 9 * q * r
  coeff3 <- 4 * b * d + 6 * c * c + 4 * q * s + 6 * r * r
  coeff4 <- 5 * c * d + 5 * r * s
  coeff5 <- d * d + s * s
  return(c(coeff0, coeff1, coeff2, coeff3, coeff4, coeff5))
}
solve <- function(ps, qs) {
  threshold <- 1E-7
  roots <- polyroot(coeffs(ps, qs))
  real_roots <- Re(roots[abs(Im(roots)) < threshold])
  for (i in seq_along(real_roots)) {
    t <- real_roots[i]
    real_roots[i] <- ifelse(t < 0, 0, ifelse(t > 1, 1, t))
  }
  return(real_roots)
}
ty5 <- function(c, n = 1000) {
  t <- seq(0, 1, length.out = n)
  ys <- matrix(0, nrow = n, ncol = 2)
  for (i in 1:n) {
    ys[i, 1] <- t[i]
    ys[i, 2] <- c[1] + c[2] * t[i] + c[3] * t[i]^2 + c[4] * t[i]^3 +
      c[5] * t[i]^4 + c[6] * t[i]^5
  }
  return(ys)
}
l_dash_points <- function(ps, qs) {
  return(ty5(coeffs(ps, qs)))
}

# plot
plot1 <- function(curve_points, real_roots, ps, qs, lwds) {
  par(mar = c(0, 6, 6, 5))
  plot(
    curve_points[, 1],
    curve_points[, 2],
    type = "l",
    lwd = 2,
    xlab = "X",
    ylab = "Y",
    asp = 1,
    xlim = c(-150, 450),
    ylim = c(250, 250),
    xaxt = "n",
    cex.lab = 3
  )
  axis(3)
  mtext(
    side = 3,
    text = "X",
    line = 2,
    cex = 2
  )
  grid()
  q <- qs[1, ]
  for (i in seq_along(real_roots)) {
    p <- bezier_point(ps, real_roots[i])
    lines(
      c(q[1], p[1]),
      c(q[2], p[2]),
      type = "l",
      col = colors[i],
      lwd = lwds[i]
    )
  }
  lines(c(ps[1, 1], ps[2, 1]),
    c(ps[1, 2], ps[2, 2]),
    type = "l",
    col = "gray"
  )
  lines(c(ps[4, 1], ps[3, 1]),
    c(ps[4, 2], ps[3, 2]),
    type = "l",
    col = "gray"
  )
  points(qs,
    col = "black",
    pch = 20,
    cex = 2
  )
  text(x = qs[1, 1] + 10, y = qs[1, 2] + 15, "点Q")
  for (i in 1:4) {
    points(
      x = c(ps[i, 1]),
      y = c(ps[i, 2]),
      col = "red",
      pch = 20,
      cex = 2
    )
    text(
      x = ps[i, 1] + 10,
      y = ps[i, 2] + 15,
      sprintf("点%s%d", ifelse(i == 1 || i == 4, "P", "C"), ifelse(i < 3, 0, 1))
    )
  }
}
plot2 <- function(curve_points, real_roots, ps, qs, lwds) {
  par(mar = c(0, 6, 1, 5))
  n <- nrow(curve_points)
  l_points <- matrix(0, nrow = n, ncol = 2)
  for (i in 1:n) {
    p <- curve_points[i, ]
    l_points[i, 1] <- p[3]
    dx <- p[1] - qs[1, 1]
    dy <- p[2] - qs[1, 2]
    l_points[i, 2] <- sqrt(dx * dx + dy * dy)
  }
  plot(
    l_points,
    type = "l",
    xlab = "t",
    ylab = "L:Bezier曲線と点Qとの距離",
    xaxt = "n",
    cex.lab = 1.5,
    lwd = 2,
  )
  for (i in seq_along(real_roots)) {
    abline(v = real_roots[i], col = colors[i], lwd = lwds[i])
  }
  abline(h = 0)
  grid()
}
plot3 <- function(curve_points, real_roots, ps, qs, lwds) {
  par(mar = c(6, 6, 1, 5))
  plot(
    l_dash_points(ps, qs),
    type = "l",
    xlab = "t",
    ylab = "L':Lの一階微分",
    cex.lab = 1.5,
    lwd = 2,
  )
  for (i in seq_along(real_roots)) {
    abline(v = real_roots[i], col = colors[i], lwd = lwds[i])
  }
  abline(h = 0)
  grid()
}
plots <- function(curve_points, real_roots, ps, qs, lwds) {
  layout(matrix(c(1, 2, 3), nr = 3, byrow = TRUE))
  par(family = "Moralerspace Krypton")
  plot1(curve_points, real_roots, ps, qs, lwds)
  plot2(curve_points, real_roots, ps, qs, lwds)
  plot3(curve_points, real_roots, ps, qs, lwds)
}
whichmin <- function(real_roots, ps, qs) {
  ls <- vector("numeric", length(real_roots))
  for (i in seq_along(real_roots)) {
    p <- bezier_point(ps, real_roots[i])
    delta <- p - qs[1, ]
    ls[i] <- sqrt(delta[1] * delta[1] + delta[2] * delta[2])
  }
  min_idx <- which.min(ls)
  # min_l <- ls[min_idx]
  min_t <- real_roots[min_idx]
  return(min_t)
}

g_q <- c(220, 220)
g_q <- c(150, 220)

g_p0 <- c(50, 150)
g_c0 <- c(450, 300)
g_c1 <- c(-150, 300)
g_p1 <- c(250, 150)

g_ps <- matrix(c(g_p0, g_c0, g_c1, g_p1), ncol = 2, byrow = TRUE)
g_qs <- matrix(c(g_q), ncol = 2, byrow = TRUE)
g_curve_points <- bezier_curve(ps)

g_real_roots <- solve(g_ps, g_qs)
g_min_t <- whichmin(g_real_roots, g_ps, g_qs)
g_lwds <- vector("integer", length(g_real_roots))
for (i in seq_along(g_real_roots)) {
  g_lwds[i] <- ifelse(g_real_roots[i] == g_min_t, 4, 2)
}

g_is_png <- TRUE
if (g_is_png) {
  png("output.png",
    width = 2480,
    height = 3508,
    res = 300
  )
  plots(g_curve_points, g_real_roots, g_ps, g_qs, g_lwds)
  dev.off()
} else {
  plots(g_curve_points, g_real_roots, g_ps, g_qs, g_lwds)
}
print(g_min_t)