Bilinear interpolation with non-aligned input points - algorithm

I have a non-grid-aligned set of input values associated with grid-aligned output values. Given a new input value I want to find the output:
(These are X,Y coordinates, calibrating an imprecise not-square eye-tracking input device to exact locations on screen.)
This looks like Bilinear Interpolation, but my input values are not grid-aligned. Given an input, how can I figure out a reasonable output value?
Answer: In this case where I have sets of input and output points, what is actually needed is to perform inverse bilinear interpolation to find the U,V coordinates of the input point within the quad, and then perform normal bilinear interpolation (as described in Nico's answer below) on the output quad using those U,V coordinates.

You can bilinearly interpolate in any convex tetragon. A cartesian grid is just a bit simpler because the calculation of interpolation parameters is trivial. In the general case you interpolate as follows:
parameters alpha, beta
interpolated value = (1 - alpha) * ((1 - beta) * p1 + beta * p2) + alpha * ((1 - beta) * p3 + beta * p4)
In order to calculate the parameters, you have to solve a system of equations. Put your input values in the places of p1 through p4 and solve for alpha and beta.
Then put your output values in the places of p1 through p4 and use the calculated parameters to calculate the final interpolated output value.
For a regular grid, the parameter calculation comes down to:
alpha = x / cell width
beta = y / cell height
which automatically solves the equations.
Here is a sample interpolation for alpha=0.3 and beta=0.6
Actually, the equations can be solved analytically. However, the formulae are quite ugly. Therefore, iterative methods are probably nicer. There are two solutions for the system of equations. You need to pick the solution where both parameters are in [0, 1].
First solution:
alpha = -(b e - a f + d g - c h + sqrt(-4 (c e - a g) (d f - b h) +
(b e - a f + d g - c h)^2))/(2 c e - 2 a g)
beta = (b e - a f - d g + c h + sqrt(-4 (c e - a g) (d f - b h) +
(b e - a f + d g - c h)^2))/(2 c f - 2 b g)
where
a = -p1.x + p3.x
b = -p1.x + p2.x
c = p1.x - p2.x - p3.x + p4.x
d = interpolated_point.x - p1.x
e = -p1.y + p3.y
f = -p1.y + p2.y
g = p1.y - p2.y - p3.y + p4.y
h = interpolated_point.y - p1.y
Second solution:
alpha = (-b e + a f - d g + c h + sqrt(-4 (c e - a g) (d f - b h) +
(b e - a f + d g - c h)^2))/(2 c e - 2 a g)
beta = -((-b e + a f + d g - c h + sqrt(-4 (c e - a g) (d f - b h) +
(b e - a f + d g - c h)^2))/( 2 c f - 2 b g))

Here's my own technique, along with code for deriving the resulting value. It requires three lerps of the output values (and three percentage calculations to determine the lerp percentages):
Note that this is not bilinear interpolation. It does not remap the quad of input points to the quad of output values, as some input points can result in output values outside the output quad.
Here I'm showing the non-aligned input values on a Cartesian plane (using the sample input values from the question above, multiplied by 10 for simplicity).
To calculate the 'north' point (top green dot), we calculate the percentage across the X axis as
(inputX - northwestX) / (northeastX - northwestX)
= (-4.2 - -19) / (10 - -19)
= 0.51034
We use this percentage to calculate the intercept at the Y axis by lerping between the top Y values:
(targetValue - startValue) * percent + startValue
= (northeastY - northwestY) * percent + northwestY
= (-8 - -7) * 0.51034 + -7
= -7.51034
We do the same on the 'south' edge:
(inputX - southwestX) / (southeastX - southwestX)
= (-4.2 - -11) / (9 - -11)
= 0.34
(southeastY - southwestY) * percent + southwestY
= (7 - 4) * 0.34 + 4
= 5.02
Finally, we use these two values to calculate the final percentage between the north and south edges:
(inputY - southY) / (northY - southY)
= (1 - 5.02) / (-7.51034 - 5.02)
= 0.3208
With these three percentages in hand we can calculate our final output values by lerping between the points:
nw = Vector(-150,-100)
ne = Vector( 150,-100)
sw = Vector(-150, 100)
se = Vector( 150, 100)
north = lerp( nw, ne, 0.51034) --> ( 3.10, -100.00)
south = lerp( sw, se, 0.34) --> (-48.00, 100.00)
result = lerp( south, north, 0.3208) --> (-31.61, 35.84)
Finally, here is some (Lua) code performing the above. It uses a mutable Vector object that supports the ability to copy values from another vector and lerp its values towards another vector.
-- Creates a bilinear interpolator
-- Corners should be an object with nw/ne/sw/se keys,
-- each of which holds a pair of mutable Vectors
-- { nw={inp=vector1, out=vector2}, … }
function tetragonalBilinearInterpolator(corners)
local sides = {
n={ pt=Vector(), pts={corners.nw, corners.ne} },
s={ pt=Vector(), pts={corners.sw, corners.se} }
}
for _,side in pairs(sides) do
side.minX = side.pts[1].inp.x
side.diff = side.pts[2].inp.x - side.minX
end
-- Mutates the input vector to hold the result
return function(inpVector)
for _,side in pairs(sides) do
local pctX = (inpVector.x - side.minX) / side.diff
side.pt:copyFrom(side.pts[1].inp):lerp(side.pts[2].inp,pctX)
side.inpY = side.pt.y
side.pt:copyFrom(side.pts[1].out):lerp(side.pts[2].out,pctX)
end
local pctY = (inpVector.y-sides.s.inpY)/(sides.n.y-sides.s.inpY)
return inpVector:copyFrom(sides.s.pt):lerp(sides.n.pt,pctY)
end
end
local interp = tetragonalBilinearInterpolator{
nw={ inp=Vector(-19,-7), out=Vector(-150,-100) },
ne={ inp=Vector( 10,-8), out=Vector( 150,-100) },
sw={ inp=Vector(-11, 4), out=Vector(-150, 100) },
se={ inp=Vector( 9, 7), out=Vector( 150, 100) }
}
print(interp(Vector(-4.2, 1))) --> <-31.60 35.84>

Related

Finite difference method for solving the Klein-Gordon equation in Matlab

I am trying to numerically solve the Klein-Gordon equation that can be found here. To make sure I solved it correctly, I am comparing it with an analytical solution that can be found on the same link. I am using the finite difference method and Matlab. The initial spatial conditions are known, not the initial time conditions.
I start off by initializing the constants and the space-time coordinate system:
close all
clear
clc
%% Constant parameters
A = 2;
B = 3;
lambda = 2;
mu = 3;
a = 4;
b = - (lambda^2 / a^2) + mu^2;
%% Coordinate system
number_of_discrete_time_steps = 300;
t = linspace(0, 2, number_of_discrete_time_steps);
dt = t(2) - t(1);
number_of_discrete_space_steps = 100;
x = transpose( linspace(0, 1, number_of_discrete_space_steps) );
dx = x(2) - x(1);
Next, I define and plot the analitical solution:
%% Analitical solution
Wa = cos(lambda * x) * ( A * cos(mu * t) + B * sin(mu * t) );
figure('Name', 'Analitical solution');
surface(t, x, Wa, 'edgecolor', 'none');
colormap(jet(256));
colorbar;
xlabel('t');
ylabel('x');
title('Wa(x, t) - analitical solution');
The plot of the analytical solution is shown here.
In the end, I define the initial spatial conditions, execute the finite difference method algorithm and plot the solution:
%% Numerical solution
Wn = zeros(number_of_discrete_space_steps, number_of_discrete_time_steps);
Wn(1, :) = Wa(1, :);
Wn(2, :) = Wa(2, :);
for j = 2 : (number_of_discrete_time_steps - 1)
for i = 2 : (number_of_discrete_space_steps - 1)
Wn(i + 1, j) = dx^2 / a^2 ...
* ( ( Wn(i, j + 1) - 2 * Wn(i, j) + Wn(i, j - 1) ) / dt^2 + b * Wn(i - 1, j - 1) ) ...
+ 2 * Wn(i, j) - Wn(i - 1, j);
end
end
figure('Name', 'Numerical solution');
surface(t, x, Wn, 'edgecolor', 'none');
colormap(jet(256));
colorbar;
xlabel('t');
ylabel('x');
title('Wn(x, t) - numerical solution');
The plot of the numerical solution is shown here.
The two plotted graphs are not the same, which is proof that I did something wrong in the algorithm. The problem is, I can't find the errors. Please help me find them.
To summarize, please help me change the code so that the two plotted graphs become approximately the same. Thank you for your time.
The finite difference discretization of w_tt = a^2 * w_xx - b*w is
( w(i,j+1) - 2*w(i,j) + w(i,j-1) ) / dt^2
= a^2 * ( w(i+1,j) - 2*w(i,j) + w(i-1,j) ) / dx^2 - b*w(i,j)
In your order this gives the recursion equation
w(i,j+1) = dt^2 * ( (a/dx)^2 * ( w(i+1,j) - 2*w(i,j) + w(i-1,j) ) - b*w(i,j) )
+2*w(i,j) - w(i,j-1)
The stability condition is that at least a*dt/dx < 1. For the present parameters this is not satisfied, they give this ratio as 2.6. Increasing the time discretization to 1000 points is sufficient.
Next up is the boundary conditions. Besides the two leading columns for times 0 and dt one also needs to set the values at the boundaries for x=0 and x=1. Copy also them from the exact solution.
Wn(:,1:2) = Wa(:,1:2);
Wn(1,:)=Wa(1,:);
Wn(end,:)=Wa(end,:);
Then also correct the definition (and use) of b to that in the source
b = - (lambda^2 * a^2) + mu^2;
and the resulting numerical image looks identical to the analytical image in the color plot. The difference plot confirms the closeness

Mathematica Code with Module and If statement

Can I simply ask the logical flow of the below Mathematica code? What are the variables arg and abs doing? I have been searching for answers online and used ToMatlab but still cannot get the answer. Thank you.
Code:
PositiveCubicRoot[p_, q_, r_] :=
Module[{po3 = p/3, a, b, det, abs, arg},
b = ( po3^3 - po3 q/2 + r/2);
a = (-po3^2 + q/3);
det = a^3 + b^2;
If[det >= 0,
det = Power[Sqrt[det] - b, 1/3];
-po3 - a/det + det
,
(* evaluate real part, imaginary parts cancel anyway *)
abs = Sqrt[-a^3];
arg = ArcCos[-b/abs];
abs = Power[abs, 1/3];
abs = (abs - a/abs);
arg = -po3 + abs*Cos[arg/3]
]
]
abs and arg are being reused multiple times in the algorithm.
In a case where det > 0 the steps are
po3 = p/3;
b = (po3^3 - po3 q/2 + r/2);
a = (-po3^2 + q/3);
abs1 = Sqrt[-a^3];
arg1 = ArcCos[-b/abs1];
abs2 = Power[abs1, 1/3];
abs3 = (abs2 - a/abs2);
arg2 = -po3 + abs3*Cos[arg1/3]
abs3 can be identified as A in this answer: Using trig identity to a solve cubic equation
That is the most salient point of this answer.
Evaluating symbolically and numerically may provide some other insights.
Using demo inputs
{p, q, r} = {-2.52111798, -71.424692, -129.51520};
Copyable version of trig identity notes - NB a, b, p & q are used differently in this post
Plot[x^3 - 2.52111798 x^2 - 71.424692 x - 129.51520, {x, 0, 15}]
a = 1;
b = -2.52111798;
c = -71.424692;
d = -129.51520;
p = (3 a c - b^2)/3 a^2;
q = (2 b^3 - 9 a b c + 27 a^2 d)/27 a^3;
A = 2 Sqrt[-p/3]
A == abs3
-(b/3) + A Cos[1/3 ArcCos[
-((b/3)^3 - (b/3) c/2 + d/2)/Sqrt[-(-(b^2/9) + c/3)^3]]]
Edit
There is also a solution shown here
TRIGONOMETRIC SOLUTION TO THE CUBIC EQUATION, by Alvaro H. Salas
Clear[a, b, c]
1/3 (-a + 2 Sqrt[a^2 - 3 b] Cos[1/3 ArcCos[
(-2 a^3 + 9 a b - 27 c)/(2 (a^2 - 3 b)^(3/2))]]) /.
{a -> -2.52111798, b -> -71.424692, c -> -129.51520}
10.499

Using conditions to find imaginary and real part

I have used Solve to find the solution of an equation in Mathematica (The reason I am posting here is that no one could answer my question in mathematica stack.)The solution is called s and it is a function of two variables called v and ro. I want to find imaginary and real part of s and I want to use the information that v and ro are real and they are in the below interval:
$ 0.02 < ro < 1 ,
40
The code I used is as below:
ClearAll["Global`*"]
d = 1; l = 100; k = 0.001; kk = 0.001;ke = 0.0014;dd = 0.5 ; dr = 0.06; dc = 1000; p = Sqrt[8 (ro l /2 - 1)]/l^2;
m = (4 dr + ke^2 (d + dd)/2) (-k^2 + kk^2) (1 - l ro/2) (d - dd)/4 -
I v p k l (4 dr + ke^2 (d + dd)/2)/4 - v^2 ke^2/4 + I v k dr l p/4;
xr = 0.06/n;
tr = d/n;
dp = (x (v I kk/2 (4 dr + ke^2 (d + dd)/2) - I v kk ke^2 (d - dd)/8 - dr l p k kk (d - dd)/4) + y ((xr I kk (ro - 1/l) (4 dr + ke^2 (d + dd)/2)) - I v kk tr ke^2 (1/l - ro/2) + I dr xr 4 kk (1/l - ro/2)))/m;
a = -I v k dp/4 - I xr y kk p/2 + l ke^2 dp p (d + dd)/8 + (-d + dd)/4 k kk x + dr l p dp;
aa = -v I kk dp/4 + xr I y k p/2 - tr y ke^2 (1/l - ro/2) - (d - dd) x kk^2/4 + ke^2 x (d - dd)/8;
ca = CoefficientArrays[{x (s + ke^2 (d + dd)/2) +
dp (v I kk - l (d - dd) k p kk/2) + y (tr ro ke^2) - (d -
dd) ((-kk^2 + k^2) aa - 2 k kk a)/(4 dr + ke^2 (d + dd)/2) == 0, y (s + dc ke^2) + n x == 0}, {x, y}];
mat = Normal[ca];
matt = Last#mat;
sha = Solve[Det[matt] == 0, s];
shaa = Assuming[v < 100 && v > 40 && ro < 1 && ro > 0.03,Simplify[%]];
reals = Re[shaa];
ims = Im[shaa];
Solve[reals == 0, ro]
but it gives no answer. Could anyone help? I really appreciate any solution to this problem.
I run your code down to this point
mat = Normal[ca]
and look at the result.
There are lots of very tiny floating point coefficients, so small that I suspect most of them are just floating point noise now. Mathematica thinks 0.1 is only known to 1 significant digit of precision and your mat result is perhaps nothing more than zero correct digits now.
I continue down to this point
sha = Solve[Det[matt] == 0, s]
If you look at the value of sha you will see it is s->stuff and I don't think that is at all what you think it is. Mathematica returns "rules" from Solve, not just expressions.
If I change that line to
sha = s/.Solve[Det[matt] == 0, s]
then I am guessing that is closer to what you are imagining you want.
I continue to
shaa = Assuming[40<v<100 && .03<ro<1, Simplify[sha]];
reals = Re[shaa]
And I instead use, because you are assuming v and ro to be Real and because ComplexExpand has often been very helpful in getting Re to provide desired results,
reals=Re[ComplexExpand[shaa]]
and I click on Show ALL to see the full expanded value of that. That is about 32 large screens full of your expression.
In that are hundreds of
Arg[-1. + 50. ro]
and if I understand your intention I believe all those simplify to 0. If that is correct then
reals=reals/.Arg[-1. + 50. ro]->0
reduces the size of reals down to about 20 large screen fulls.
But there are still hundreds of examples of Sqrt[(-1.+50. ro)^2] and ((-1.+50. ro)^2)^(1/4) making up your reals. Unfortunately I'm expecting your enormous expression is too large and will take too long for Simplify with assumptions to be able to be practically effective.
Perhaps additional replacements to coax it into dramatically simplifying your reals without making any mistakes about Real versus Complex, but you have to be extremely careful with such things because it is very common for users to make mistakes when dealing with complex numbers and roots and powers and functions and end up with an incorrect result, might get your problem down to the point where it might be feasible for
Solve[reals == 0, ro]
to give you a meaningful answer.
This should give you some ideas of what you need to think carefully about and work on.

Quadratic Equation From Points

I need to implement a function that finds the trajectory of a projectile and I have three points - origin, destination and the point of maximum height.
I need to find the correct quadratic function that includes these points.
I'm having a hard time figuring out what to do. Where should I start?
Assume you have your origin and destination on the y-axis,namely x1 and x2. If not you can shift them later.
a*x*x + b*x + c = 0//equation
x1*x2=(c/a);
c = (x1*x2)*a;
x1+x2=(-b/a);
b = (x1+x2)/(-a);
a*((x1+x2)/2)^2 + b*((x1+x2)/2) + c = h//max height
let X=(x1+x2)/2;
a*X*X + ((2*X)/(-a))*X + (x1*x2)*a - h = 0;
Now you can iterate through a=0 until the above equation is true as you have all the values X ,x1 , x2 and h.
double eqn = (-h),a=0;//a=0.Assuming you have declared x1,x2 and X already
while(eqn!=0)
{
a++;
eqn = a*X*X + ((2*X)/(-a))*X + (x1*x2)*a - h;
}
b = (x1+x2)/(-a);
c = (x1*x2)*a;
Thus you got all your coeffecients.

Trilateration and locating the point (x,y,z)

I want to find the coordinate of an unknown node which lie somewhere in the space which has its reference distance away from 3 or more nodes which all of them have known coordinate.
This problem is exactly like Trilateration as described here Trilateration.
However, I don't understand the part about "Preliminary and final computations" (refer to the wikipedia site). I don't get where I could find P1, P2 and P3 just so I can put to those equation?
Thanks
Trilateration is the process of finding the center of the area of intersection of three spheres. The center point and radius of each of the three spheres must be known.
Let's consider your three example centerpoints P1 [-1,1], P2 [1,1], and P3 [-1,-1]. The first requirement is that P1' be at the origin, so let us adjust the points accordingly by adding an offset vector V [1,-1] to all three:
P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]
Note: Adjusted points are denoted by the ' (prime) annotation.
P2' must also lie on the x-axis. In this case it already does, so no adjustment is necessary.
We will assume the radius of each sphere to be 2.
Now we have 3 equations (given) and 3 unknowns (X, Y, Z of center-of-intersection point).
Solve for P4'x:
x = (r1^2 - r2^2 + d^2) / 2d //(d,0) are coords of P2'
x = (2^2 - 2^2 + 2^2) / 2*2
x = 1
Solve for P4'y:
y = (r1^2 - r3^2 + i^2 + j^2) / 2j - (i/j)x //(i,j) are coords of P3'
y = (2^2 - 2^2 + 0 + -2^2) / 2*-2 - 0
y = -1
Ignore z for 2D problems.
P4' = [1,-1]
Now we translate back to original coordinate space by subtracting the offset vector V:
P4 = P4' - V = [0,0]
The solution point, P4, lies at the origin as expected.
The second half of the article is describing a method of representing a set of points where P1 is not at the origin or P2 is not on the x-axis such that they fit those constraints. I prefer to think of it instead as a translation, but both methods will result in the same solution.
Edit: Rotating P2' to the x-axis
If P2' does not lie on the x-axis after translating P1 to the origin, we must perform a rotation on the view.
First, let's create some new vectors to use as an example:
P1 = [2,3]
P2 = [3,4]
P3 = [5,2]
Remember, we must first translate P1 to the origin. As always, the offset vector, V, is -P1. In this case, V = [-2,-3]
P1' = P1 + V = [2,3] + [-2,-3] = [0, 0]
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1]
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1]
To determine the angle of rotation, we must find the angle between P2' and [1,0] (the x-axis).
We can use the dot product equality:
A dot B = ||A|| ||B|| cos(theta)
When B is [1,0], this can be simplified: A dot B is always just the X component of A, and ||B|| (the magnitude of B) is always a multiplication by 1, and can therefore be ignored.
We now have Ax = ||A|| cos(theta), which we can rearrange to our final equation:
theta = acos(Ax / ||A||)
or in our case:
theta = acos(P2'x / ||P2'||)
We calculate the magnitude of P2' using ||A|| = sqrt(Ax + Ay + Az)
||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)
Plugging that in we can solve for theta
theta = acos(1 / sqrt(2)) = 45 degrees
Now let's use the rotation matrix to rotate the scene by -45 degrees.
Since P2'y is positive, and the rotation matrix rotates counter-clockwise, we'll use a negative rotation to align P2 to the x-axis (if P2'y is negative, don't negate theta).
R(theta) = [cos(theta) -sin(theta)]
[sin(theta) cos(theta)]
R(-45) = [cos(-45) -sin(-45)]
[sin(-45) cos(-45)]
We'll use double prime notation, '', to denote vectors which have been both translated and rotated.
P1'' = [0,0] (no need to calculate this one)
P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414]
[1 sin(-45) + 1 cos(-45)] = [0] = [0]
P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)] = [ 1.414]
[3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828]
Now you can use P1'', P2'', and P3'' to solve for P4''. Apply the reverse rotation to P4'' to get P4', then the reverse translation to get P4, your center point.
To undo the rotation, multiply P4'' by R(-theta), in this case R(45). To undo the translation, subtract the offset vector V, which is the same as adding P1 (assuming you used -P1 as your V originally).
This is the algorithm I use in a 3D printer firmware. It avoids rotating the coordinate system, but it may not be the best.
There are 2 solutions to the trilateration problem. To get the second one, replace "- sqrtf" by "+ sqrtf" in the quadratic equation solution.
Obviously you can use doubles instead of floats if you have enough processor power and memory.
// Primary parameters
float anchorA[3], anchorB[3], anchorC[3]; // XYZ coordinates of the anchors
// Derived parameters
float Da2, Db2, Dc2;
float Xab, Xbc, Xca;
float Yab, Ybc, Yca;
float Zab, Zbc, Zca;
float P, Q, R, P2, U, A;
...
inline float fsquare(float f) { return f * f; }
...
// Precompute the derived parameters - they don't change unless the anchor positions change.
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]);
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]);
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]);
Xab = anchorA[0] - anchorB[0];
Xbc = anchorB[0] - anchorC[0];
Xca = anchorC[0] - anchorA[0];
Yab = anchorA[1] - anchorB[1];
Ybc = anchorB[1] - anchorC[1];
Yca = anchorC[1] - anchorA[1];
Zab = anchorB[2] - anchorC[2];
Zbc = anchorB[2] - anchorC[2];
Zca = anchorC[2] - anchorA[2];
P = ( anchorB[0] * Yca
- anchorA[0] * anchorC[1]
+ anchorA[1] * anchorC[0]
- anchorB[1] * Xca
) * 2;
P2 = fsquare(P);
Q = ( anchorB[1] * Zca
- anchorA[1] * anchorC[2]
+ anchorA[2] * anchorC[1]
- anchorB[2] * Yca
) * 2;
R = - ( anchorB[0] * Zca
+ anchorA[0] * anchorC[2]
+ anchorA[2] * anchorC[0]
- anchorB[2] * Xca
) * 2;
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P);
A = (P2 + fsquare(Q) + fsquare(R)) * 2;
...
// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc)
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
// P, Q and R depend only on the anchor positions, so they are pre-computed
const float S = - Yab * (fsquare(Lc) - Dc2)
- Yca * (fsquare(Lb) - Db2)
- Ybc * (fsquare(La) - Da2);
const float T = - Xab * (fsquare(Lc) - Dc2)
+ Xca * (fsquare(Lb) - Db2)
+ Xbc * (fsquare(La) - Da2);
// Calculate quadratic equation coefficients
const float halfB = (S * Q) - (R * T) - U;
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2;
// Solve the quadratic equation for z
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;
// Substitute back for X and Y
float x = (Q * z + S)/P;
float y = (R * z + T)/P;
Here are the Wikipedia calculations, presented in an OpenSCAD script, which I think helps to understand the problem in a visual wayand provides an easy way to check that the results are correct. Example output from the script
// Trilateration example
// from Wikipedia
//
// pA, pB and pC are the centres of the spheres
// If necessary the spheres must be translated
// and rotated so that:
// -- all z values are 0
// -- pA is at the origin
pA = [0,0,0];
// -- pB is on the x axis
pB = [10,0,0];
pC = [9,7,0];
// rA , rB and rC are the radii of the spheres
rA = 9;
rB = 5;
rC = 7;
if ( pA != [0,0,0]){
echo ("ERROR: pA must be at the origin");
assert(false);
}
if ( (pB[2] !=0 ) || pC[2] !=0){
echo("ERROR: all sphere centers must be in z = 0 plane");
assert(false);
}
if (pB[1] != 0){
echo("pB centre must be on the x axis");
assert(false);
}
// show the spheres
module spheres(){
translate (pA){
sphere(r= rA, $fn = rA * 10);
}
translate(pB){
sphere(r = rB, $fn = rB * 10);
}
translate(pC){
sphere (r = rC, $fn = rC * 10);
}
}
function unit_vector( v) = v / norm(v);
ex = unit_vector(pB - pA) ;
echo(ex = ex);
i = ex * ( pC - pA);
echo (i = i);
ey = unit_vector(pC - pA - i * ex);
echo (ey = ey);
d = norm(pB - pA);
echo (d = d);
j = ey * ( pC - pA);
echo (j = j);
x = (pow(rA,2) - pow(rB,2) + pow(d,2)) / (2 * d);
echo( x = x);
// size of the cube to subtract to show
// the intersection of the spheres
cube_size = [10,10,10];
if ( ((d - rA) >= rB) || ( rB >= ( d + rA)) ){
echo ("Error Y not solvable");
}else{
y = (( pow(rA,2) - pow(rC,2) + pow(i,2) + pow(j,2)) / (2 * j))
- ( i / j) * x;
echo(y = y);
zpow2 = pow(rA,2) - pow(x,2) - pow(y,2);
if ( zpow2 < 0){
echo ("z not solvable");
}else{
z = sqrt(zpow2);
echo (z = z);
// subtract a cube with one of its corners
// at the point where the sphers intersect
difference(){
spheres();
translate ([x,y - cube_size[1],z]){
cube(cube_size);
}
}
translate ([x,y - cube_size[1],z]){
%cube(cube_size);
}
}
}

Resources