Poor mans navigation system.

[clive]

Ok, so a slight optimization.

Code Select Expand

double ComputeDeltaD(double latA, double lonA, double latB, double lonB)
{
  const double deg2rad = 0.017453292519943334;
  const double deg2metres = 111194.92664455897;
  const double half = 0.5;
  double delta;

  __asm
  {
      fld     qword ptr [latB]
      fadd    qword ptr [latA]
      fmul    qword ptr [half]
      fmul    qword ptr [deg2rad]
      fcos
      fld     qword ptr [lonA]
      fsub    qword ptr [lonB]
      fmulp   st(1),st
      fmul    st(0),st
      fld     qword ptr [latA]
      fsub    qword ptr [latB]
      fmul    st(0),st
      faddp   st(1),st
      fsqrt
      fmul    qword ptr [deg2metres]
      fstp qword ptr [delta]
  }

  return(delta);
}

[oex]

You might be interested in the Vincenty formula Clive.... I havent had a chance to code it yet (and it's more accurate than I need right now) however it works on an ellipsoidal model of the earth....

It's on (one hard drive of :lol) my 'todo' list however I will post it to the forum when done

[clive]

What, my code = chopped liver?  :P

No, yours looks to be good, my only concern would be treating the global as a spheroid over long distances. I'd probably look to using a WGS84 ellipsoid model. Then again a radio wave is probably travelling significantly further as it bounces between the surface and the atmosphere.

I butchered your routine up a bit to fit it in my test framework (using REAL8 to pass parameters on the stack, and tweaked the radius of the earth to match the one I'm using in metres [6,371,000])

I should do some more testing, but here is a quick comparison

Code Select Expand


11.111100,22.222200 to 11.110800,22.222500
46.735966 metres (ComputeDelta)
46.735966 metres 135.542015 (BearingDistance)

11.111100,22.222200 to 10.991100,22.342200
18696.262895 metres
18696.260965 metres 135.524668

11.111100,22.222200 to 10.811100,22.522200
46747.664995 metres
46747.634890 metres 135.498814

11.111100,22.222200 to 10.124100,23.209200
153886.132556 metres
153885.067293 metres 135.402712


Mine is going to fall over as the curvature of the earth comes into play, and will probably not handle crossing the date line properly. For cars, the fact the roads are not straight (ie non Roman) the estimate is more of a crow-flies number. Mine would be more appropriate for geo-fencing or calling out the distance to a road junction, and where the embedded processor is less capable.

[oex]

and tweaked the radius of the earth to match the one I'm using

I aspire to be able to code as well as Clive :bg

[clive]

You might be interested in the Vincenty formula Clive.... I havent had a chance to code it yet (and it's more accurate than I need right now) however it works on an ellipsoidal model of the earth....

Yes, was just musing the ellipsoid issue in the post I was formulated as you posted. Things tend to get more complex as you pull in specific ellipsoid and datum models, the WGS84 and NAD83 model used by GPS are US centric, for other parts of the world NAD83 is going to be less than ideal for local cartography, although the use of GPS coordinates is probably becoming more prevalent. When dealing with regional maps knowing the datum used in it's creation is going to be important, and how that effects a bitmap/scanned map being pulled into a computer for display.

I need to delve into some of the navigation code to see how some of the maritime gear is handling this, as I've mostly done datum conversion work with ECEF based GPS coordinates (ie ellipsoid models, centre, and distortions wrt WGS84). The other maritime issue is one of bearing and the magnetic deviation (ie true vs compass), and the heading/speed are impacted by wind and currents, etc.

[clive]

and tweaked the radius of the earth to match the one I'm using

I aspire to be able to code as well as Clive :bg

I have a team of elephants and tortoises at my command...

[dedndave]

lol

i was poking around a little and came across this site that has code by Mike Gavaghan
the guy seems to have it worked out, and allows selection of a number of different elipsoids
if he was an assembly programmer, i think we'd be done   :bg
but, alas, he likes C#

the site is a little slow coming up, but probably worth the wait...
http://www.gavaghan.org/blog/free-source-code/geodesy-library-vincentys-formula/

[clive]

Converting Gavaghan's implementation into C (not sure I want the pain to get it to assembler)

Code Select Expand

38.889220,-77.497800 to 45.858890,2.295830 - Lincoln Monument to Eiffle Tower
6600417.926524 metres (ComputeDelta, bogus for such distances)
6323278.164571 metres 54.934371 (BearingDistance, Clive's World 6371.0 km)
6330920.487890 metres 54.934371 (BearingDistance, Greg's World 6378.7 km)
6339542.878390 metres 54.948287 293.854750 (Gavaghan,Vincentys,WGS84)

The C compiler generated about 1100 lines of assembler

[dedndave]

not too far out of the ballpark
thing is....
....we don't know the actual correct distance
maybe you do - you have GPS numbers on them or something ?
i suppose we could walk it off   :bg

[clive]

not too far out of the ballpark
thing is....
....we don't know the actual correct distance
maybe you do - you have GPS numbers on them or something ?
i suppose we could walk it off   :bg

<G>
I took these numbers,

Code Select Expand

    // set Lincoln Memorial coordinates

    GlobalCoordinates lincolnMemorial;

    lincolnMemorial = new GlobalCoordinates(
    new Angle(38.88922), new Angle(-77.04978)

    );

    // set Eiffel Tower coordinates

    GlobalCoordinates eiffelTower;

    eiffelTower = new GlobalCoordinates(
    new Angle(48.85889), new Angle(2.29583)

    );
http://www.gavaghan.org/blog/2007/08/06/c-gps-receivers-and-geocaching-vincentys-formula/

The proposed "course" probably wouldn't be achievable.

I can see a couple of parts of the code that can be folded, and would need to narrow the scope of various intermediate/temporary values to see if more can be held in the x87 registers and discarded once there usefulness has been exhausted. Still it's never going to provide a "fast" solution. If I were navigating I think I'd break the trip into several waypoints. Plus this doesn't handle 3D (probably not an issue for a boat, but certainly for a plane), I would use another algorithm with a higher update rate to recompute for an auto-pilot, and take into account the speed/course over ground being achieved with the current control inputs.

Looks as if I could use FSINCOS in a couple of places
http://siyobik.info/index.php?module=x86&id=115

I suspect we could get Greg's BearingDistance code to work with an ellipsoid model.

[GregL]

oex,

It calculates the bearing (azimuth) and distance between a point and another point.

[GregL]

... my only concern would be treating the global as a spheroid over long distances. I'd probably look to using a WGS84 ellipsoid model.

That's getting a lot more complicated than I wanted or needed to get into.

[oex]

It calculates the bearing (azimuth) and distance between a point and another point.

Oh right I get you, bearing could be quite useful.... I'll take a good look at it :bg

[clive]

That's getting a lot more complicated than I wanted or needed to get into.

Indeed, and not really a criticism, most of my usage tends to be points that are relatively close together. Things like breadcrumbing or odometer where you want to know where something went, but without tracking GPS position dither for a stationary object, but accumulating enough points so the lines Google Earth draws are actually on the road rather than cutting corners and appearing to take cross-country routes over the fields/medians

Some of the more algorithmic optimizations would come from doing crude/course filtering, say at an integer degree level, of huge lists of speed camera locations, too those likely to be reachable in the next hour by a car going at say 120 mph, and then re-evaluating on an hourly basis. The best way to save time is to avoid doing as much pointless computation as possible.

Vincenty does seem like a huge amount of overkill for finding a geocache within walking distance. Using Greg's or my implementation directly in the handheld GPS receiver would make much more sense, I could get a rough angle out of mine by looking at the delta Northing and Easting values. And for that matter GPS receivers don't have an intrinsic sense of direction (compass) so you either have to be moving for them to determine a direction of travel, or have an antenna array to determine orientation.

Unless I'm missing something obvious, why wouldn't the guy download the cache location(s) as waypoints into something like a Garmin eTrex and just use that. Perhaps he can download a list of caches as a Excel sheet or textfile, and spit out a list of viable ones to visit in a certain area and provide an optimal list of ones to visit in a travelling sales man type of way.

[KetilO]

Thanks all for the feedback and advices.

This will probably be the distance / bearing calculations i will use.

Code Select Expand



.const

rad2deg      REAL8 57.29577951308232088
deg2rad      REAL8 0.017453292519943334
deg2metres   REAL8 111194.92664455897
half         REAL8 0.5
dqdiv        REAL8 1000000.0
dq180        REAL8 180.0

.code

Distance proc latA:REAL8,lonA:REAL8,latB:REAL8,lonB:REAL8,lpfDist:PTR REAL10

    fld     REAL8 ptr [latB]
    fadd    REAL8 ptr [latA]
    fmul    REAL8 ptr [half]
    fmul    REAL8 ptr [deg2rad]
    fcos
    fld     REAL8 ptr [lonA]
    fsub    REAL8 ptr [lonB]
    fmulp   st(1),st
    fmul    REAL8 ptr [deg2metres]
    fmul    st(0),st
    fld     REAL8 ptr [latA]
    fsub    REAL8 ptr [latB]
    fmul    REAL8 ptr [deg2metres]
    fmul    st(0),st
    faddp   st(1),st
    fsqrt
    mov    eax,lpfDist
    fstp  REAL10 ptr [eax]
    ret

Distance endp

;Bearing=ATAN2(SIN(lon2-lon1)*COS(lat2),COS(lat1)*SIN(lat2)-SIN(lat1)*COS(lat2)*COS(lon2-lon1)
Bearing proc latA:REAL8,lonA:REAL8,latB:REAL8,lonB:REAL8,lpfBear:PTR REAL10

    ;Convert to radians
    fld     REAL8 ptr [latA]
    fmul    REAL8 ptr [deg2rad]
    fstp    REAL8 ptr [latA]
    fld     REAL8 ptr [latB]
    fmul    REAL8 ptr [deg2rad]
    fstp    REAL8 ptr [latB]
    fld     REAL8 ptr [lonA]
    fmul    REAL8 ptr [deg2rad]
    fstp    REAL8 ptr [lonA]
    fld     REAL8 ptr [lonB]
    fmul    REAL8 ptr [deg2rad]
    fstp    REAL8 ptr [lonB]

    ;x=SIN(lonB-lonA)*COS(latB)
    fld     REAL8 ptr [lonB]
    fsub    REAL8 ptr [lonA]
    fsin
    fld     REAL8 ptr [latB]
    fcos
    fmulp  st(1),st
    ;y=COS(latA)*SIN(latB)-SIN(latA)*COS(latB)*COS(lonB-lonA)
    fld     REAL8 ptr [latA]
    fcos
    fld     REAL8 ptr [latB]
    fsin
    fmulp   st(1),st
    fld     REAL8 ptr [latA]
    fsin
    fld     REAL8 ptr [latB]
    fcos
    fmulp   st(1),st
    fld     REAL8 ptr [lonB]
    fsub    REAL8 ptr [lonA]
    fcos
    fmulp   st(1),st
    fsubp   st(1),st
    ;ATAN2(x,y) NOTE this will fail if y=0
    fdivp   st(1),st
    fld1
    fpatan
    ;Convert to degrees
    fld     REAL8 ptr [rad2deg]
    fmulp   st(1),st
    ;Set result
    mov     edx,lpfBear
    fstp    REAL10 ptr [edx]
    ret

Bearing endp

;In:  Integer Longitude,Lattitude
;Out: REAL10 distance and bearing
BearingDistanceInt proc iLonA:DWORD,iLatA:DWORD,iLonB:DWORD,iLatB:DWORD,lpfDist:PTR REAL10,lpfBear:PTR REAL10
    LOCAL  fLatA:REAL8
    LOCAL  fLonA:REAL8
    LOCAL  fLatB:REAL8
    LOCAL  fLonB:REAL8
    LOCAL  fBear:REAL10

    ;Convert to decimal by dividing with 1 000 000
    fild  iLonA
    fdiv  dqdiv
    fstp  fLonA
    fild  iLatA
    fdiv  dqdiv
    fstp  fLatA
    fild  iLonB
    fdiv  dqdiv
    fstp  fLonB
    fild  iLatB
    fdiv  dqdiv
    fstp  fLatB
    ;Get distance
    invoke Distance,fLatA,fLonA,fLatB,fLonB,lpfDist
    ;Get Bearing
    invoke Bearing,fLatA,fLonA,fLatB,fLonB,lpfBear
    mov    eax,lpfBear
    fld    REAL10 PTR [eax]
    mov    ecx,iLonA
    mov    edx,iLatA
    .if ecx<=iLonB
        ;0 to 180
        .if edx>iLatB
            ;90 to 180
            fadd  REAL8 ptr [dq180]
        .endif
    .else
        ;180 to 360
        fadd  REAL8 ptr [dq180]
        .if edx<=iLatB
            ;270 to 360
            fadd  REAL8 ptr [dq180]
        .endif
    .endif
    fstp  REAL10 PTR [eax]
    ret

BearingDistanceInt endp


KetilO