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
Related
I am trying to calculate the partial first derivatives with respect to each of the two dimensions in a 2D matrix, i.e dF/dx and dF/dy, using the five point method. I have managed to do this successfully by looping over the points:
dF_dx = zeros(size(F));
dF_dy = zeros(size(F));
% Derivative with respect to y for each x value (Apply to all columns simultaneously)
dF_dy(2,1:(Nx-1)) = ( F(3,1:(Nx-1)) - F(1,1:(Nx-1)) )/(2*dy);
for m = 3:(Ny-2)
dF_dy(m,1:(Nx-1)) = ( F(m-2,1:(Nx-1)) - F(m+2,1:(Nx-1)) + 8*F(m+1,1:(Nx-1)) - 8*F(m-1,1:(Nx-1)) )/(12*dy);
end
dF_dy(Ny-1,1:(Nx-1)) = ( F(Ny,1:(Nx-1)) - F(Ny-2,1:(Nx-1)) )/(2*dy);
% Derivative with respect to x for each y value (Apply to all rows simultaneously)
dF_dx(2:(Ny-1),2) = ( F(2:(Ny-1),3) - F(2:(Ny-1),1) )/(2*dx);
for n = 3:(Nx-2)
dF_dx(2:(Ny-1),n) = ( F(2:(Ny-1),n-2) - F(2:(Ny-1),n+2) + 8*F(2:(Ny-1),n+1) - 8*F(2:(Ny-1),n-1) )/(12*dx);
end
dF_dx(2:(Ny-1),(Nx-1)) = ( F(2:(Ny-1),Nx) - F(2:(Ny-1),Nx-2) )/(2*dx);
Is there a clever Matlab way to vectorize this to make it much faster to execute? I have read several functions which seem like they might do the trick (such as diff(), circshift(), or maybe even kron() ), but am not sure how they might be used to solve this problem.
(My plan is to the implement this as a gpuArray later, if that is relevant for the solution).
Thank you!
1st Edit
By looking at the source code for gradient(), I was able to make the following version which is vectorized (i.e without the loop):
F = rand(5,8);
Nx = size(F,2);
Ny = size(F,1);
dx = 2;
dy = 3;
dF_dx = zeros(size(F));
dF_dy = zeros(size(F));
dF_dx(2:(Ny-1),3:Nx-2) = (F(2:(Ny-1),1:Nx-4) - F(2:(Ny-1),5:Nx) + 8*F(2:(Ny-1),4:Nx-1) - 8*F(2:(Ny-1),2:Nx-3))/(12*dx);
dF_dx(2:(Ny-1),2) = ( F(2:(Ny-1),3) - F(2:(Ny-1),1) )/(2*dx);
dF_dx(2:(Ny-1),(Nx-1)) = ( F(2:(Ny-1),Nx) - F(2:(Ny-1),Nx-2) )/(2*dx);
dF_dy(3:Ny-2,1:(Nx-1)) = (F(1:Ny-4,1:(Nx-1)) - F(5:Ny,1:(Nx-1)) + 8*F(4:Ny-1,1:(Nx-1)) - 8*F(2:Ny-3,1:(Nx-1)))/(12*dy);
dF_dy(2,1:(Nx-1)) = ( F(3,1:(Nx-1)) - F(1,1:(Nx-1)) )/(2*dy);
dF_dy((Ny-1),1:(Nx-1)) = ( F(Ny,1:(Nx-1)) - F(Ny-2,1:(Nx-1)) )/(2*dy);
Is there a way to do it without all the indexing (which I suspect is what is taking the most time)?
2nd Edit
I have now implemented Cris and chtz's suggestion and achieved this using a convolution with conv2(), as follows:
F = rand(500,600);
Nx = size(F,2);
Ny = size(F,1);
dx = 2;
dy = 3;
[dF_dx, dF_dy] = partial_derivatives(F, dx, dy, Nx, Ny)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dF_dx, dF_dy] = partial_derivatives(F, dx, dy, Nx, Ny)
kernx = [-1 8 0 -8 1]/(12*dx); % Convolution kernel for x dimension
kerny = [-1;8;0;-8;1]/(12*dy); % Convolution kernel for y dimension
%%%%%%%% Partial derivative across x dimension %%%%%%%%
dF_dx = conv2(F, kernx, 'same') ; % Internal mesh points (five-point method)
% Second and penultimate mesh points (two-point method)
dF_dx(2:(Ny-1),2) = ( F(2:(Ny-1),3) - F(2:(Ny-1),1) )/(2*dx);
dF_dx(2:(Ny-1),(Nx-1)) = ( F(2:(Ny-1),Nx) - F(2:(Ny-1),Nx-2) )/(2*dx);
dF_dx(:,[1 Nx]) = 0; % Set boundary conditions
dF_dx([1 Ny],:) = 0; %
%%%%%%%% Partial derivative across x dimension %%%%%%%%
dF_dy = conv2(F, kerny, 'same') ; % Internal mesh points (five-point method)
% Second and penultimate mesh points (two-point method)
dF_dy(2,1:(Nx-1)) = ( F(3,1:(Nx-1)) - F(1,1:(Nx-1)) )/(2*dy);
dF_dy((Ny-1),1:(Nx-1)) = ( F(Ny,1:(Nx-1)) - F(Ny-2,1:(Nx-1)) )/(2*dy);
dF_dy(:,Nx) = 0; % Set boundary conditions
dF_dy([1 Ny],:) = 0; %
end
As a gpuArray, this does not provide any noticable improvement over the vectorized version in the 1st Edit. Is there an obvious way to improve it? Thanks.
SOS i keep getting errors in the loop solving by finite difference method.
I either get the following error when i start with i = 2 : N :
diffusion: A(I,J): row index out of bounds; value 2 out of bound 1
error: called from
diffusion at line 37 column 10 % note line change due to edit!
or, I get the following error when i do i = 2 : N :
subscript indices must be either positive integers less than 2^31 or logicals
error: called from
diffusion at line 37 column 10 % note line change due to edit!
Please help
clear all; close all;
% mesh in space
dx = 0.1;
x = 0 : dx : 1;
% mesh in time
dt = 1 / 50;
t0 = 0;
tf = 10;
t = t0 : dt : tf;
% diffusivity
D = 0.5;
% number of nodes
N = 11;
% number of iterations
M = 10;
% initial conditions
if x <= .5 && x >= 0 % note, in octave, you don't need parentheses around the test expression
u0 = x;
elseif
u0 = 1-x;
endif
u = u0;
alpha = D * dt / (dx^2);
for j = 1 : M
for i = 1 : N
u(i, j+1) = u(i, j ) ...
+ alpha ...
* ( u(i-1, j) ...
+ u(i+1, j) ...
- 2 ...
* u(i, j) ...
) ;
end
u(N+1, j+1) = u(N+1, j) ...
+ alpha ...
* ( ...
u(N, j) ...
- 2 ...
* u(N+1, j) ...
+ u(N, j) ...
) ;
% boundary conditions
u(0, :) = u0;
u(1, :) = u1;
u1 = u0;
u0 = 0;
end
% exact solution with 14 terms
%k=14 % COMMENTED OUT
v = (4 / ((k * pi) .^ 2)) ...
* sin( (k * pi) / 2 ) ...
* sin( k * pi * x ) ...
* exp .^ (D * ((k * pi) ^ 2) * t) ;
exact = symsum( v, k, 1, 14 );
error = exact - u;
% plot stuff
plot( t, error );
xlabel( 'time' );
ylabel( 'error' );
legend( 't = 1 / 50' );
Have a look at the edited code I cleaned up for you above and study it.
Don't underestimate the importance of clean, readable code when hunting for bugs.
It will save you more time than it will cost. Especially a week from now when you will need to revisit this code and you will not remember at all what you were trying to do.
Now regarding your errors. (all line references are with respect to the cleaned up code above)
Scenario 1:
In line 29 you initialise u as a single value.
If you start your loop in line 35 starting with i = 2, then as soon as you try to do u(i, j+1), i.e. u(2,2) in the next line, octave will complain that you're trying to index the second row, in an array that so far only contains one row. (in fact, the same will apply for j at this point, since at this point you only have one column as well)
Scenario 2:
I assume the second scenario was a typo and you meant to say i = 1 : N.
If you start with i=1 in the loop, then have a look at line 38: you are trying to get element u(i-1, j), i.e. u(0,1). Therefore octave will complain that you're trying to get the zero element, but in octave arrays start from one and zero is not defined. Attempting to access any array with a zero will result in the error you see (try it in a terminal!).
UPDATE
Also, now that the code is clean, you can spot another bug, which octave helpfully warns you about if you try to run the code.
Look at line 26. There is NO condition in the elseif leg, so octave looks for the next statement as the test condition.
This means that the elseif condition will always succeed as long as the result of u0 = 1-x is non-zero.
This is clearly a bug. Either you forgot to put the condition for the elseif, or more likely, you probably just meant to say else, rather than elseif.
Actually i have two intersecting circles as specified in the figure
i want to find the area of each part separately using Monte carlo method in Matlab .
The code doesn't draw the rectangle or the circles correctly so
i guess what is wrong is my calculation for the x and y and i am not much aware about the geometry equations for solving it so i need help about the equations.
this is my code so far :
n=1000;
%supposing that a rectangle will contain both circles so :
% the mid point of the distance between 2 circles will be (0,6)
% then by adding the radius of the left and right circles the total distance
% will be 27 , 11 from the left and 16 from the right
% width of rectangle = 24
x=27.*rand(n-1)-11;
y=24.*rand(n-1)+2;
count=0;
for i=1:n
if((x(i))^2+(y(i))^2<=25 && (x(i))^2+(y(i)-12)^2<=100)
count=count+1;
figure(2);
plot(x(i),y(i),'b+')
hold on
elseif(~(x(i))^2+(y(i))^2<=25 &&(x(i))^2+(y(i)-12)^2<=100)
figure(2);
plot(x(i),y(i),'y+')
hold on
else
figure(2);
plot(x(i),y(i),'r+')
end
end
Here are the errors I found:
x = 27*rand(n,1)-5
y = 24*rand(n,1)-12
The rectangle extents were incorrect, and if you use rand(n-1) will give you a (n-1) by (n-1) matrix.
and
first If:
(x(i))^2+(y(i))^2<=25 && (x(i)-12)^2+(y(i))^2<=100
the center of the large circle is at x=12 not y=12
Second If:
~(x(i))^2+(y(i))^2<=25 &&(x(i)-12)^2+(y(i))^2<=100
This code can be improved by using logical indexing.
For example, using R, you could do (Matlab code is left as an excercise):
n = 10000
x = 27*runif(n)-5
y = 24*runif(n)-12
plot(x,y)
r = (x^2 + y^2)<=25 & ((x-12)^2 + y^2)<=100
g = (x^2 + y^2)<=25
b = ((x-12)^2 + y^2)<=100
points(x[g],y[g],col="green")
points(x[b],y[b],col="blue")
points(x[r],y[r],col="red")
which gives:
Here is my generic solution for any two circles (without any hardcoded value):
function [ P ] = circles_intersection_area( k1, k2, N )
%CIRCLES_INTERSECTION_AREA Summary...
% Adnan A.
x1 = k1(1);
y1 = k1(2);
r1 = k1(3);
x2 = k2(1);
y2 = k2(2);
r2 = k2(3);
if sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)) >= (r1 + r2)
% no intersection
P = 0;
return
end
% Wrapper rectangle config
a_min = x1 - r1 - 2*r2;
a_max = x1 + r1 + 2*r2;
b_min = y1 - r1 - 2*r2;
b_max = y1 + r1 + 2*r2;
% Monte Carlo algorithm
n = 0;
for i = 1:N
rand_x = unifrnd(a_min, a_max);
rand_y = unifrnd(b_min, b_max);
if sqrt((rand_x - x1)^2 + (rand_y - y1)^2) < r1 && sqrt((rand_x - x2)^2 + (rand_y - y2)^2) < r2
% is a point in the both of circles
n = n + 1;
plot(rand_x,rand_y, 'go-');
hold on;
else
plot(rand_x,rand_y, 'ko-');
hold on;
end
end
P = (a_max - a_min) * (b_max - b_min) * n / N;
end
Call it like: circles_intersection_area([-0.4,0,1], [0.4,0,1], 10000) where the first param is the first circle (x,y,r) and the second param is the second circle.
Without using For loop.
n = 100000;
data = rand(2,n);
data = data*2*30 - 30;
x = data(1,:);
y = data(2,:);
plot(x,y,'ro');
inside5 = find(x.^2 + y.^2 <=25);
hold on
plot (x(inside5),y(inside5),'bo');
hold on
inside12 = find(x.^2 + (y-12).^2<=144);
plot (x(inside12),y(inside12),'g');
hold on
insidefinal1 = find(x.^2 + y.^2 <=25 & x.^2 + (y-12).^2>=144);
insidefinal2 = find(x.^2 + y.^2 >=25 & x.^2 + (y-12).^2<=144);
% plot(x(insidefinal1),y(insidefinal1),'bo');
hold on
% plot(x(insidefinal2),y(insidefinal2),'ro');
insidefinal3 = find(x.^2 + y.^2 <=25 & x.^2 + (y-12).^2<=144);
% plot(x(insidefinal3),y(insidefinal3),'ro');
area1=(60^2)*(length(insidefinal1)/n);
area3=(60^2)*(length(insidefinal2)/n);
area2= (60^2)*(length(insidefinal3)/n);
I am building my first large-scale MATLAB program, and I've managed to write original vectorized code for everything so for until I came to trying to create an image representing vector density in stereographic projection. After a couple failed attempts I went to the Mathworks file exchange site and found an open source program which fits my needs courtesy of Malcolm Mclean. With a test matrix his function produces something like this:
And while this is almost exactly what I wanted, his code relies on a triply nested for-loop. On my workstation a test data matrix of size 25000x2 took 65 seconds in this section of code. This is unacceptable since I will be scaling up to a data matrices of size 500000x2 in my project.
So far I've been able to vectorize the innermost loop (which was the longest/worst loop), but I would like to continue and be rid of the loops entirely if possible. Here is Malcolm's original code that I need to vectorize:
dmap = zeros(height, width); % height, width: scalar with default value = 32
for ii = 0: height - 1 % 32 iterations of this loop
yi = limits(3) + ii * deltay + deltay/2; % limits(3) & deltay: scalars
for jj = 0 : width - 1 % 32 iterations of this loop
xi = limits(1) + jj * deltax + deltax/2; % limits(1) & deltax: scalars
dd = 0;
for kk = 1: length(x) % up to 500,000 iterations in this loop
dist2 = (x(kk) - xi)^2 + (y(kk) - yi)^2;
dd = dd + 1 / ( dist2 + fudge); % fudge is a scalar
end
dmap(ii+1,jj+1) = dd;
end
end
And here it is with the changes I've already made to the innermost loop (which was the biggest drain on efficiency). This cuts the time from 65 seconds down to 12 seconds on my machine for the same test matrix, which is better but still far slower than I would like.
dmap = zeros(height, width);
for ii = 0: height - 1
yi = limits(3) + ii * deltay + deltay/2;
for jj = 0 : width - 1
xi = limits(1) + jj * deltax + deltax/2;
dist2 = (x - xi) .^ 2 + (y - yi) .^ 2;
dmap(ii + 1, jj + 1) = sum(1 ./ (dist2 + fudge));
end
end
So my main question, are there any further changes I can make to optimize this code? Or even an alternative method to approach the problem? I've considered using C++ or F# instead of MATLAB for this section of the program, and I may do so if I cannot get to a reasonable efficiency level with the MATLAB code.
Please also note that at this point I don't have ANY additional toolboxes, if I did then I know this would be trivial (using hist3 from the statistics toolbox for example).
Mem consuming solution
yi = limits(3) + deltay * ( 1:height ) - .5 * deltay;
xi = limits(1) + deltax * ( 1:width ) - .5 * deltax;
dx = bsxfun( #minus, x(:), xi ) .^ 2;
dy = bsxfun( #minus, y(:), yi ) .^ 2;
dist2 = bsxfun( #plus, permute( dy, [2 3 1] ), permute( dx, [3 2 1] ) );
dmap = sum( 1./(dist2 + fudge ) , 3 );
EDIT
handling extremely large x and y by breaking the operation into blocks:
blockSize = 50000; % process up to XX elements at once
dmap = 0;
yi = limits(3) + deltay * ( 1:height ) - .5 * deltay;
xi = limits(1) + deltax * ( 1:width ) - .5 * deltax;
bi = 1;
while bi <= numel(x)
% take a block of x and y
bx = x( bi:min(end, bi + blockSize - 1) );
by = y( bi:min(end, bi + blockSize - 1) );
dx = bsxfun( #minus, bx(:), xi ) .^ 2;
dy = bsxfun( #minus, by(:), yi ) .^ 2;
dist2 = bsxfun( #plus, permute( dy, [2 3 1] ), permute( dx, [3 2 1] ) );
dmap = dmap + sum( 1./(dist2 + fudge ) , 3 );
bi = bi + blockSize;
end
This is a good example of why starting a loop from 1 matters. The only reason that ii and jj are initiated at 0 is to kill the ii * deltay and jj * deltax terms which however introduces sequentiality in the dmap indexing, preventing parallelization.
Now, by rewriting the loops you could use parfor() after opening a matlabpool:
dmap = zeros(height, width);
yi = limits(3) + deltay*(1:height) - .5*deltay;
matlabpool 8
parfor ii = 1: height
for jj = 1: width
xi = limits(1) + (jj-1) * deltax + deltax/2;
dist2 = (x - xi) .^ 2 + (y - yi(ii)) .^ 2;
dmap(ii, jj) = sum(1 ./ (dist2 + fudge));
end
end
matlabpool close
Keep in mind that opening and closing the pool has significant overhead (10 seconds on my Intel Core Duo T9300, vista 32 Matlab 2013a).
PS. I am not sure whether the inner loop instead of the outer one can be meaningfully parallelized. You can try to switch the parfor to the inner one and compare speeds (I would recommend going for the big matrix immediately since you are already running in 12 seconds and the overhead is almost as big).
Alternatively, this problem can be solved in using kernel density estimation techniques. This is part of the Statistics Toolbox, or there's this KDE implementation by Zdravko Botev (no toolboxes required).
For the example code below, I get 0.3 seconds for N = 500000, or 0.7 seconds for N = 1000000.
N = 500000;
data = [randn(N,2); rand(N,1)+3.5, randn(N,1);]; % 2 overlaid distrib
tic; [bandwidth,density,X,Y] = kde2d(data); toc;
imagesc(density);
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);
}
}
}