Quick way to determine if facing a given lat/lon pair with a heading

Question

I'm looking for a fast way, preferably without trigonometric operations (just multiply, divide, add, sqrt and subtract) to determine if a point is visible. E.g. say I am facing 0 degrees N, I am at 51.40,-1.08, the camera has a HFOV of 30 degrees and the point is at 51.20,-1.085 it should return TRUE, because the point would be facing me and is within the 30 degree FOV.

It's required that it returns TRUE (false positive) instead of FALSE given edge cases, otherwise points may be hidden when they are actually visible. Currently I have my augmented reality project using atan2 to calculate the actual angle but if I can shave calculations off from points which are definitely not visible every little bit helps. I only need it to be accurate over distances of less than 2 km.

Here's my code to calculate horizontal and vertical position. It's only accurate over <5km distances but accuracy isn't critical anyway as those waypoints aren't drawn. This project is open source, so it's okay for me to post a lot of code.

res->est_dist = hypot2(p_viewer->p.lat - p_subj->lat, p_viewer->p.lon - p_subj->lon) * 2000.0f;
// Save precious cycles if outside of visible world.
if(res->est_dist > cliph)
    goto quick_exit;
// Compute the horizontal and vertical angle to the point. 
res->h_angle = angle_dist_deg(RAD2DEG(atan2(p_viewer->p.lon - p_subj->lon, p_viewer->p.lat - p_subj->lat)), p_viewer->yaw);
res->v_angle = RAD2DEG(atan2(p_viewer->p.alt - p_subj->alt, res->est_dist)); // constant scale factor: needs to be adjusted
// Normalize the results to fit in the field of view of the camera.
res->h_angle += p_viewer->hfov / 2;
res->v_angle += p_viewer->vfov / 2;
// Compute projected X and Y position.
res->pos.x = (res->h_angle / p_viewer->hfov) * p_viewer->width;
res->pos.y = (res->v_angle / p_viewer->vfov) * p_viewer->height;

See that call to atan2 for the h_angle? If I could eliminate it by saying "this point is definitely not visible" it would be nice.

Answer 1

Given that your inputs are in degrees, trigonometric operations are unavoidable. At a minimum you need to account for the distortion in latitude relative to longitude at all points not on the equator.

The simplest approximation (based on a spherical model of the earth and suitable only for small distances) I can think of projects the two points (lat1, lon1), (lat2, lon2) to the points

(x1, y1) = (lon1 * cos(lat1), lat1) and

(x2, y2) = (lon2 * cos(lat2), lat2)

in the Euclidean plane. (This is a Sinusoidal projection.) The direction vector from the viewpoint (#1) to the target point (#2) therefore is

(u, v) = (x2-x1, y2-y1)

with length n = Sqrt(u^2 + v^2), whence the unit direction vector equals

(u/n, v/n).

If you can specify the direction you're facing as a vector, rather than as an angle, you can avoid some more trig. Otherwise, a direction t degrees east of north has to be converted to the unit direction vector

(a, b) = (sin(t), cos(t)).

Finally, visibility is tested by comparing the inner product

(u/n, v/n) . (a, b) = (a*u + v*b)/n

to the cosine of half the horizontal field of view (another trig value, but it can be precomputed once and for all). The inner product is less than this cosine only for the invisible points.

This method requires computing one cosine for each base point and target point, as well as a sine and a cosine for the direction of view.

For example, with a HFOV of 30 degrees you can look 15 degrees to the right and left. The cosine of 15 degrees equals 0.965926. If you are standing at lon = -1.080 and lat = 51.40 and the target point is located at lon = -1.085 and lat = 51.20, then

(x1, y1) = (-1.080 / cos(51.4), 51.4) = (-0.67397, 51.4)

(x2, y2) = (-1.085 / cos(51.2), 51.2) = (-0.67987, 51.2)

(u, v) = (-0.00608, -0.200)

n = 0.200092

(u/n, v/n) = (-0.03036, -0.99954).

The direction vector for due north is (sin(0), cos(0)) = (0, 1). Its inner product with (u/n, v/n) equals -0.99954. Because this is (a lot) less than 0.965926, the point is invisible. (This, I hope, was obvious at the outset, because the target point is south of--that is, behind--the point of view.) If you happened to be looking due south, your direction vector would be (sin(180), cos(180)) = (0, -1), the inner product would equal +0.99954, and because this exceeds 0.965926, you would conclude that the target point is visible.

---

In some cases you can avoid a lot of trig. If you have a single base point and lots of target points nearby, all the cosines of the latitudes will be approximately the same and equal to the cosine of the base point's latitude. In this case you need compute it only once and use that approximation for all the other calculations. In this fashion, no matter how many target points there are, you need only four trig evaluations for each base point, HFOV, and direction of view combination. Of course, because this is an approximation (albeit a good one), it would be silly to worry about "edge cases" (which I presume are points right at the edge of the field of view).

Answer 2

First: What whuber said about trig being unavoidable.

Since you're doing augmented reality, here is my recommendation:

• Drop the lat/lon for everything as early as possible in your data pipeline: convert to 3D cartesian coordinates and use those instead, and keep them around so you don't have to re-do it all the time. (There's the trig!)
• Then use traditional frustum culling to drop points that aren't visible.
  • Using OpenGL? Don't want to do the math+bookkeeping for a bunch of plane-side tests? Use gluProject with the current modelview and projection matrices & viewport. If the resulting coordinate is inside the screen rectangle (pixels) and has a 'z' value of between 0 and 1, it will be visible on the camera! (From memory so check the manual.)

Here's what your computational costs look like:

• Four trig operations to convert from spherical coordinates (lat/lon/alt, or phi/theta/rho, depending if you're a geographer or a mathematician) to cartesian.
  • However, you just need to do it once! (This is relevant to this question too: since you're now just working in cartesian, just add the vector and you're done -- accurately so, too, without fudge factor.)
• A bunch of multiplies and adds (three vector-matrix multiplies), plus three divisions and six comparisons for the actual visibility test. Fast!

To summarise, Spherical Coordinates Bad. Cartesian Good.