I need to split trapezoid in 2 part of given size with line, parallel basement. I need to get new h1 of new trapezoid.
For example I have trapezoid of area S and I want to split it in 2 trapezoids of areas S1 and S2.
S1 = aS; S2 = (1-a)S;
S1 = (a+z)*(h1)/2;
S2 = (b+z)*(1-h1)/2;
S1/S2 = KS;
To get new h1 I compare a and b, if a != b, I solve square equation and if a == b I work like with square. But sometimes I get mistakes because of rounding (for example when I solve this analytically I get a = b and program thinks a > b). How can I handle this? Or maybe there is another better way to split trapezoid?
Here is simplifyed code:
if (base > base_prev) {
b_t = base; // base of trapezoid
h = H; //height of trapezoid
a_t = base_prev; //another base of trapezoid
KS = S1 / S2;
a_x = (a_t - b_t) * (1 + KS) / h;
b_x = 2 * KS * b_t + 2 * b_t;
c_x = -(a_t * h + b_t * h);
h_tmp = (-b_x + sqrt(b_x * b_x - 4 * a_x * c_x)) / (2 * a_x);
if (h_tmp > h || h_tmp < 0)
h_tmp = (-b_x - sqrt(b_x * b_x - 4 * a_x * c_x)) / (2 * a_x);
} else if (base < base_prev) {
b_t = base_prev;
a_t = base;
KS = S1 / S2;
a_x = (a_t - b_t) * (1 + KS) / h;
b_x = 2 * KS * b_t + 2 * b_t;
c_x = -(a_t * h + b_t * h);
h_tmp = (-b_x + sqrt(b_x * b_x - 4 * a_x * c_x)) / (2 * a_x);
if (h_tmp > h || h_tmp < 0)
h_tmp = (-b_x - sqrt(b_x * b_x - 4 * a_x * c_x)) / (2 * a_x);
}
else {
KS = S1 / S2;
h_tmp = h * KS;
}
If you're dealing with catastrophic cancellation, one approach, dating back to a classic article by Forsythe, is to use the alternative solution form x = 2c/(-b -+ sqrt(b^2 - 4ac)) for the quadratic equation ax^2 + bx + c = 0. One way to write the two roots, good for b < 0, is
x = (-b + sqrt(b^2 - 4ac))/(2a)
x = 2c/(-b + sqrt(b^2 - 4ac)),
and another, good for b >= 0, is
x = 2c/(-b - sqrt(b^2 - 4ac))
x = (-b - sqrt(b^2 - 4ac))/(2a).
Alternatively, you could use the bisection method to obtain a reasonably good guess and polish it with Newton's method.
Related
I've got this code:
const a = 2; // always > 0 and known in advance
const b = 3; // always > 0 and known in advance
const c = 4; // always > 0 and known in advance
for (let x = 0; x <= a; x++) {
for (let y = 0; y <= b; y++) {
for (let z = 0; z <= c; z++) {
for (let p = 0; p <= 1; p++) {
for (let q = 0; q <= 2; q++) {
let u = b + x - y + p;
let v = a + b + 2 * c - x - y - 2 * z + q;
let w = c + x + y - z;
}
}
}
}
}
The code generates (a+1)*(b+1)*(c+1)*2*3 triplets of (u,v,w), each of them is unique. And because of that fact, I think it should be possible to write reversed version of this algorithm that will calculate x,y,z,p,q based on u,v,w. I understand that there are only 3 equations and 5 variables to get, but known boundaries for x,y,z,p,q and the fact that all variables are integers should probably help.
for (let u = ?; u <= ?; u++) {
for (let v = ?; v <= ?; v++) {
for (let w = ?; w <= ?; w++) {
x = ?;
y = ?;
z = ?;
p = ?;
q = ?;
}
}
}
I even managed to produce the first line: for (let u = 0; u <= a + b + 1; u++) by taking the equation for u and finding min and max but I'm struggling with moving forward. I understand that min and max values for v are depending on u, but can't figure out the formulas.
Examples are in JS, but I will be thankful for any help in any programming language or even plain math formulas.
If anyone is interested in what this code is actually about - it projects voxel 3d model to triangles on a plain. u,v are resulting 2d coordinates and w is distance from the camera. Reversed algorithm will be actually a kind of raytracing.
UPDATE: Using line equations from 2 points I managed to create minmax conditions for v and code now looks like this:
for (let u = 0; u <= a + b + 1; u++) {
let minv = u <= a ? a - u : -a + u - 1;
let maxv = u <= b ? a + 2 * c + u + 2 : a + 2 * b + 2 * c - u + 3;
for (let v = minv; v <= maxv; v++) {
...
}
}
I think I know what to do with x, y, z, p, q on the last step so the problem left is minw and maxw. As far as I understand those values should depend both on u and v and I must use plane equations?
If the triplets are really unique (didn't check that) and if p and q always go up to 1 and 2 (respectively), then you can "group" triplets together and go up the loop chain.
We'll first find the 3 triplets that where made in the same "q loop" : the triplets make with the same x,y,z,p. As only q change, the only difference will be v, and it will be 3 consecutive numbers.
For that, let's group triplets such that, in a group, all triplets have the same u and same w. Then we sort triplets in groups by their v parameters, and we group them 3 by 3. Inside each group it's easy to assign the correct q variable to each triplet.
Then reduce the groups of 3 into the first triplet (the one with q == 0). We start over to assign the p variable : Group all triplets such that they have same v and w inside a group. Then sort them by the u value, and group them 2 by 2. This let's us find their p value. Remember that each triplet in the group of 3 (before reducing) has that same p value.
Then, for each triplet, we have found p and q. We solve the 3 equation for x,y,z :
z = -1 * ((v + w) - a - b - 3c -q)/3
y = (w - u + z + b - c - p)/2
x = u + y - b - p
After spending some time with articles on geometry and with the huge help from Wolfram Alpha, I managed to write needed equations myself. And yes, I had to use plane equations.
const a = 2; // always > 0 and known in advance
const b = 3; // always > 0 and known in advance
const c = 4; // always > 0 and known in advance
const minu = 0;
const maxu = a + b + 1;
let minv, maxv, minw, maxw;
let x, y, z, p, q;
for (let u = minu; u <= maxu; u++) {
if (u <= a) {
minv = a - u;
} else {
minv = -a + u - 1;
}
if (u <= b) {
maxv = a + 2 * c + u + 2;
} else {
maxv = a + 2 * b + 2 * c - u + 3;
}
for (let v = minv; v <= maxv; v++) {
if (u <= b && v >= a + u + 1) {
minw = (-a + 2 * b - 3 * u + v - 2) / 2;
} else if (u > b && v >= a + 2 * b - u + 2) {
minw = (-a - 4 * b + 3 * u + v - 5) / 2;
} else {
minw = a + b - v;
}
if (u <= a && v <= a + 2 * c - u + 1) {
maxw = (-a + 2 * b + 3 * u + v - 1) / 2;
} else if (u > a && v <= -a + 2 * c + u) {
maxw = (5 * a + 2 * b - 3 * u + v + 2) / 2;
} else {
maxw = a + b + 3 * c - v + 2;
}
minw = Math.round(minw);
maxw = Math.round(maxw);
for (let w = minw; w <= maxw; w++) {
z = (a + b + 3 * c - v - w + 2) / 3;
q = Math.round(2 - (z % 1) * 3);
z = Math.floor(z);
y = (a + 4 * b + q - 3 * u - v + 2 * w + 3) / 6;
p = 1 - (y % 1) * 2;
y = Math.floor(y);
x = (a - 2 * b - 3 * p + q + 3 * u - v + 2 * w) / 6;
x = Math.round(x);
}
}
}
This code passes my tests, but if someone can create better solution, I would be very interested.
I'm working on reverse-engineering a software system doing some computer graphics math. I've wound up in a weird situation where the following math is coming up:
M * X * M^-1 * A = B
Where M is a 3x3 constant invertible matrix, M^-1 is the inverse of M, X is a 3x3 matrix I can control, A is a 3x1 column vector I can also control. B is measurable.
How can I find what M is? I'm pretty sure that I could find the answer by expanding out all matrix components algebraically into a few massive equations - but it sure feels like there should be a more straightforward mechanism.
This looks like the Eigendecomposition of a matrix.
where the matrix A is decomposed into a diagonal matrix Λ and a vector of eigenvectors Q.
In your case you have
C = (M X M-1)
If you can confirm that indeed X in your case is a diagonal matrix, then you know M to be the eigenvectors of C. You can get C from C = A-1 B
As far as finding the eigenvectors, even for 3×3 matrices, is a genuinely hard problem. Typically you do a Shur Decomposition and recover the eigevectors.
I did find an online resource for the 3×3 problem, which I ported into C# for an other project. Original source here.
/// <summary>
/// Calculates the three eigenvalues analytically.
/// </summary>
/// <remarks>
/// Code taken from:
/// https://www.mpi-hd.mpg.de/personalhomes/globes/3x3/index.html
/// </remarks>
/// <returns>A vector containing the three eigenvalues.</returns>
public Vector3 GetEigenValues()
{
// Determine coefficients of characteristic polynomial. We write
// | A D F |
// A = | D* B E |
// | F* E* C |
var de = data.m_12 * data.m_23;
var dd = data.m_12 * data.m_12;
var ee = data.m_23 * data.m_23;
var ff = data.m_13 * data.m_13;
var m = data.m_11 + data.m_22 + data.m_33;
var c1 = (data.m_11 * data.m_22 + data.m_11 * data.m_33 + data.m_22 * data.m_33) - (dd + ee + ff);
var c0 = data.m_33 * dd + data.m_11 * ee + data.m_22 * ff - data.m_11 * data.m_22 * data.m_33 - 2.0 * data.m_13 * de;
var p = m * m - 3.0 * c1;
var q = m * (p - (3.0 / 2.0) * c1) - (27.0 / 2.0) * c0;
var sqrt_p = (float) Math.Sqrt(Math.Abs(p));
var sqrt_z = (float) Math.Sqrt(Math.Abs(27.0 * (0.25 * c1 * c1 * (p - c1) + c0 * (q + 27.0 / 4.0 * c0))));
var phi = (1 / 3f) * (float) Math.Atan2(sqrt_z, q);
var c = sqrt_p * (float) Math.Cos(phi);
var s = sqrt_p * (float)( Math.Abs(Math.Sin(phi))/ Math.Sqrt(3));
var w = (1 / 3f) * (m - c);
// sort the eigenvalues
if (c >= s)
{
return new Vector3(
w - s,
w + s,
w + c);
}
else if (c >= -s)
{
return new Vector3(
w - s,
w + c,
w + s);
}
else
{
return new Vector3(
w + c,
w - s,
w + s);
}
}
public Matrix3 GetEigenVectors() => GetEigenVectors(GetEigenValues());
public Matrix3 GetEigenVectors(Vector3 eigenValues)
{
Vector3 ev1 = GetEigenVector(eigenValues.X).Unit();
Vector3 ev2 = GetEigenVector(eigenValues.Y).Unit();
Vector3 ev3 = GetEigenVector(eigenValues.Z).Unit();
return FromColumns(ev1, ev2, ev3);
}
Vector3 GetEigenVector(float w)
{
return new Vector3(
data.m_12 * (data.m_23 - data.m_33 + w) - data.m_13 * (data.m_22 - data.m_23 - w)
+ data.m_22 * (data.m_33 - w) - data.m_23 * data.m_23 - w * (data.m_33 - w),
-data.m_11 * (data.m_23 - data.m_33 + w) + data.m_12 * (data.m_13 - data.m_33 + w)
- data.m_13 * data.m_13 + data.m_13 * data.m_23 + w * (data.m_23 - data.m_33 + w),
data.m_11 * (data.m_22 - data.m_23 - w) - data.m_12 * data.m_12 + data.m_12 * (data.m_13 + data.m_23)
+ data.m_13 * (w - data.m_22) - w * (data.m_22 - data.m_23 - w));
}
For your case you would do M=C.GetEigenVectors();
A final note here is that you can scale M up or down by a factor, and it wont change the equation since you are multiplying with M and M-1 at the same time. So you just need to find just one of the infinite matrices M that would make this equation work.
I recently encountered the following
problem:
Given a set of points with height yᵢ, find the height of the line for which the average distance to points above equals the average distance to points below the line:
More abstract definition: Given a set of real valued data points Y = {y1, ..., yn}, find ȳ which splits Y into two sets Y⁺ = {y ∊ Y : y > ȳ} and Y⁻ = {y ∊ Y : y < ȳ} so that the average distance between ȳ and elements of Y⁺ equals the average distance between ȳ and elements of Y⁻.
Naive solution: Initialize ȳ with the average of Y, compute average upper and lower distances and iteratively move up or down depending on whether the upper or lower average distance is greater.
Question: This problem is pretty basic, so there is probably a better solution (?) Even a non-iterative algebraic algorithm?
As mentioned in the comment, if you know which points are above and below the line, then you can solve it like this:
a = number of points above the line
b = number of points below the line
sa = sum of all y above the line
sb = sum of all y below the line
Now we can create the following equation:
(sa - a * y) / a = (b * y - sb) / b | * a * b
sa * b - a * b * y = a * b * y - a * sb | + a * b * y + a * sb
sa * b + a * sb = 2 * a * b * y | / (2 * a * b)
==> y = (a * sb + b * sa) / (2 * a * b)
= sa / (2 * a) + sb / (2 * b)
= (sa / a + sb / b) / 2
If we interprete the result then we could say it is the average between the averages of the points above and below the line.
An iterative solution based on maraca's answer:
Initialize ȳ with the mean of the given values.
Split the given values into those above and below ȳ.
Calculate the new optimal ȳ for this split.
Repeat until ȳ converges.
This is slightly faster than the algorithm outlined in the question.
// Find mean with equal average distance to upper and lower values:
function findEqualAverageDistanceMean(values) {
let mean = values.reduce((a, b) => a + b) / values.length,
last = NaN;
// Iteratively equalize average distances:
while (last != mean) {
let lower_total = 0,
lower_n = 0,
upper_total = 0,
upper_n = 0;
for (let value of values) {
if (value > mean) {
upper_total += value;
++upper_n;
} else if (value < mean) {
lower_total += value;
++lower_n;
}
}
last = mean;
mean = (upper_total / upper_n + lower_total / lower_n) / 2;
}
return mean;
}
// Example:
let canvas = document.getElementById("canvas"),
ctx = canvas.getContext("2d"),
points = Array.from({length: 100}, () => Math.random() ** 4),
mean = points.reduce((a, b) => a + b) / points.length,
equalAverageDistanceMean = findEqualAverageDistanceMean(points);
function draw(points, mean, equalAverageDistanceMean) {
for (let [i, point] of points.entries()) {
ctx.fillStyle = (point < equalAverageDistanceMean) ? 'red' : 'green';
ctx.fillRect(i * canvas.width / points.length, canvas.height * point, 3, 3);
}
ctx.fillStyle = 'black';
ctx.fillRect(0, canvas.height * mean, canvas.width, .5);
ctx.fillRect(0, canvas.height * equalAverageDistanceMean, canvas.width, 3);
}
draw(points, mean, equalAverageDistanceMean);
<canvas id="canvas" width="400" height="200">
First of all, sorry for the bad title. I'm not really sure how to title this topic, so feel free to mod it where necessary.
I am drawing X rings inside my stage with given dimensions. To give this some sense of depth, each ring towards the screen boundaries is slightly wider:
The largest ring should be as wide as the largest dimension of the stage (note that in the picture i am drawing 3 extra rings which are drawn outside the stage boundaries). Also it should be twice as wide as the smallest ring. With ring i am refering to the space between 2 red circles.
After calculating an _innerRadius, which is the width of the smallest ring, i am drawing them using
const RINGS:Number = 10; //the amount of rings, note we will draw 3 extra rings to fill the screen
const DEPTH:Number = 7; //the amount of size difference between rings to create depth effect
var radius:Number = 0;
for (var i:uint = 0; i < RINGS + 3; i++) {
radius += _innerRadius + _innerRadius * ((i*DEPTH) / (RINGS - 1));
_graphics.lineStyle(1, 0xFF0000, 1);
_graphics.drawCircle(0, 0, radius * .5);
}
One of the sliders at the bottom goes from 0-100 being a percentage of the radius for the green ring which goes from the smallest to the largest ring.
I tried lerping between the smallest radius and the largest radius, which works fine if the DEPTH value is 1. However I don't want the distance between the rings to be the same for the sake of the illusion of depth.
Now I've been trying to figure this out for hours but it seems I've run into a wall.. it seems like I need some kind of non-linear formula here.. How would I calculate the radius based on the slider percentage value? Effectively for anywhere in between or on the red circles going from the smallest to the largest red circle?
thanks!
[edit]
Here's my example calculation for _innerRadius
//lets calculate _innerRadius for 10 rings
//inner ring width = X + 0/9 * X;
//ring 1 width = X + 1/9 * X;
//ring 2 width = X + 2/9 * X
//ring 3 width = X + 3/9 * X
//ring 4 width = X + 4/9 * X
//ring 5 width = X + 5/9 * X
//ring 6 width = X + 6/9 * X
//ring 8 width = X + 7/9 * X
//ring 9 width = X + 8/9 * X
//ring 10 width = X + 9/9 * X
//extent = Math.max(stage.stageWidth, stage.stageHeight);
//now we should solve extent = X + (X + 0/9 * X) + (X + 1/9 * X) + (X + 2/9 * X) + (X + 3/9 * X) + (X + 4/9 * X) + (X + 5/9 * X) + (X + 6/9 * X) + (X + 7/9 * X) + (X + 8/9 * X) + (X + 9/9 * X);
//lets add all X's
//extent = 10 * X + 45/9 * X
//extent = 15 * X;
//now reverse to solve for _innerRadius
//_innerRadius = extent / 15;
The way your drawing algorithm works, your radii are:
r[i + 1] = r[i] + (1 + i*a)*r0
where a is a constant that is depth / (rings - 1). This results in:
r0
r1 = r0 + (1 + a)*r0 = (2 + a)*r0
r2 = r1 + (1 + 2*a)*r0 = (3 + 3*a)*r0
r3 = r2 + (1 + 3*a)*r0 = (4 + 6*a)*r0
...
rn = (1 + n + a * sum(1 ... n))*r0
= (1 + n + a * n*(n - 1) / 2)*r0
Because you want ring n - 1 to correspond with your outer radius (let's forget about the three extra rings for now), you get:
r[n - 1] = (n + (n - 1)*(n - 2) / 2)*r0
a = 2 * (extent / r0 - n) / (n - 1) / (n - 2)
Then you can draw the rings:
for (var i = 0; i < rings; i++) {
r = (1 + i + 0.5 * a * (i - 1)*i) * r0;
// draw circle with radius r
}
You must have at least three rings in order not to have a division by zero when calculating a. Also note that this does not yield good results for all combinations: if the ratio of outer and inner circles is smaller than the number of rings, you get a negative a and have the depth effect reversed.
Another, maybe simpler, approach to create the depth effect is to make each circle's radius a constant multiple of the previous:
r[i + 1] = r[i] * c
or
r[i] = r0 * Math.pow(c, i)
and draw them like this:
c = Math.pow(extent / r0, 1 / (rings - 1))
r = r0
for (var i = 0; i < rings; i++) {
// draw circle with radius r
r *= c;
}
This will create a "positive" depth effect as long as the ratio of radii is positive. (And as long as there is more than one ring, of course.)
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);
}
}
}