Willem Melching / Mbed 2 deprecated bootje_v4

Dependencies:   mbed

verwerking/rhumb.cpp@0:bec310bde899, 2011-09-27 (annotated)

Committer:
pd0wm
Date:
Tue Sep 27 19:46:30 2011 +0000
Revision:
0:bec310bde899

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


pd0wm

0:bec310bde899


     1

#include "rhumb.h"





pd0wm

0:bec310bde899


     2

#include <math.h>





pd0wm

0:bec310bde899


     3







pd0wm

0:bec310bde899


     4







pd0wm

0:bec310bde899


     5

double afstand(pos *cur, pos *tar){





pd0wm

0:bec310bde899


     6

    double lat_cur_r = cur->lat * deg2rad;





pd0wm

0:bec310bde899


     7

    double lon_cur_r = cur->lon * deg2rad;





pd0wm

0:bec310bde899


     8

    double lat_tar_r = tar->lat * deg2rad;





pd0wm

0:bec310bde899


     9

    double lon_tar_r = tar->lon * deg2rad;





pd0wm

0:bec310bde899


    10

    





pd0wm

0:bec310bde899


    11

    double df = log(tan(lat_tar_r/2.0 + PI/4.0)/tan(lat_cur_r/2.0 + PI/4.0));





pd0wm

0:bec310bde899


    12







pd0wm

0:bec310bde899


    13







pd0wm

0:bec310bde899


    14

    double dla = lat_tar_r - lat_cur_r;





pd0wm

0:bec310bde899


    15

    double dlo = lon_tar_r - lon_cur_r;





pd0wm

0:bec310bde899


    16







pd0wm

0:bec310bde899


    17

    double q;





pd0wm

0:bec310bde899


    18

    





pd0wm

0:bec310bde899


    19

    if (df == 0)





pd0wm

0:bec310bde899


    20

        q = cos(lat_cur_r);





pd0wm

0:bec310bde899


    21

    else





pd0wm

0:bec310bde899


    22

        q = dla / df;





pd0wm

0:bec310bde899


    23

    





pd0wm

0:bec310bde899


    24

        





pd0wm

0:bec310bde899


    25

    double r = 6371.0;





pd0wm

0:bec310bde899


    26

    





pd0wm

0:bec310bde899


    27







pd0wm

0:bec310bde899


    28

    double d = sqrt(pow(dla,2) + pow(q,2) * pow(dlo,2)) * r;





pd0wm

0:bec310bde899


    29

    return d;





pd0wm

0:bec310bde899


    30







pd0wm

0:bec310bde899


    31







pd0wm

0:bec310bde899


    32

}





pd0wm

0:bec310bde899


    33







pd0wm

0:bec310bde899


    34

double koers(pos *cur, pos *tar){





pd0wm

0:bec310bde899


    35

    double lat_cur_r = cur->lat * deg2rad;





pd0wm

0:bec310bde899


    36

    double lon_cur_r = cur->lon * deg2rad;





pd0wm

0:bec310bde899


    37

    double lat_tar_r = tar->lat * deg2rad;





pd0wm

0:bec310bde899


    38

    double lon_tar_r = tar->lon * deg2rad;





pd0wm

0:bec310bde899


    39

    





pd0wm

0:bec310bde899


    40

    double df = log(tan(lat_tar_r/2.0 + PI/4.0)/tan(lat_cur_r/2.0 + PI/4.0));





pd0wm

0:bec310bde899


    41







pd0wm

0:bec310bde899


    42







pd0wm

0:bec310bde899


    43

    double dla = lat_tar_r - lat_cur_r;





pd0wm

0:bec310bde899


    44

    double dlo = lon_tar_r - lon_cur_r;





pd0wm

0:bec310bde899


    45







pd0wm

0:bec310bde899


    46

    





pd0wm

0:bec310bde899


    47

    double k = atan2(dlo,df) * rad2deg;





pd0wm

0:bec310bde899


    48

    if (k < 0)





pd0wm

0:bec310bde899


    49

        k += 360;





pd0wm

0:bec310bde899


    50







pd0wm

0:bec310bde899


    51

    return k;





pd0wm

0:bec310bde899


    52







pd0wm

0:bec310bde899


    53







pd0wm

0:bec310bde899


    54

}