## Help Request: Triangulation in 3D

A place to discuss the implementation and style of computer programs.

Moderators: phlip, Moderators General, Prelates

super_aardvark
Posts: 54
Joined: Tue Jan 22, 2008 9:26 am UTC

### Help Request: Triangulation in 3D

Not sure whether to post this here or in Math... I'll try here, 'cause they're talking about some esoteric stuff over there that I don't understand.

I want to write a function that calculates a position in (x,y,z) based on two known points and the heading and azimuth from the unknown position to each of them. I'm hoping someone knows this off the top of their head, or can give me a good link, and save me a bunch of time and scratch paper. (Google just wants to tell me all about dead reckoning.)

I've got a simple case working already, where the two known points are on the y-axis (one at the origin) and it's only calculating (x,y). It goes like this: (Heading 0 is north [positive y-axis] and increases in the clockwise direction)

Code: Select all

`function findMe(      a, // heading in radians to origin      b, // heading in radians to 2nd point (on y-axis)      dist) // distance between known points, i.e. 2nd point is at (0,dist){   x = sin(a - pi) * (dist / sin(b-a)) * sin(2*pi - b);   y = x * tan(1.5*pi - a);}`

Obviously, this doesn't work if the unknown point is on the y-axis. I want to first generalize this, so take x1, y1, x2, and y2 for the two known points (right now y2 is dist and the others are 0). Then I want to add a third dimension: z1 and z2 for the known points, and an azimuth for each, and calculate x, y, and z for the unknown position.

Code: Select all

`function findMe(x1, y1, z1, heading1, azimuth1, x2, y2, z2, heading2, azimuth2)`

Thanks for any help you can give me.

The Great Hippo
Swans ARE SHARP
Posts: 7368
Joined: Fri Dec 14, 2007 4:43 am UTC
Location: behind you

### Re: Help Request: Triangulation in 3D

I'm not sure how to do this very well, but I know that if you have three points (A, B, C), you know two of them (A, B), and you know the angle to get to those two points from the point you don't know (you know the angle of C to A, and you know the angle of C to B), then you also know the angle of A to C and B to C (just reverse the angles).

In other words, one way to get C is to take C to A and C to B, reverse them, use them to create rays from A to C (Ray D) and B to C (Ray E), then figure out where D and E intersect (that's going to be C).

WanderingLinguist
Posts: 237
Joined: Tue May 22, 2012 5:14 pm UTC
Location: Seoul
Contact:

### Re: Help Request: Triangulation in 3D

super_aardvark wrote:I've got a simple case working already, where the two known points are on the y-axis (one at the origin)

This really isn't my strong point, so I might be totally wrong, but couldn't you do a simple transformation to place both points on the y-axis (and the unknown point off the y-axis)? Then reverse the transformation after getting the result?

f5r5e5d
Posts: 104
Joined: Tue May 08, 2012 3:22 am UTC

### Re: Help Request: Triangulation in 3D

how about finding normalized direction vectors from X to A, B from the given heading, azimuth info (may depend on your axis, angular reference conventions)

then the magnitude of the cross product of the normalized vectors is the sine of the included angle at X, if the cross product is 0 then X is on the AB line and you're screwed

reversing one of the direction vectors, say the X_to_A vector you can find the sine of the interior angle at A, again by taking the magnitude of the cross product between -X_to_A and (B-A)/|B-A|

now the law of sines gives you the length from B to X

with direction, length from known point B you can calc X

thoughtfully
Posts: 2253
Joined: Thu Nov 01, 2007 12:25 am UTC
Location: Minneapolis, MN
Contact:

### Re: Help Request: Triangulation in 3D

What you're trying to do is find the intersection of two lines. In 2D, this is pretty straightforward. In 3D, it's pretty tricky, even if the lines actually intersect, and there's no reason they need to. Even if the lines are set up so that they ought to intersect, small rounding errors are going to foil you unless symbolic math is being used. This is compounded in real life with its imprecise measurements. Once you find the closest pair of points, the midpoint of these is your answer.

I googled for "lines closest points" and got a number of helpful looking links, including this one. You might also try a variant on the search: "lines closest pair". You should also try "skew lines" which is the term for two lines in 3D that are not parallel and don't intersect.

Wolfram Mathworld is a good resource for basic geometry problems like this.
http://mathworld.wolfram.com/Line-LineIntersection.html

As far as crazy math goes, you should probably gird yourself for some work with matrices and solving systems of linear equations. Not all that bad. Perfection is achieved, not when there is nothing more to add, but when there is nothing left to take away.
-- Antoine de Saint-Exupery

dwarduk2
Posts: 31
Joined: Thu Mar 08, 2012 1:17 am UTC

### Re: Help Request: Triangulation in 3D

As thoughtfully says, if you know that your lines actually do intersect, simultaneous equations is probably the easiest way to do it. With rounding errors, if you choose different variables in this you may get slightly different answers, but that shouldn't matter beyond rounding errors. Let's call the coordinates of the first known point (X1,Y1,Z1), the azimuth A1 and the heading H1; similarly for the second point. The unit direction vector for the azimuth and heading is (sinAcosH, sinAsinH, cosA), so you can write the equation of the ray from a known point to the unknown point as either:

r = (X1, Y1, Z1) + t(sinH1cosA1, sinH1sinA1, cosH1)
r = (X2, Y2, Z2) + s(sinH2cosA2, sinH2sinA2, cosH2)

If you equate the two through r, you get 3 equations to find 2 variables. Because we're already assuming the system of equations is consistent, as the lines do intersect, you can just solve as normal for s and t, and plug those values back into the two equations above:

Y1 + tsinA1sinH1 = Y2 + ssinA2sinH2
=> ssinA2sinH2 - tsinA1sinH1 = Y1 - Y2

Z1 + tcosH1 = Z2 + scosH2
=> scosH2 - tcosH1 = Z1 - Z2

As the guy above said, a matrix is probably the best way to solve this, so

(sinA2sinH2, -sinA1sinH1; cosH2, -cosH1)(s; t) = (Y1 - Y2; Z1 - Z2)

Writing up the solution algebraically in ASCII is messy, as you can see above, so I'll leave it here, but that's quite an easy one to solve if you have numbers (or code for the nice data structures)

f5r5e5d
Posts: 104
Joined: Tue May 08, 2012 3:22 am UTC

### Re: Help Request: Triangulation in 3D

the vector algebra, Law of Sines approach always gives a solution for X excepting where the angle AXB is 0 or pi and there is no triangle because all 3 points are co-linear

you could work the vector algebra vertex angle, side length calc the other way 'round using the angle XBA to find the A to X side's length giving a 2nd calc for X to check consistency - but you will always get a result

other consistency checks include seeing if the vertices's cross product vector parts are all oriented the same, the interior angles all add up to pi

super_aardvark
Posts: 54
Joined: Tue Jan 22, 2008 9:26 am UTC

### Re: Help Request: Triangulation in 3D

Very helpful suggestions. I think I've got enough to go on here. Thanks, everyone!

P.S. The lines must intersect because both pairs of measurements are being taken from a single unknown point. Rounding is a big problem however, because the measurements are only accurate to .01 degrees--though they're perfectly reliable.

Edit:
dwarduk2 wrote:As thoughtfully says, if you know that your lines actually do intersect, simultaneous equations is probably the easiest way to do it. With rounding errors, if you choose different variables in this you may get slightly different answers, but that shouldn't matter beyond rounding errors. Let's call the coordinates of the first known point (X1,Y1,Z1), the azimuth A1 and the heading H1; similarly for the second point. The unit direction vector for the azimuth and heading is (sinAcosH, sinAsinH, cosA), so you can write the equation of the ray from a known point to the unknown point as either:

r = (X1, Y1, Z1) + t(sinH1cosA1, sinH1sinA1, cosH1)
r = (X2, Y2, Z2) + s(sinH2cosA2, sinH2sinA2, cosH2)

This seemed like the easiest approach, and the formula for the unit vector is very helpful... but you mixed it up when you inserted it into those two equtions. Also it's wrong. It took me a while to figure out; I thought we were just using different coordinate systems at first (otherwise how could heading be the only factor on the z-axis?) It was close enough for me to figure it out though: it should be (cosHcosA, sinHcosA, sinA). So, thanks for the help, and thanks for keeping me on my toes!

dwarduk2
Posts: 31
Joined: Thu Mar 08, 2012 1:17 am UTC

### Re: Help Request: Triangulation in 3D

You're welcome! And sorry; I messed it up the first time and edited it but clearly forgot to update the conclusion as well as the earlier information. To my shame, I got azimuth and heading the wrong way round originally gordo
Posts: 44
Joined: Wed Oct 10, 2012 2:17 pm UTC

### Re: Help Request: Triangulation in 3D

dwarduk2 wrote:As thoughtfully says, if you know that your lines actually do intersect, simultaneous equations is probably the easiest way to do it. With rounding errors, if you choose different variables in this you may get slightly different answers, but that shouldn't matter beyond rounding errors. Let's call the coordinates of the first known point (X1,Y1,Z1), the azimuth A1 and the heading H1; similarly for the second point. The unit direction vector for the azimuth and heading is (sinAcosH, sinAsinH, cosA), so you can write the equation of the ray from a known point to the unknown point as either:

r = (X1, Y1, Z1) + t(sinH1cosA1, sinH1sinA1, cosH1)
r = (X2, Y2, Z2) + s(sinH2cosA2, sinH2sinA2, cosH2)

If you equate the two through r, you get 3 equations to find 2 variables. Because we're already assuming the system of equations is consistent, as the lines do intersect, you can just solve as normal for s and t, and plug those values back into the two equations above:

Y1 + tsinA1sinH1 = Y2 + ssinA2sinH2
=> ssinA2sinH2 - tsinA1sinH1 = Y1 - Y2

Z1 + tcosH1 = Z2 + scosH2
=> scosH2 - tcosH1 = Z1 - Z2

As the guy above said, a matrix is probably the best way to solve this, so

(sinA2sinH2, -sinA1sinH1; cosH2, -cosH1)(s; t) = (Y1 - Y2; Z1 - Z2)

Writing up the solution algebraically in ASCII is messy, as you can see above, so I'll leave it here, but that's quite an easy one to solve if you have numbers (or code for the nice data structures)

Trying to equate r(t) and r(s) is only going work if the two rays actually intersect. Instead, I would define the distance d(t,s) and find the minimizers t_min and s_min, and plug those back in r(t) and r(s). This will always give you a solution, even if the lines don't intersect. However, differentiating d(t,s) with respect to t and s can be a messy exercise if you don't have some symbolic manipulation tool (I like Sympy for Python a lot).

Return to “Coding”

### Who is online

Users browsing this forum: No registered users and 5 guests