Here is an illustration of the problem
I have 3 points in a 3d space, in my example the points are: (0, 0, 0), (26, 0, 0) and (13, 27, 0)
I then have a plane with 3 holes in it, with positions relative to the center of the plane, positions of those points are: (-0.5, -0.25, 0), (-0.1, -0.5, 0), (-0.2, 0.5, 0) and a point located 1 unit above the plane (so the relative position of this point to the plane is (0, 0, 1) )
The size of the plane is 1 x 1 if it were layed down on the x/y plane in the 3d space.
How can I find where in 3d space the center point of the plane is located, and the normal of the plane; such that lines can be drawn from all the 3 points through the holes in the plane and intersect at the point above the plane.
If this were in real life I could hold a piece of paper up to my eye and move it around until the 3 holes in the paper line up with the 3 points.
Best Answer
We have three points $P_1, P_2, P_3$ whose coordinates are specified in world coordinates. We also have a plane with a coordinate system $F$ attached to it, with unknown origin and unknown orientation. In addition, we have three points (holes) on the plane $q_1, q_2, q_3$ specified in the coordinate system attached to the plane. And finally we have a certain point $e_1 $ specified in the coordinate system attached to the plane.
The relation between the world coordinates and the coordinates in frame $F$ is
$ p = R q + d $
where $p$ is the vector in world coordinates, $q$ is the same vector but with coordinates expressed in frame $F$. Matrix $R$ is a rotation matrix and $d$ is the position of the origin of frame $F$ expressed in world coordinates.
From point $e_1$ to the three points $q_1, q_2, q_3$ we can draw three rays, and we want $P_1$ to lie on the ray $\vec{e_1 q_1}$ and $P_2$ to lie on the ray $e_1 q_2$ and $P_3$ to lie on the ray $\vec{e_1 q_3} $. The distances $a = \overline{P_1P_2}, b = \overline{P_1 P_3}, c = \overline{P_2 P_3} $ are the sides of $\triangle P_1 P_2 P_3 $
Hence, we can write
$ p_1 = e_1 + t_1 \vec{e_1 q_1} $
$ p_2 = e_1 + t_2 \vec{e_1 q_2 } $
$ p_3 = e_1 + t_3 \vec{e_1 q_3 } $
where $p_1 , p_2, p_3 $ are the points $P_1, P_2, P_3 $ but expressed in frame $F$.
Now we can write
$a^2 = ( t_1 \vec{e_1 q_1} - t_2 \vec{e_1 q_2} ) \cdot ( t_1 \vec{e_1 q_1} - t_2 \vec{e_1 q_2} )$
$ b^2 = ( t_1 \vec{e_1 q_1} - t_3 \vec{e_1 q_3} ) \cdot ( t_1 \vec{e_1 q_1} - t_3 \vec{e_1 q_3} )$
$ c^2 = ( t_2 \vec{e_1 q_2} - t_3 \vec{e_1 q_3} ) \cdot ( t_2 \vec{e_1 q_2} - t_3 \vec{e_1 q_3} )$
which is a quadratic system of three equations in three unknowns, and can be solved numerically, for $t_1, t_2, t_3 $.
Once $t_1, t_2, t_3$ are found, then $p_1, p_2, p_3$ are known. But
$p_1 = R^T (P_1 - d) $
$p_2 = R^T (P_2 - d) $
$p_3 = R^T (P_3 - d) $
Taking the differences
$p_1 - p_2 = R^T (P_1 - P_2)$
$p_1 - p_3 = R^T (P_1 - P_3)$
thus eliminating $d$. Now we can solve for $R$, because we know that
$(p_1 - p_2) \times (p_1 - p_3) = R^T \left( (P_1-P_2) \times (P_1 - P_3) \right)$
Therefore, we can now write
$ \begin{bmatrix} (P_1 - P_2) && (P_1 - P_3) && (P_1 - P_2) \times (P_1 - P_3) \end{bmatrix} = R \begin{bmatrix} (p_1 - p_2) && (p_1 - p_3) && (p_1 - p_2) \times (p_1 - p_3) \end{bmatrix} $
and we can solve for $R$ by matrix inversion.
Once $R$ is known we can solve for $d$ from (for example)
$P_1 = R p_1 + d $
or any of the other two equations involving $P_2, p_2$ or $P_3, p_3$.
Now the plane and the frame ($F$) attached to it are completely specified.
Now some numerical results. I will go through the steps to find a solution.
$\underline{\text{Step 1}}$: Find $\vec{e_1 q_1} , \vec{e_1 q_2 } , \vec{e_1 q_3 } $
$\vec{e_1 q_1} = q_1 - e_1 = (-0.5, -0.25, -1) $
$\vec{e_1 q_2} = q_2 - e_2 = (-0.1, -0.5, -1)$
$ \vec{e_1 q_3} = q_3 - e_1 = (-0.2, 0.5, -1) $
$\underline{\text{Step 2}}$: Find a,b,c
$ a = \overline{P_1 P_2} = 26 $
$ b = \overline{P_1 P_3} = \sqrt{898} $
$ c = \overline{P_2 P_3} = \sqrt{898} $
$\underline{\text{Step 3}}$: Write down the quadratic system in the parameters $t_1, t_2, t_3$, for example, the first such equation is
$a^2 = ( t_1 \vec{e_1 q_1} - t_2 \vec{e_1 q_2} ) \cdot ( t_1 \vec{e_1 q_1} - t_2 \vec{e_1 q_2} )$
Expanding the right hand side, this becomes
$ a^2 = t_1^2 \left( \vec{e_1 q_1}\cdot\vec{e_1 q_1} \right) + t_2^2 \left( \vec{e_1 q_2} \cdot \vec{e_1 q_2} \right) - 2 t_1 t_2 \left( \vec{e_1 q_1} \cdot \vec{e_1 q_2} \right) $
And we have
$\vec{e_1 q_1}\cdot\vec{e_1 q_1} = (-0.5)^2 + (-0.25)^2 + (-1)^2 = 1.3125 $
$\vec{e_1 q_2} \cdot \vec{e_1 q_2} = (-0.1)^2 + (-0.5)^2 +(-1)^2 = 1.26$
$\vec{e_1 q_1} \cdot \vec{e_1 q_2} = (-0.5)(-0.1) + (-0.25)(-0.5) + (-1)(-1) = 1.175 $
which completely specifies the first equation. The second and third equations can be built similarly.
Next, we have to solve these equations. And for that, we can use the multivariate Newton-Raphson method, or you can use an app such as this WolframAlpha Widget
$\underline{\text{Step 4}}$: From the solver, we get four solutions, let's take one of them: $t_1 = 6.137053, t_2 = 28.74539, t_3 = 30.70222$
Using these values for the $t$'s, we can compute the $p's$
$p_1 = e_1 + t_1 \vec{e_1 q_1} = (0, 0, 1) + 6.137053 (-0.5, -0.25, -1) = ( -3.06853, -1.53426 , -5.137053 ) $
$p_2 = e_1 + t_2 \vec{e_1 q_2} = (0, 0, 1) + 28.74539 (-0.1, -0.5, -1) = (-2.87454, -14.372695 , -27.74539 ) $
$ p_3 = e_1 + t_3 \vec{e_1 q_3} = (0, 0, 1) + 30.70222 (-0.2, 0.5, -1) = (-6.140444, 15.35111, -29.70222) $
$\underline{\text{Step 5}}$: Compute $(P_1 - P_2), (P_1 - P_3) $ and $(p_1 - p_2)$ , (p_1 - p_3) $
$P_1 - P_2 = (-26, 0, 0) $
$P_1 - P_3 = (-13, -27, 0 ) $
$p_1 - p_2 = (-0.19399, 12.838435, 22.608337) $
$p_1 - p_3 = (3.071914,-16.88537,24.565167)$
Next, compute
$(P_1 - P_2) \times (P_1 - P_3) = \begin{vmatrix} \mathbf{i} && \mathbf{j} && \mathbf{k} \\ -26 && 0 && 0 \\ -13 && -27 && 0 \end{vmatrix} = (0, 0, 702) $
and
$(p_1 - p_2) \times (p_1 - p_3) = \begin{vmatrix} \mathbf{i} && \mathbf{j} && \mathbf{k} \\ -0.19399&& 12.838435&&22.608337 \\ 3.071914 && -16.88537&&24.565167 \end{vmatrix} = ( 697.128435 , 74.21626 , -36.16297 ) $
And define the $A$ matrix as follows
$A = \begin{bmatrix} P_1 - P_2 && P_1 - P_3 && (P_1 - P_2) \times (P_1 - P_3 \end{bmatrix} $
That is,
$ A = \begin{bmatrix} -26 && -13 && 0 \\ 0 && -27 && 0 \\ 0 && 0 && 702 \end{bmatrix} $
and define matrix $B$ to be
$ B = \begin{bmatrix} p_1 - p_2 && p_1 - p_3 && (p_1 - p_2) \times (p_1 - p_3) \end{bmatrix} $
so that,
$B = \begin{bmatrix} -0.19399 && 3.071914 && 697.128435 \\ 12.838435 && -16.88537 && 74.21626 \\ 22.608337 && 24.565167 && -36.16297 \end{bmatrix}$
Now we have
$ A = R B $
so
$ R = A B^{-1} $
Inverting $B$ we get
$ B^{-1} = 10^{-3} \begin{bmatrix} -2.46043 && 34.97571 && 24.34895 \\ 4.346926 && - 31.967889 && 18.190676 \\ 1.414616 && 0.1506 && -0.073382 \end{bmatrix} $
Hence,
$R = A B^{-1} = \begin{bmatrix} 0.0074611 && -0.493786 && -0.8695515 \\ -0.117367 && 0.863133 && -0.491148 \\ 0.99306071 && 0.1057212 && -0.051514 \end{bmatrix} $
$\underline{\text{Step 6}}$: Now we want to compute the world coordinates of the origin of the frame $F$ that is attached to the plane, which is vector $d$. We have
$ P_1 = R p_1 + d $
from which
$ d = P_1 - R p_1 = (-5.20164, -1.55892, 2.94481 ) $
And finally the eye world coordinates are
$ E = d + R e_1 = (-6.07119, -2.05007, 2.893291)$
These are all the steps.
Finally, here is a summary of the results for the $4$ solutions of the triple $(t_1, t_2, t_3)$ that the solver found, and their corresponding $R, d$ and $E$. The first one is the one we explained in detail.
I. $ t_1 = 6.137053, t_2 = 28.74539, t_3 = 30.70222$
These values result in
$R = \begin{bmatrix} 0.00746105 && -0.493785883 && -0.869551514 \\ -0.117367085 && 0.863132973 && -0.491148083 \\ 0.993060572 && 0.105721207 && -0.051514331 \end{bmatrix} \hspace{25pt} d = \begin{bmatrix} -5.20164 \\ -1.55892 \\ 2.944805 \end{bmatrix} \hspace{25pt} E = \begin{bmatrix} -6.07119 \\ -2.05007 \\ 2.893291 \end{bmatrix} $
II. $t_1 = -39.1468, t_2 = -19.9873, t_3 = -33.1025 $
with these values, we get
$R = \begin{bmatrix} -0.675949507 && 0.007959405 && -0.736904955 \\ -0.154280656 && -0.979311947 && 0.130941165 \\ -0.720617613 && 0.202199796 && 0.66319341 \end{bmatrix} \hspace{25pt} d = \begin{bmatrix} 42.73715 \\ 7.347167 \\ -14.499 \end{bmatrix} \hspace{25pt} E = \begin{bmatrix} 42.00024 \\ 7.478108 \\ -13.8358 \end{bmatrix}$
III. $t_1 = -6.13705, t_2 = -28.7454, t_3 = -30.7022 $
These result in
$R = \begin{bmatrix} -0.00746105 && 0.493785883 && 0.869551514 \\ 0.117367085 && -0.863132973 && 0.491148083 \\ 0.993060572 && 0.105721207 && -0.051514331 \end{bmatrix} \hspace{25pt} d = \begin{bmatrix} -6.94074 \\ -2.54122 \\ -2.84178 \end{bmatrix} \hspace{25pt} E = \begin{bmatrix} -6.07119 \\ -2.05007 \\ -2.89329 \end{bmatrix}$
IV. $ t_1 = 39.14684 , t_2 = 19.98731, t_3 = 33.10248$
The corresponding $R, d, E$ are
$R = \begin{bmatrix} 0.675949507 && -0.007959405 && 0.736904955 \\ 0.154280656 && 0.979311947 && -0.130941165 \\ -0.720617613 && 0.202199796 && 0.66319341 \end{bmatrix} \hspace{25pt} d = \begin{bmatrix} 41.26334 \\ 7.60905 \\ 13.17265 \end{bmatrix} \hspace{25pt} E = \begin{bmatrix} 42.00024 \\ 7.478108 \\ 13.83584 \end{bmatrix} $