There's a task to approximate 4th point located inside of triangle of 3 points.
3 points, each with coordinates $[x, y, z]$ – find value for the 4th point $z$ coordinate – given its $[x, y]$ coordinates. The 4th point located inside of a triangle.
The solution – get equation of a plane from 3 points – and use it to get coordinate $z$ for 4th point (see code below).
The problem is – how to handle the degenerative cases when two or tree of 3 points are the same or dependent? In more or less compact and simple way?
function interpolate_3points(three_points, forth_point) {
const [a, b, c] = three_points
// Two vectors in plane i = a - b, j = a - c
const i = [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
const j = [a[0] - c[0], a[1] - c[1], a[2] - c[2]]
// Normal vector perpendicular to the plane n = i x j
const n = [
+ (i[1]*j[2] - i[2]*j[1]),
- (i[0]*j[2] - i[2]*j[0]),
+ (i[0]*j[1] - i[1]*j[0])
]
// Plane equation n1*x + n2*y + n3*z + k = 0
// Finding k by substituting values with any of the point from three points
const k = -(n[0]*a[0] + n[1]*a[1] + n[2]*a[2])
// Finding value of plane z in given [x, y] coordinates
// z = -(n1*x + n2*y + k)/n3
const v = forth_point
return -(n[0]*v[0] + n[1]*v[1] + k) / n[2]
}
console.log(interpolate_3points([[3, 3, 1], [5, 3, 2], [5, 5, 2]], [2, 3]))
// => 0.5 Good
console.log(interpolate_3points([[3, 3, 1], [3, 3, 1], [3, 3, 1]], [3, 3]))
// => NaN Bad
Best Answer
First, let's recap the problem. OP already knows this, we'll just repeat the actual math since not everyone can read the program code. (OP, feel free to add this to your question and remove from this answer, if you want.)
We have a system of four linear equations, $$\left\lbrace ~ \begin{aligned} n_x x_1 + n_y y_1 + n_z z_1 &= n_d \\ n_x x_2 + n_y y_2 + n_z z_2 &= n_d \\ n_x x_3 + n_y y_3 + n_z z_3 &= n_d \\ n_x x + n_y y + n_z z &= n_d \\ \end{aligned} \right .$$ where $(n_x, n_y, n_z)$ is the normal vector for the plane, $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, and $(x_3, y_3, z_3)$ are the three known coplanar points, and $(x, y, z)$ is the fourth point whose $z$ we are trying to solve in a robust manner. While there are technically five unknowns ($n_x$, $n_y$, $n_z$, $n_d$, and $z$), we are only interested in $z$.
The traditional method, and the one OP is using, is to find the plane normal using vector cross product – since all three edges of the triangle are perpendicular to the normal vector, cross product between any pair of edges yields a vector either parallel or opposite to the normal vector: $$\left[\begin{matrix} n_x \\ n_y \\ n_z \end{matrix}\right] = \left[\begin{matrix} x_2 - x_1 \\ y_2 - y_1 \\ z_2 - z_1 \end{matrix} \right] \times \left[\begin{matrix} x_3 - x_1 \\ y_3 - y_1 \\ z_3 - z_1 \end{matrix} \right]$$ and then finding $n_d$ trivially, as all points in the triangle fulfill the equation, $$n_d = \left[\begin{matrix} n_x \\ n_y \\ n_z \end{matrix}\right] \cdot \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right] = \left[\begin{matrix} n_x \\ n_y \\ n_z \end{matrix}\right] \cdot \left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right] = \left[\begin{matrix} n_x \\ n_y \\ n_z \end{matrix}\right] \cdot \left[\begin{matrix} x_3 \\ y_3 \\ z_3 \end{matrix}\right]$$ With these, the unknown $z$ is solved using $$z = \frac{n_d - n_x x - n_y y}{n_z}$$
The problem is with degenerate cases. When all four points are collinear, the cross product will yield a zero vector $(0, 0, 0)$, and thus leads to division by zero. (Also, if the plane is parallel to the $z$ axis, $n_z = 0$; then, any finite $z$ is a solution.)
One way is to treat this as purely a 2D problem, where $z$ is just the value of a linear function $z = f(x, y)$ that applies to all four points.
First, calculate $$\Delta = x_1 (y_2 - y_3) + x_2 (y_3 - y_1) + x_3 (y_1 - y_2)$$ whose magnitude is twice the area of the 2D triangle, and sign is positive if the three points are counterclockwise, negative if clockwise.
If $\lvert\Delta\rvert \le \epsilon$, where $\epsilon$ is the largest positive value you still consider logically zero (i.e. machine epsilon, typically on the order of the precision of your coordinates –, then the three points are collinear.
Otherwise, you can calculate $$\left\lbrace ~ \begin{aligned} C_x &= \displaystyle \frac{z_1 (y_2 - y_3) + z_2 (y_3 - y_1) + z_3 (y_1 - y_2)}{\Delta} \\ C_y &= \displaystyle \frac{z_1 (x_3 - x_2) + z_2 (x_1 - x_3) + z_3 (x_2 - x_1)}{\Delta} \\ \end{aligned}\right.$$ and the solution is $$z = z_1 + C_x ( x - x_1 ) + C_y ( y - y_1 )$$
Efficiency-wise, if you use six temporary variables for the differences between the three fixed points, this costs 2 divisions, 11 multiplications, 13 additions or subtractions, and one comparison.
Note that any further fourth points only cost two multiplications and four additions or subtractions, if you keep $C_x$, $C_y$, $x_1$, $y_1$, and $z_1$.
When the points are collinear, we switch to linear interpolation between them.
Then, you calculate squared distances, $$\begin{aligned} s_1 &= (x - x_1)^2 + (y - y_1)^2 \\ s_2 &= (x - x_2)^2 + (y - y_2)^2 \\ s_3 &= (x - x_3)^2 + (y - y_3)^3 \\ \end{aligned}$$ Pick $i$, $j$, and $k$ such that $s_i \ge s_j \ge s_k$.
If $s_k \le \epsilon^2$, return $z = z_k$. This takes care of the case where the partially known point has the same coordinates as one of the known points.
Finally, calculate $$\begin{aligned} r_i &= \sqrt{s_i} \\ r_k &= \sqrt{s_k} \\ z &= \displaystyle \frac{r_i z_k + r_k z_i}{r_i + r_k} \\ \end{aligned}$$ If all three points have the same coordinates, this returns the average of the two (furthest) ones.
This case uses a total of five to eight conditionals (if cases, including the sorting/ordering, depending on how you implement it), two square roots, one division, 11 multiplications, and 16 subtractions or additions.
More efficient implementations are possible with increased code complexity.
In Python:
Note that when the three points are collinear, and the given point is not on that line, this will return an answer that may not be suitable.
In general, I personally prefer an approach where you create a factory class that accepts three points as a parameter, and returns a function-like object (a closure) with precalculated $C_0$, $C_x$, and $C_y$, that returns $(x = x, y = y, z = C_0 + x C_x + y C_y)$ given $(x, y, z)$ as parameters (the $z$ ignored). (It could be used to calculate many points with respect to the same triangle, too.)
If the three points are not degenerate, $C_0 = n_d/n_z$, $C_x = -n_x/n_z$, and $C_y = -n_y/n_z$.
If the three points are collinear, the constants would be calculated based on the two outermost points.
If all three points have the same coordinates, then $C_0 = z_1 = z_2 = z_3$, $C_x = 0$, and $C_y = 0$.
You see, that same interface – closures that take a partially defined point, ignoring one of the Cartesian coordinates, and return a point on the plane defined by the internal scope of the closure – would also work if the coordinate system changes.
Another possible interface would be two-fold: one that constructs a 2D coordinate system $(u, v)$ based on the three given points, with those three points at locations $(0, 0)$, $(1,0)$, and $(0,1)$ (so the triangle edges are at $u = 0$, $v = 0$, and $u + v = 1$); and another that when given $(u, v)$ coordinates, returns the corresponding 3D coordinates. The scope in the first needs nine variables; six in the second.