I want to find the closest intersection point to multiple lines/vectors in 3D space but I want to weight the influence of each vector based on its length (the greater its length, the greater its influence). I am working with R. The answer to this question works perfectly to find the closest intersection point to multiple vectors, but lack a weighted component. Where do I need to insert my weights matrix ?
This (4th slide) suggests where, but I didn't succeed to find where this part could be in Dan's code (I use linear algebra way too rarely to fully understand what's going on).
Theory on line-line intersection can be find in this answer and on Wikipedia.
Here is my translation of Dan's MATLAB code in R:
XYZ_intersect <- function(XYZ_start, XYZ_end)
{
# XYZ_start: matrix n x 3 containing starting XYZ coordinates
# XYZ_end: matrix n x 3 containing ending XYZ coordinates
direction_vectors <- XYZ_end - XYZ_start
length_vectors <- matrix(sqrt(.rowSums(direction_vectors^2, dim(direction_vectors)[1], 3)))
normalized_vectors <- direction_vectors / (length_vectors %*% matrix(1, ncol = 3))
weights <- length_vectors / sum(length_vectors)
Nx <- normalized_vectors[,1, drop = FALSE]
Ny <- normalized_vectors[,2, drop = FALSE]
Nz <- normalized_vectors[,3, drop = FALSE]
Sxx <- sum(Nx^2 - 1)
Syy <- sum(Ny^2 - 1)
Szz <- sum(Nz^2 - 1)
Sxy <- sum(Nx*Ny)
Sxz <- sum(Nx*Nz)
Syz <- sum(Ny*Nz)
symmetric <- matrix(c(Sxx,Sxy,Sxz,Sxy,Syy,Syz,Sxz,Syz,Szz), nrow = 3)
Cx <- sum(XYZ_start[,1, drop = FALSE]*(Nx^2 - 1) + XYZ_start[,2, drop = FALSE]*(Nx*Ny) + XYZ_start[,3, drop = FALSE]*(Nx*Nz))
Cy <- sum(XYZ_start[,1, drop = FALSE]*(Nx*Ny) + XYZ_start[,2, drop = FALSE]*(Ny^2 - 1) + XYZ_start[,3, drop = FALSE]*(Ny*Nz))
Cz <- sum(XYZ_start[,1, drop = FALSE]*(Nx*Nz) + XYZ_start[,2, drop = FALSE]*(Ny*Nz) + XYZ_start[,3, drop = FALSE]*(Nz^2 - 1))
C <- matrix(c(Cx,Cy,Cz))
XYZ_intersect <- t(solve(symmetric,C))
return(XYZ_intersect)
}
So if I have four vectors described as:
# (X0,Y0,Z0,X1,Y1,Z1)
vec1 <- c(0,0,1,3,3,6)
vec2 <- c(3,0,2,3,4,8)
vec3 <- c(0,3,3,4,3,9)
vec4 <- c(2,3,2,4,3,7)
XYZ_start <- matrix(c(vec1[1:3], vec2[1:3], vec3[1:3], vec4[1:3]), ncol = 3, byrow = TRUE)
XYZ_end <- matrix(c(vec1[4:6], vec2[4:6], vec3[4:6], vec4[4:6]), ncol = 3, byrow = TRUE)
XYZ_intersect(XYZ_start, XYZ_end)
# [,1] [,2] [,3]
#[1,] 3.132048 3.063237 6.522456
In this case, I would want to give the following weights:
weights
# [,1]
#[1,] 0.2487194
#[2,] 0.2735124
#[3,] 0.2735124
#[4,] 0.2042558
Finally, someone in flesh and blood helped me. The weights matrix has to be add inside each sum:
XYZ_intersect <- function(XYZ_start, XYZ_end)
{
# XYZ_start: matrix n x 3 containing starting XYZ coordinates
# XYZ_end: matrix n x 3 containing ending XYZ coordinates
direction_vectors <- XYZ_end - XYZ_start
length_vectors <- matrix(sqrt(.rowSums(direction_vectors^2, dim(direction_vectors)[1], 3)))
normalized_vectors <- direction_vectors / (length_vectors %*% matrix(1, ncol = 3))
weights <- length_vectors / sum(length_vectors)
Nx <- normalized_vectors[,1, drop = FALSE]
Ny <- normalized_vectors[,2, drop = FALSE]
Nz <- normalized_vectors[,3, drop = FALSE]
Wxx <- weights * (Nx^2 - 1)
Wyy <- weights * (Ny^2 - 1)
Wzz <- weights * (Nz^2 - 1)
Wxy <- weights * Nx * Ny
Wxz <- weights * Nx * Nz
Wyz <- weights * Ny * Nz
Sxx <- sum(Wxx)
Syy <- sum(Wyy)
Szz <- sum(Wzz)
Sxy <- sum(Wxy)
Sxz <- sum(Wxz)
Syz <- sum(Wyz)
symmetric <- matrix(c(Sxx,Sxy,Sxz,Sxy,Syy,Syz,Sxz,Syz,Szz), nrow = 3)
Cx <- sum(XYZ_start[,1, drop = FALSE] * Wxx + XYZ_start[,2, drop = FALSE] * Wxy + XYZ_start[,3, drop = FALSE] * Wxz)
Cy <- sum(XYZ_start[,1, drop = FALSE] * Wxy + XYZ_start[,2, drop = FALSE] * Wyy + XYZ_start[,3, drop = FALSE] * Wyz)
Cz <- sum(XYZ_start[,1, drop = FALSE] * Wxz + XYZ_start[,2, drop = FALSE] * Wyz + XYZ_start[,3, drop = FALSE] * Wzz)
C <- matrix(c(Cx,Cy,Cz))
XYZ_intersect <- t(solve(symmetric,C))
return(XYZ_intersect)
}
So with my four vectors, the result is:
XYZ_intersect(XYZ_start, XYZ_end)
# [,1] [,2] [,3]
#[1,] 3.092853 3.069695 6.556273