Convert 2 3D Points to Directional Vectors to Euler Angles - algorithm

Here's essentially my problem. Also maybe I am not familiar enough with Euler angles and what I'm attempting to do is not possible.
I have 2 points in 3d space.
p1 (1,2,3)
p2 (4,5,6)
In order to get the unit vectors for these two points I'm doing this basically.
var productX = (position.X2 - position.X1);
var productY = (position.Y2 - position.Y1);
var productZ = (position.Z2 - position.Z1);
var normalizedTotal = Math.sqrt(productX * productX + productY * productY + productZ * productZ);
var unitVectorX, unitVectorY, unitVectorZ;
if(normalizedTotal == 0)
{
unitVectorX = productX;
unitVectorY = productY;
unitVectorZ = productZ;
}
else
{
unitVectorX = productX / normalizedTotal;
unitVectorY = productY / normalizedTotal;
unitVectorZ = productZ / normalizedTotal;
}
So now I have a unit vector x y z for these 2 3d points.
I'm attempting now to convert from directional vector to euler angle. Is this possible. What am I missing here as I can't find any good resource on how to do this.
Thanks for the help.
Sometimes a picture helps.
maybe this will give a better example of what i'm trying to solve for.
Given 2 points, I have determined a midpoint, length, and now i'm trying to figure out hte angles to set so that the cylinder is correctly oriented around the x,y,z axis. I think I need to figure out all 3 angles not just 1 and 2 is that correct? I think the euler angles from a directional vector bit through you off.

What you want is a transformation from Cartesian coordinates of the vector
v = (v_x, v_y, v_z)
to the spherical coordinates r, ψ and θ where
v = ( r*COS(ψ)*COS(θ), r*SIN(θ), r*SIN(ψ)*COS(θ) )
This is done with the following equations
r = SQRT(v_x^2+v_y^2+v_z^2)
TAN(ψ) = (v_z)/(v_x)
TAN(θ) = (v_y)/(v_x^2+v_z^2)
To get the angles ψ and θ, use the ATAN2(dy,dx) function as in
ψ = ATAN2(v_z, v_x)
θ = ATAN2(v_y, SQRT(v_x^2+v_z^2))
Now that you have the along direction vector
j = ( COS(ψ)*COS(θ), SIN(θ), SIN(ψ)*COS(θ) )
you can get the two perpendicular vectors from
i = ( SIN(ψ), 0, -COS(ψ) )
k = ( COS(ψ)*SIN(θ), -COS(θ), SIN(ψ)*SIN(θ) )
These three vectors make up the columns of the 3×3 rotation matrix
| SIN(ψ) COS(ψ)*COS(θ) COS(ψ)*SIN(θ) |
E =[i j k] = | 0 SIN(θ) -COS(θ) |
| -COS(ψ) SIN(ψ)*COS(θ) SIN(ψ)*SIN(θ) |
In terms of Euler angles the above is equivalent to
E = RY(π/2-ψ)*RX(π/2-θ)
Example
Two points p_1=(3,2,3) and p_2=(5,6,4) define the vector
v = (5,6,4) - (3,2,3) = (2,4,1)
NOTE: I am using the notation of v[i] for the i-th element of the vector, as in v[1]=2 above. This is neither like C, Python which is zero based, nor like VB, FORTRAN or MATLAB which uses parens () for the index.
Using the expressions above you get
r = √(2^2+4^2+1^2) = √21
TAN(ψ) = 1/2
TAN(θ) = 4/√(2^2+1^2) = 4/√5
ψ = ATAN2(1,2) = 0.463647
θ = ATAN2(4,√5) = 1.061057
Now to find the direction vectors
j = ( COS(ψ)*COS(θ), SIN(θ), SIN(ψ)*COS(θ) ) = (0.4364, 0.87287, 0.21822 )
i = ( SIN(ψ), 0, -COS(ψ) ) = (0.44721, 0, -0.89443 )
k = ( COS(ψ)*SIN(θ), -COS(θ), SIN(ψ)*SIN(θ) ) = (0.78072, -0.48795, 0.39036)
Put the direction vectors as columns of the local to world coordinate transformation (rotation)
E[1,1] = i[1] E[1,2] = j[1] E[1,3] = k[1]
E[2,1] = i[2] E[2,2] = j[2] E[2,3] = k[2]
E[3,1] = i[3] E[3,2] = j[3] E[3,3] = k[3]
| 0.447213595499957 0.436435780471984 0.780720058358826 |
| |
E = | 0 0.872871560943969 -0.487950036474266 |
| |
| -0.894427190999915 0.218217890235992 0.390360029179413 |

Related

How to vectorize this five-point difference code to fast calculate derivatives of a matrix?

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.

Implement Adaptive watershed segmentation in Matlab

I will like to implement "Adaptive Watershed Segmentation" in Matlab.
There are six steps in this algorithm. Input is figure(a) and result is figure(d).
Would you please to help me check is there any mistake in my code, and I don't know how to implement the sixth step.
Thank you so much!
Load image:
input_image = imread('test.gif');
Step 1 : Calculate D(x,y) at each (x,y), obtain the Euclidian distance map of the binary image and assign each value of M(x,y) as 0.
DT = bwdist(input_image,'euclidean'); % Trandform distance:Euclidian distance
[h,w]=size(DT);
M = zeros(h,w);
Step 2 : Smooth the distance map using Gaussian filter to merge the adjacent maxima, set M(x,y) as 1 if D(x,y) is a local maximum, and then obtain the marker map of the distance map.
H = fspecial('gaussian');
gfDT = imfilter(DT,H);
M = imregionalmax(gfDT); % maker map, M = local maximum of gfDT
Step3 : Scan the marker map pixel by pixel. If M(x0,y0) is 1, seek the spurious maxima in its neighbourhood with a radius of D(x ,y ).When M(x,y) equals 1 and sqr((x − x0)^2 + (y − y0)^2 ) ≤ D(x0, y0) , set M(x,y) as 0 if D(x,y) < D(x0,y0).
for x0 = 1:h
for y0 = 1:w
if M(x0,y0) == 1
r = ceil(gfDT(x0,y0));
% range begin:(x0-r,y0-r) end:(x0+r,y0+r)
xb = x0-r;
if xb <= 0
xb =1;
end
yb = y0-r;
if yb <= 0
yb =1;
end
xe = x0+r;
if xe > w
xe = w;
end
ye = y0+r;
if ye > h
ye = h;
end
for x = yb:ye
for y = xb:xe
if M(x,y)==1
Pos = [x0,y0 ;x,y];
Dis = pdist(Pos,'euclidean');
IFA = Dis<= (gfDT(x0,y0));
IFB = gfDT(x,y)<gfDT(x0,y0);
if ( IFA && IFB)
M(x,y) = 0;
end
end
end
end
end
end
end
Step 4:
Calculate the inverse of the distance map,and the local maxima turn out to be the local minima.
igfDT = -(gfDT);
STep5:
Segment the distance map according to the markers by the conventional watershed algorithm and obtain the segmentation of binary image.
I2 = imimposemin(igfDT,M);
L = watershed(I2);
igfDT (L==0)=0;
Step 6 : Straighten the watershed lines by linking the ends of the watershed lines with a straight line and reclassifying the pixels along the straight line.
I don't know how to implement this step
Try distance transform and then watershed transform.
im=imread('n6BRI.gif');
imb=bwdist(im);
sigma=3;
kernel = fspecial('gaussian',4*sigma+1,sigma);
im2=imfilter(imb,kernel,'symmetric');
L = watershed(max(im2(:))-im2);
[x,y]=find(L==0);
lblImg = bwlabel(L&~im);
figure,imshow(label2rgb(lblImg,'jet','k','shuffle'));

Implement a fast optimization algorithm using fixed point method in matlab

I am implementing a fast optimization algorithm using fixed point method in matlab. The goal of that method is that find optimal value of u. Denote u={u_i,i=1..2}. The optimal value of u can be obtained as following steps:
Sorry about my image because I cannot type mathematics equation in here.
To do that task, I tried to find u follows above steps. However, I don't know how to implement the term \sum_{j!=i} (u_j-1) in equation 25. This is my code. Please see it and could you give me some comment or suggestion about my implementation to correct them. Currently, I tried to run that code but it give an incorrect answer.
function u = compute_u_TV(Im0, N_class)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
theta=0.001;
gamma=0.01;
tau=0.1;
sigma=0.1;
N_class=2; % only have u1 and u2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Iterative segmentation process
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:N_class
v(:,:,i) = Im0/max(Im0(:)); % u between 0 and 1.
qxv(:,:,i) = zeros(size(Im0));
qyv(:,:,i) = zeros(size(Im0));
u(:,:,i) = v(:,:,i);
for iteration=1:10000
u_temp=u;
% Update v
Divqi = ( BackwardX(qxv(:,:,i)) + BackwardY(qyv(:,:,i)) );
Term = Divqi - u(:,:,i)/ (theta*gamma);
TermX = ForwardX(Term);
TermY = ForwardY(Term);
Norm = sqrt(TermX.^2 + TermY.^2);
Denom = 1 + tau*Norm;
%Equation 24
qxv(:,:,i) = (qxv(:,:,i) + tau*TermX)./Denom;
qyv(:,:,i) = (qyv(:,:,i) + tau*TermY)./Denom;
v(:,:,i) = u(:,:,i) - theta*gamma* Divqi; %Equation 23
% Update u
u(:,:,i) = (v(:,:,i) - theta* gamma* Divqi -theta*gamma*sigma*(sum(u(:))-u(:,:,i)-1))./(1+theta* gamma*sigma);
u(:,:,i) = max(u(:,:,i),0);
u(:,:,i) = min(u(:,:,i),1);
check=u_temp(:,:,i)-u(:,:,i);
if(abs(sum(check(:)))<=0.1)
break;
end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sub-functions- X.Berson
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dx]=BackwardX(u);
[Ny,Nx] = size(u);
dx = u;
dx(2:Ny-1,2:Nx-1)=( u(2:Ny-1,2:Nx-1) - u(2:Ny-1,1:Nx-2) );
dx(:,Nx) = -u(:,Nx-1);
function [dy]=BackwardY(u);
[Ny,Nx] = size(u);
dy = u;
dy(2:Ny-1,2:Nx-1)=( u(2:Ny-1,2:Nx-1) - u(1:Ny-2,2:Nx-1) );
dy(Ny,:) = -u(Ny-1,:);
function [dx]=ForwardX(u);
[Ny,Nx] = size(u);
dx = zeros(Ny,Nx);
dx(1:Ny-1,1:Nx-1)=( u(1:Ny-1,2:Nx) - u(1:Ny-1,1:Nx-1) );
function [dy]=ForwardY(u);
[Ny,Nx] = size(u);
dy = zeros(Ny,Nx);
dy(1:Ny-1,1:Nx-1)=( u(2:Ny,1:Nx-1) - u(1:Ny-1,1:Nx-1) );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of sub-function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
You should do
u(:,:,i) = (v(:,:,i) - theta* gamma* Divqi -theta*gamma*sigma* ...
(sum(u(:,:,1:size(u,3) ~= i),3) -1))./(1+theta* gamma*sigma);
The part you were searching for is
sum(u(:,:,1:size(u,3) ~= i),3)
Let's decompose this :
1:size(u,3) ~= i
is a vector containing all values from 1 to the max size of u on the third dimension except i.
Then
u(:,:,1:size(u,3) ~= i)
is all the matrix of the third dimension of u except for j = i
Finally,
sum(...,3)
is the sum of all the matrix by the thrid dimension.
Let me know if it does help!

Stick (line segments) percolation algorithm - graph theory?

I wrote an algorithm for study stick percolation (i.e.: networks between line segments that intersect between them). In my algorithm N sticks (line segments) are created inside a rectanglar box of sides 'b' and 'h' and then, one by one, the algorithm explores the intersection between all line segments. This is a Monte Carlo simulation, so the 'experiment' is executed many times (no less than 100 times). Writen like that, very much RAM is consumed:
array_x1=uniform.rvs(loc=-b/2, scale=b, size=N)
array_y1=uniform.rvs(loc=-h/2, scale=h, size=N)
array_x2=uniform.rvs(loc=-b/2, scale=b, size=N)
array_y2=uniform.rvs(loc=-h/2, scale=h, size=N)
M = np.zeros([N,N])
for u in xrange(100): ----> This '100' is the number of experiments.
for j in xrange(N):
if j>0:
x_A1B1 = array_x2[j]-array_x1[j]
y_A1B1 = array_y2[j]-array_y1[j]
x_A1A2 = array_x1[0:j]-array_x1[j]
y_A1A2 = array_y1[0:j]-array_y1[j]
x_A2A1 = -1*x_A1A2
y_A2A1 = -1*y_A1A2
x_A2B2 = array_x2[0:j]-array_x1[0:j]
y_A2B2 = array_y2[0:j]-array_y1[0:j]
x_A1B2 = array_x2[0:j]-array_x1[j]
y_A1B2 = array_y2[0:j]-array_y1[j]
x_A2B1 = array_x2[j]-array_x1[0:j]
y_A2B1 = array_y2[j]-array_y1[0:j]
p1 = x_A1B1*y_A1A2 - y_A1B1*x_A1A2
p2 = x_A1B1*y_A1B2 - y_A1B1*x_A1B2
p3 = x_A2B2*y_A2B1 - y_A2B2*x_A2B1
p4 = x_A2B2*y_A2A1 - y_A2B2*x_A2A1
condition_1=p1*p2
condition_2=p3*p4
for k in xrange (j):
if condicion_1[k]<=0 and condicion_2[k]<=0:
M[j,k]=1
if j+1<N+4:
x_A1B1 = array_x2[j]-array_x1[j]
y_A1B1 = array_y2[j]-array_y1[j]
x_A1A2 = array_x1[j+1:]-array_x1[j]
y_A1A2 = array_y1[j+1:]-array_y1[j]
x_A2A1 = -1*x_A1A2
y_A2A1 = -1*y_A1A2
x_A2B2 = array_x2[j+1:]-array_x1[j+1:]
y_A2B2 = array_y2[j+1:]-array_y1[j+1:]
x_A1B2 = array_x2[j+1:]-array_x1[j]
y_A1B2 = array_y2[j+1:]-array_y1[j]
x_A2B1 = array_x2[j]-array_x1[j+1:]
y_A2B1 = array_y2[j]-array_y1[j+1:]
p1 = x_A1B1*y_A1A2 - y_A1B1*x_A1A2
p2 = x_A1B1*y_A1B2 - y_A1B1*x_A1B2
p3 = x_A2B2*y_A2B1 - y_A2B2*x_A2B1
p4 = x_A2B2*y_A2A1 - y_A2B2*x_A2A1
condicion_1=p1*p2
condicion_2=p3*p4
for k in xrange (N-j-1):
if condicion_1[k]<=0 and condicion_2[k]<=0:
M[j,k+j+1]=1
Here, the element Mij=1 if stick i intersect stick j and Mij=0 if not.
How can i optimize my algorithm? Graph theory is usefull in this case? How?
Waiting for your answers.
Thanks a lot!
Best regards

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