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.