There exists an unknown target location (latitude and longitude co-ordinates). I have 3 latitude and longitude co-ordinate pairs and for each pair a distance in kilometers to the target location. How can I calculate the co-ordinates of the target location?
For example, say I have the following data points
37.418436,-121.963477 0.265710701754km
37.417243,-121.961889 0.234592423446km
37.418692,-121.960194 0.0548954278262km
What I'd like is what would the guts of the function that takes that as input and returns 37.417959,-121.961954 as output look like?
I understand how to calculate the distance between two points, from http://www.movable-type.co.uk/scripts/latlong.html I understand the general principle that with three circles you get exactly one point of overlap. What I'm hazy on is the math needed to calculate that point with this input.
Wikipedia gives a pretty thorough discussion of the algebra here:
http://en.wikipedia.org/wiki/Trilateration
The first step, not really covered in the Wikipedia entry, is to convert your lat/long coordinates to Cartesian coordinates:
x0 = cos( lon0 ) * cos( lat0 ) , y0 = sin( lon0 ) * cos( lat0 ) , z0 = sin( lat0 )
x1 = cos( lon1 ) * cos( lat0 ) , y1 = sin( lon1 ) * cos( lat1 ) , z1 = sin( lat1 )
x2 = cos( lon2 ) * cos( lat0 ) , y2 = sin( lon2 ) * cos( lat2 ) , z2 = sin( lat2 )
(To keep calculations simple, I've fudged things so we are working in units of "earth radii" instead of kilometers)
For your data, I get
p0 p1 p2
X -0.420442596 -0.420430618 -0.42040255
Y -0.67380418 -0.673826567 -0.673825967
Z 0.607631426 0.607614889 0.607634975
The next step, which is covered in the Wikipedia article, is to simplify the coordinates, by translating the points so p0 is at the origin, and then rotating so that p1 is on the X axis, and p2 is in the X-Y plane.
For the translation, just subtract p0 from p1 and p2:
p0a p1a p2a
X 0 1.19779E-05 4.00462E-05
Y 0 -2.23864E-05 -2.17865E-05
Z 0 -1.65372E-05 3.5486E-06
The rotation isn't much harder. p1b gets (x,y) = (d,0), where d is just the distance from the origin to p1a (Pythagorean theorem)
For p2b, we need to resolve p2a into two components: one parallel to p1a (which goes on our x axis), and one perpendicular to p1a, (which goes on our y axis in the "b" coordinate system).
To do this, we need a unit vector in the direction of p1a, which is just p1a * ( 1/d ). Take the dot product of this unit vector (call it p1a_hat, if you like) with p2a, and that's the X coordinate for p2b. The Wikipedia article calls this value "I"
Now the Y coordinate is easy. The length from the origin to p2 can't change under the coordinate transformation. So calculate p2a's length using the Pythagorean theorem, then use the Pythagorean theorem "backwards" to get what the Y coordinate for p2b has to be to keep the length the same. That's the variable that Wikipedia calls "J". (Note, there's an ambiguity that I'll leave for you to figure out over whether J is positive or negative).
Now you've got the three variables d, I and J, that the Wikipedia article uses for the calculation. You can convert them back to kilometers now, by multiplying by the earth's radius. You should be able to do the rest of the calculation from here
(Incidentally, Wikipedia gives a different calculation for the coordinate transformation. I like to avoid trig where possible).
I asked this question on the newly-formed GIS Stack Exchange, and got some good answers there as well.
https://gis.stackexchange.com/questions/66/trilateration-using-3-latitude-and-longitude-points-and-3-distances
The accepted answer there has a (presumably) working solution in Python:
https://gis.stackexchange.com/questions/66/trilateration-using-3-latitude-and-longitude-points-and-3-distances/415#415
On the Paul Bourke Geometry pages
intersection of two circles
Consider the following 9 circles
Points A,B,C and distances d1, d2, d3
Center of A, radius d1
Center of A, radius d2
Center of A, radius d3
Center of B, radius d1
Center of B, radius d2
Center of B, radius d3
Center of C, radius d1
Center of C, radius d2
Center of C, radius d3
These are your possible circles. Now we can cull these, because we know if d1 is used on A, it won't be used on B.
This makes your possible entries, where A1 means circle with center A and radius D1:
{A1, B2, C3}
{A1, B3, C2}
{A2, B1, C3}
{A2, B3, C1}
{A3, B1, C2}
{A3, B2, C1}
You should be able to convert the lat/long to X,Y,Z knowing the radius of the earth, and the distances from the curved distance along the earths crust to the straight distance, and from there you can see which of them intersect at a common point. Remember to allow for small margins of error due to float imperfection.
Related
I'm writing a function that takes in an object with a trajectory (including starting position, starting velocity, and acceleration, all represented as Vector3s) in 3D space and if it hits another object, returns the point of collision and time of the collision. I'm using kinematic equations with a timestep to detect possible collisions and I can get the point of collision that way, but once I have that I want to find the exact time that that collision would occur at.I thought of rearranging a kinematic equation to solve for time and plug in what I already had, but I can't figure out how I can use all three axes of motion to do this, since my other values are Vec3's and time is just scalar. I've thought about just doing the calculation on one axis, but I'm not sure if that would lead to an accurate result.
Would it be accurate to calculate just based on one axis, or is there a way to incorporate all three into the calculation? The formula I'm using to solve for time is:
t = (v_init +/- Sqrt((v_init)^2 - (accel * disp * 4 * .5)))/accel;
Where v_init is initial velocity, disp is total displacement, and accel is acceleration. I'm basing this off of the kinematic equation:
d = v*t + .5*a*t^2
Let me write in the general case. The component-wise motion law is
x(t) = x0 + v_x t + 0.5 a_x t^2
y(t) = y0 + v_y t + 0.5 a_y t^2
z(t) = z0 + v_z t + 0.5 a_z t^2
where (x0,y0,z0)^t is the initial position, (v_x, v_y, v_z)^t is the initial velocity vector, and (a_x, a_y, a_z)^t is the vector of acceleration. The 3rd component of the latter may include also the gravity acceleration.
I assume that the collision plane is horizontal, having thus equation z = k. Solve in t the equation
z(t) = k
for finding the time t_c in which the projectile hits the plane. Compute then the collision coordinates x(t_c) and y(t_c) using the above formula by substituting t with t_c.
If the plane has the general equation
a x + b y +c z + d = 0
I suggest to put the frame of reference on the plane, having the xy plane on the collision plane, and then apply the above procedure.
You may also solve the non linear system
x = x0 + v_x t + 0.5 a_x t^2
y = y0 + v_y t + 0.5 a_y t^2
z = z0 + v_z t + 0.5 a_z t^2
a x + b y +c z + d = 0
taking the solution for t>0 (I dropped the dependency on t for x, y and z).
To solve it in C++, you may search a math library, such as Eigen which has a module for non linear systems.
Is there any numerically stable angle bisector algorithm?
The problem is the following:
Given three vectors (2 dimensional) A,B,C
Find the bisector of angle B (angle between AB and BC)
Actually I'm computing it in the following way:
Normalize AB
Normalize BC
Find (AB+CD)/2f (Mid Point)
The bisector is line passing between B and the Mid Point.
The problem with my approach is that when the angle is almost 180° (AB almost parallel to BC) the bisector is very inaccurate (of course because mid point is almost coincident with B). The current algorithm is so inaccurate that sometimes the resulting bisector is almost parallel to one of the other 2 segments.
And yes there are no "cast" problems, all computations are done in single precision floating point.
You could use that the angle bisector remains the same if you rotate BA by +90° and BC by -90°.
So use the original formula if the situation is stable, that is, if the dot product of BA and BC is positive.
If it is negative, apply the rotations, for BA (x,y) -> (-y,x) and for BC (x,y) -> (y,-x), which also renders the dot product positive. Proceed as before with the new vectors.
If you try this out you will note that the jump in direction of the bisector now occurs for the angle -90° between the vectors. It is not possible to avoid this jump, as a continuous bisector will only be the same after two turns (fixing BA and moving C).
It’s not trivial. Let’s say the two edge vectors are a and b:
float2 a = A - B;
float2 b = C - B;
Compute the dot product float dp = dot( a, b )
Normalize both vectors:
float2 a_norm = normalize( a );
float2 b_norm = normalize( b );
Check the sign bit of the dot product. When the dp is non-negative,
return normalize( a_norm + b_norm ); and you’re done.
When the dot product is negative, you have obtuse angle between input vectors.
Applying a naïve formula in this case would screw up the numerical precision.
Need another way.
float2 c = normalize( a_norm - b_norm );
float dir = dot( a, rotate90( b ) );
return ( dir < 0 ) ? rotate90( c ) : rotate270( c );
Note - instead of the +, this is what gives the precision win. When the angle between a and b is greater than 90°, the angle between a and -b is less than 90°, and the length of a_norm - b_norm is large enough to give accurate direction. We just need to rotate it by 90° afterwards, in the correct direction.
P.S. Rotating 2D vectors by multiples of 90° is lossless operation.
Here’s pseudocode for rotate90 and rotate270 functions:
float2 rotate90( float2 vec )
{
return float2( vec.y, -vec.x );
}
float2 rotate270( float2 vec )
{
return float2( -vec.y, vec.x );
}
A simple enough way to do this follows in two formats (but the content is otherwise identical):
Pseudocode
// Move A and C to the origin for easier rotation calculations
Aprime=A-B;
Cprime=C-B;
// The counter-clockwise angle between the positive X axis to A'
angle_a = arctan(Aprime.y, Aprimet.x);
// ditto for C'
angle_c = arctan(Cprime.y, Cprime.x);
// The counter-clockwise angle from A' to C'
angle_ac = angle_c - angle_a;
// The counter-clockwise angle from the positive X axis to M'
angle_m = angle_ac/2 + angle_a;
// Construct M' which, like A' and C', is relative to the origin.
Mprime=(cos(angle_m), sin(angle_m));
// Construct M which is relative to B rather than relative to the origin.
M=Mprime+B
In English
Move the vectors to the origin by
A'=A-B
B'=B
C'=C-B
Get the angle from the positive X axis to A' as angle_a = arctan(A_y, A_x).
Get the angle from the positive X axis to C' as angle_c = arctan(C_y, C_x).
Get the counter-clockwise angle from A' to C' as angle_ac = angle_c - angle_a.
Get the angle from the positive X axis to M' as angle_m = angle_ac/2 + angle_a.
Construct M' from this angle as M' = (cos(angle_m), sin(angle_m)).
Construct M as M = M' + B.
The vector BM bisects the angle ABC.
Since there is arbitrary division, there are no difficulties with this method. Here's a graphing calculator to encourage intuition with the solution: https://www.desmos.com/calculator/xwbno717da
You can find the bisecting vector quite simply with:
∥BC∥ * BA + ∥BA∥ * BC
But that also won't be numerically stable with ABC collinear or nearly so. What might work better would be to find the angle between AB and BC, via the dot product.
cos θ = (BA · BC) / (∥BC∥ * ∥BA∥)
That will produce the correct angle even in the collinear case.
Definition: If A and B are points, vector(A,B) is the vector from point A to B.
Lets say that point O is the point of origin for our coordinate system.
The coordinates of point A are the same as of radius-vector(O,A).
Let point M be the middle point for the bisector,so you need to:
-normalize vector(B,A)
-normalize vector(B,C)
-vector(B,M) = vector(B,A)+vector(B,C) //vector from B to middle point
-(optionally) You can multiply vector(B,M) with a scalar to get a longer vector / increase distance between B and M
-vector(O,M) = vector(O,B) + vector(B,M)//radius-vector from O to M
Now middle point M has the same coordinates as radius-vector(O,M).
Closed. This question needs to be more focused. It is not currently accepting answers.
Want to improve this question? Update the question so it focuses on one problem only by editing this post.
Closed 7 years ago.
Improve this question
How to calculate endpoints of the shortest line connecting two circles without using the trig functions?
Two circles and a line
The principle explained below is quite intuitive - instead of analyzing two-dimensional task we divide it in two one-dimensional operations. To be precise we take apart x and y coordinate values for known circle centers and calculate values separately. We calculate new x' and y' only knowing the distance between circle centers and the proportions of radii of said circles to the distance between centers.
(x1 , y1 ), (x2 , y2 ), r1, r2 - known values
(x1', y1'), (x2', y2') - values we are looking for
And all you need to calculate the values we are looking for are following operations:
ΔY = y2 - y1
ΔX = x2 - x1
L = √(ΔX² + ΔY²)
r1L = r1 / L
r2L = r2 / L
y1' = y1 + ΔY * r1L
y2' = y2 - ΔY * r2L
x1' = x1 + ΔX * r1L
x2' = x2 - ΔX * r2L
And you get (x1', y1') and (x2', y2')
The theory behind this calculation is as follows...
Having two circles with their radii r1 and r2 and center coordinates (x1, y1) and (x2, y2) we need to find points (x1', y1') and (x2', y2') at which the line connecting two centers intersects the circles.
Having centers of the two circles (x1,y1) and (x2,y2) we calculate ΔX and ΔY which will be used later twice.
ΔY = y2 - y1 It is worth noting here that Δ can
ΔX = x2 - x1 be negative if x1 > x2 or y1 > y2
First to calculate distance between the centers using Pythagoras theorem:
L = √(ΔX² + ΔY²)
And second time to calculate the offsets using the ratios of the radii to the L (the length of the whole line).
Now looking at the plot below we see that we have a trapezoid with one of the sides being the y axis and the other the line connecting circles' centers.
We know that first circle radius is r1 and the length between centers is L.
We also know that line paralel to the base line splitting trapezoid splits its sides with the same ratios.
Because we know the distance L and the radius r1 we can calculate the ratio.
r1L = r1 / L
Now we can use this ratio to get the point (0, y2').
y1' = y1 + ΔY * r1L
So now we have got the y component of (x1', y1') coordinate. We do similarly with y2'.
r2L = r2 / L
y2' = y2 - ΔY * r2L
To get x1' and x2' we use the x axis to form the other trapezoid and similarly repeat steps shown above.
x1' = x1 + ΔX * r1L
x2' = x2 - ΔX * r2L
As a result we end up with new endpoints (x1', y1') and (x2', y2').
It has to be noted that x1' and y1' values are calculated by adding to them but x2' and y2' values are calculated by subtracting from them. It is so because we initially assume that (x1, y1) is closer to the center coordinate (0, 0), i.e. x1 < x2 and y1 < y2, in ΔY = y2 - y1 and ΔX = x2 - x1.
Imagine a line between the center of the two circles. Find the points where that line intersects the circles. Your line is between those two points.
Call the centers of the two circles (x1,y1) and (x2,y2).
ΔY = y2-y1 \___ for the whole line (blue-red-blue)
ΔX = x2-x1 /
The length of the line between the circle centers is:
L = √(ΔX² + ΔY²)
Using the each circle's radius, r, you can compute the Δy and Δx from the center to the other end of the blue line:
Δx = r/L ΔX
Δy = r/L ΔY
So the points are (x1+Δx, y1+Δy) and similarly for the other blue line.
Now you have the two endpoints of the red line.
Now, on each end you need a line (the blue part) whose length is equal to the radius of the relevant circle. At this point you can forget about the circles!
I want to find out the clockwise angle between 2 vectors(2D, 3D).
The clasic way with the dot product gives me the inner angle(0-180 degrees) and I need to use some if statements to determine if the result is the angle I need or its complement.
Do you know a direct way of computing clockwise angle?
2D case
Just like the dot product is proportional to the cosine of the angle, the determinant is proprortional to its sine. So you can compute the angle like this:
dot = x1*x2 + y1*y2 # dot product between [x1, y1] and [x2, y2]
det = x1*y2 - y1*x2 # determinant
angle = atan2(det, dot) # atan2(y, x) or atan2(sin, cos)
The orientation of this angle matches that of the coordinate system. In a left-handed coordinate system, i.e. x pointing right and y down as is common for computer graphics, this will mean you get a positive sign for clockwise angles. If the orientation of the coordinate system is mathematical with y up, you get counter-clockwise angles as is the convention in mathematics. Changing the order of the inputs will change the sign, so if you are unhappy with the signs just swap the inputs.
3D case
In 3D, two arbitrarily placed vectors define their own axis of rotation, perpendicular to both. That axis of rotation does not come with a fixed orientation, which means that you cannot uniquely fix the direction of the angle of rotation either. One common convention is to let angles be always positive, and to orient the axis in such a way that it fits a positive angle. In this case, the dot product of the normalized vectors is enough to compute angles.
dot = x1*x2 + y1*y2 + z1*z2 #between [x1, y1, z1] and [x2, y2, z2]
lenSq1 = x1*x1 + y1*y1 + z1*z1
lenSq2 = x2*x2 + y2*y2 + z2*z2
angle = acos(dot/sqrt(lenSq1 * lenSq2))
Edit: Note that some comments and alternate answers advise against the use of acos for numeric reasons, in particular if the angles to be measured are small.
Plane embedded in 3D
One special case is the case where your vectors are not placed arbitrarily, but lie within a plane with a known normal vector n. Then the axis of rotation will be in direction n as well, and the orientation of n will fix an orientation for that axis. In this case, you can adapt the 2D computation above, including n into the determinant to make its size 3×3.
dot = x1*x2 + y1*y2 + z1*z2
det = x1*y2*zn + x2*yn*z1 + xn*y1*z2 - z1*y2*xn - z2*yn*x1 - zn*y1*x2
angle = atan2(det, dot)
One condition for this to work is that the normal vector n has unit length. If not, you'll have to normalize it.
As triple product
This determinant could also be expressed as the triple product, as #Excrubulent pointed out in a suggested edit.
det = n · (v1 × v2)
This might be easier to implement in some APIs, and gives a different perspective on what's going on here: The cross product is proportional to the sine of the angle, and will lie perpendicular to the plane, hence be a multiple of n. The dot product will therefore basically measure the length of that vector, but with the correct sign attached to it.
To compute angle you just need to call atan2(v1.s_cross(v2), v1.dot(v2)) for 2D case.
Where s_cross is scalar analogue of cross production (signed area of parallelogram).
For 2D case that would be wedge production.
For 3D case you need to define clockwise rotation because from one side of plane clockwise is one direction, from other side of plane is another direction =)
Edit: this is counter clockwise angle, clockwise angle is just opposite
This answer is the same as MvG's, but explains it differently (it's the result of my efforts in trying to understand why MvG's solution works). I'm posting it on the off chance that others find it helpful.
The anti-clockwise angle theta from x to y, with respect to the viewpoint of their given normal n (||n|| = 1), is given by
atan2( dot(n, cross(x,y)), dot(x,y) )
(1) = atan2( ||x|| ||y|| sin(theta), ||x|| ||y|| cos(theta) )
(2) = atan2( sin(theta), cos(theta) )
(3) = anti-clockwise angle between x axis and the vector (cos(theta), sin(theta))
(4) = theta
where ||x|| denotes the magnitude of x.
Step (1) follows by noting that
cross(x,y) = ||x|| ||y|| sin(theta) n,
and so
dot(n, cross(x,y))
= dot(n, ||x|| ||y|| sin(theta) n)
= ||x|| ||y|| sin(theta) dot(n, n)
which equals
||x|| ||y|| sin(theta)
if ||n|| = 1.
Step (2) follows from the definition of atan2, noting that atan2(cy, cx) = atan2(y,x), where c is a scalar. Step (3) follows from the definition of atan2. Step (4) follows from the geometric definitions of cos and sin.
Since one of the simplest and most elegant solutions is hidden in one the comments, I think it might be useful to post it as a separate answer.
acos can cause inaccuracies for very small angles, so atan2 is usually preferred. For the 3D case:
dot = x1 * x2 + y1 * y2 + z1 * z2
cross_x = (y1 * z2 – z1 * y2)
cross_y = (z1 * x2 – x1 * z2)
cross_z = (x1 * y2 – y1 * x2)
det = sqrt(cross_x * cross_x + cross_y * cross_y + cross_z * cross_z)
angle = atan2(det, dot)
Scalar (dot) product of two vectors lets you get the cosinus of the angle between them.
To get the 'direction' of the angle, you should also calculate the cross product, it will let you check (via z coordinate) is angle is clockwise or not (i.e. should you extract it from 360 degrees or not).
For a 2D method, you could use the law of
cosines and the "direction" method.
To calculate the angle of segment P3:P1
sweeping clockwise to segment P3:P2.
P1 P2
P3
double d = direction(x3, y3, x2, y2, x1, y1);
// c
int d1d3 = distanceSqEucl(x1, y1, x3, y3);
// b
int d2d3 = distanceSqEucl(x2, y2, x3, y3);
// a
int d1d2 = distanceSqEucl(x1, y1, x2, y2);
//cosine A = (b^2 + c^2 - a^2)/2bc
double cosA = (d1d3 + d2d3 - d1d2)
/ (2 * Math.sqrt(d1d3 * d2d3));
double angleA = Math.acos(cosA);
if (d > 0) {
angleA = 2.*Math.PI - angleA;
}
This has the same number of transcendental
operations as suggestions above and only one
more or so floating point operation.
the methods it uses are:
public int distanceSqEucl(int x1, int y1,
int x2, int y2) {
int diffX = x1 - x2;
int diffY = y1 - y2;
return (diffX * diffX + diffY * diffY);
}
public int direction(int x1, int y1, int x2, int y2,
int x3, int y3) {
int d = ((x2 - x1)*(y3 - y1)) - ((y2 - y1)*(x3 - x1));
return d;
}
If by "direct way" you mean avoiding the if statement, then I don't think there is a really general solution.
However, if your specific problem would allow loosing some precision in angle discretization and you are ok with loosing some time in type conversions, you can map the [-pi,pi) allowed range of phi angle onto the allowed range of some signed integer type. Then you would get the complementarity for free. However, I didn't really use this trick in practice. Most likely, the expense of float-to-integer and integer-to-float conversions would outweigh any benefit of the directness. It's better to set your priorities on writing autovectorizable or parallelizable code when this angle computation is done a lot.
Also, if your problem details are such that there is a definite more likely outcome for the angle direction, then you can use compilers' builtin functions to supply this information to the compiler, so it can optimize the branching more efficiently. E.g., in case of gcc, that's __builtin_expect function. It's somewhat more handy to use when you wrap it into such likely and unlikely macros (like in linux kernel):
#define likely(x) __builtin_expect(!!(x), 1)
#define unlikely(x) __builtin_expect(!!(x), 0)
For 2D case atan2 can easily calculate angle between (1, 0) vector (X-axis) and one of your vectors.
Formula is:
Atan2(y, x)
So you can easily calculate difference of two angles relatively X-axis
angle = -(atan2(y2, x2) - atan2(y1, x1))
Why is it not used as default solution? atan2 is not efficient enough. Solution from the top answer is better. Tests on C# showed that this method has 19.6% less performance (100 000 000 iterations). It's not critical but unpleasant.
So, another info that can be useful:
The smallest angle between outer and inner in degrees:
abs(angle * 180 / PI)
Full angle in degrees:
angle = angle * 180 / PI
angle = angle > 0 ? angle : 360 - angle
or
angle = angle * 180 / PI
if (angle < 0) angle = 360 - angle;
A formula for clockwise angle,2D case, between 2 vectors, xa,ya and xb,yb.
Angle(vec.a-vec,b)=
pi()/2*((1+sign(ya))*
(1-sign(xa^2))-(1+sign(yb))*
(1-sign(xb^2))) +pi()/4*
((2+sign(ya))*sign(xa)-(2+sign(yb))*
sign(xb)) +sign(xa*ya)*
atan((abs(ya)-abs(xa))/(abs(ya)+abs(xa)))-sign(xb*yb)*
atan((abs(yb)-abs(xb))/(abs(yb)+abs(xb)))
just copy & paste this.
angle = (acos((v1.x * v2.x + v1.y * v2.y)/((sqrt(v1.x*v1.x + v1.y*v1.y) * sqrt(v2.x*v2.x + v2.y*v2.y))))/pi*180);
you're welcome ;-)
For example, GetAngle((0,0),(100,0),(100,100)) = 90. How could I find the angle between 3 2D Points.
Given points A, B, and C, you want the angle between AB and AC? First compute the vectors AB and AC -- it's just the coordinates of B minus coordinates of A and likewise for AC. Take the dot product of the two vectors. This is just the product of the x coordinates plus the product of the y coordinates of the vectors. Divide this number by the length of AB, and again by the length of AC. This result is the cosine of the angle between the two vectors, so take the arccos() and you have it.
The problem with using just the dot product here is that it is unstable near 0 or 180 degrees -- the slope of acos() approaches infinity near +/- 1.0 which will cause you to lose precision.
To fix this, you can compute a pseudo-cross product, and use atan2(), as follows:
// given A, B, C are 2D points:
BA= B - A; CA= C - A // vector subtraction, to get vector between points
dot= BA.x * CA.x + BA.y * CA.y
pcross= BA.x * CA.y - BA.y * CA.x
angle= atan2(pcross, dot) // this should be the angle BAC, in radians
This should be numerically robust unless one of the legs of the angle has zero length.
Note that this will also give you a signed angle, depending on whether BAC goes clockwise or counterclockwise; the acos() method will always give you a positive value. Of course, if you want only a positive angle, you can take abs(angle); the atan2() method will still be more robust, and probably faster.
Use the dot product:
(a,b,c) dot (d,e,f) = ad + be + bf.
A dot B = length(A)*length(B)* cos(theta)
theta = arccos((A dot B)/(length(A)*length(B)) is the angle between vectors A and B.
This is easy if you have some basic knowledge of linear algebra.
A vector v (in a linear algebra sense, not std::vector ;) ) is a tuple v = (x,y,z).
The norm is the length of the vector |v| = sqrt(xx + yy + z*z)
The inner product of two vectors v1 = (x1, y1, z1) and v2 = (x2, y2, z2) is v1·v2 = x1*x2 + y1*y2 + z1*z2
The angle of vectors v1 and v2 is a = acos(v1·v2/(|v1|*|v2|))