Point closest to a set four of lines in 3D
Solution 1:
As pointed out by the others, minimising the sum of distances is different from minimising the sum of squared distances. The former requires the use of an optimisation package. The latter is a least square problem that is fairly well studied with free solvers in abundance. Specifically:
- Let each straight line, say $L_i$, be specfied by two points $v_i$ and $w_i$ on it.
- Compute the unit vector $u_i = (v_i-w_i)/\Vert v_i-w_i\Vert$. This is the direction vector of $L_i$. The projection matrix $I-u_iu_i^T$ will project every vector $x\in\mathbb{R}^3$ to the plane $P_i$ that passes through the origin and orthogonal to $L_i$.
- In particular, the intersection of $L_i$ and $P_i$ is given by $p_i = (I-u_iu_i^T)w_i$ and the distance from a point $x\in\mathbb{R}^3$ to the line $L_i$ is $\Vert (I-u_iu_i^T)x - p_i\Vert$.
- Hence the sum of squared distances is $\sum_{i=1}^4 \Vert (I-u_iu_i^T)x - p_i\Vert^2$, which can be expanded into $x^T Ax - 2b^T x + c$ with $A = \sum_i(I-u_iu_i^T)$ (a 3x3 matrix), $b = \sum_i p_i$ (a 3-vector) and $c=\sum_i p_i^Tp_i$ (a scalar).
- Now come the pretty standard stuffs. The minimiser of $x^T Ax - 2b^T x + c$ is the least square solution to $Ax=b$, which in theory is $x = A^+ b$, where $A^+$ denotes the Moore-Penrose pseudoinverse of $A$.
- In practice, $A$ is usually nonsingular; in this case, $A^+$ is just $A^{-1}$. If you are sure that $A$ is nonsingular, you may simply invert the matrix and calculate $x$ as $A^{-1}b$ or (for better numerical stability) solve the equation by QR factorisation.
- However, $A$ can at times be singular or nearly singular. If you really want to play safe, you may solve the least square problem by applying singular value decomposition (SVD) or by using the conjugate gradient method. For C++ users, the Eigen3 library already contains a SVD solver. See the section "Least squares solving" in the official tutorial.
Solution 2:
I made a program in Mathematica for calculating the point coordinates.
The result is a large algebraic formula. I uploaded it to ideone
for you.
Here is the program, in case you have Mathematica at hand:
(*Load package*)
Needs["VectorAnalysis`"]
(*Define four lines, by specifying 2 points in each one*)
Table[p[i, j] = {x[i, j], y[i, j], z[i, j]}, {i, 4}, {j, 2}];
(*Define the target point*)
p0 = {x0, y0, z0};
(*Define a Norm function // using Std norm squared here*)
norm[a_] := a[[1]]^2 + a[[2]]^2 + a[[3]]^2
(*Define a function for the distance from line i to point v
used http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html (11) *)
d[i_, v_] := norm[Cross[(v - p[i, 1]), (v - p[i, 2])]]/norm[p[i, 2] - p[i, 1]]
(*Define a function for the sum of distances*)
dt[p_] := Sum[d[i, p], {i, 4}]
(*Now take the gradient, and Solve for Gradient == 0*)
s = Solve[Grad[dt[p0], Cartesian[x0, y0, z0]] == 0, {x0, y0, z0}]
(* Result tooooo long. Here you have it for downloading
http://ideone.com/XwbJu *)
RESULT