From 53076747ae4a6686295eebb86adf1f21e6e5f1f3 Mon Sep 17 00:00:00 2001
From: Ronan LE MEILLAT <rtkgps@outlook.com>
Date: Sat, 4 Aug 2018 08:37:23 +0200
Subject: [PATCH] Add supports for RAF09 geoid
---
src/geoid.c | 310 ++++++++++++++++++++++++++++++++++++++++++++++++++--
1 file changed, 303 insertions(+), 7 deletions(-)
diff --git a/src/geoid.c b/src/geoid.c
index 791b5d8..62fa0c0 100644
--- a/src/geoid.c
+++ b/src/geoid.c
@@ -14,12 +14,35 @@
* opengeoid(),closegeoid()
*-----------------------------------------------------------------------------*/
#include "rtklib.h"
+#define RAF09_READ_BUFFER_SIZE 4096
+
+static const char rcsid[]="$Id: geoid.c,v 1.1 2008/07/17 21:48:06 ttaka Exp $";
static const double range[4]; /* embedded geoid area range {W,E,S,N} (deg) */
static const float geoid[361][181]; /* embedded geoid heights (m) (lon x lat) */
static FILE *fp_geoid=NULL; /* geoid file pointer */
static int model_geoid=GEOID_EMBEDDED; /* geoid model */
+typedef struct{
+ int lonSize;
+ int latSize;
+ double minLon;
+ double maxLon;
+ double minLat;
+ double maxLat;
+ double stepLon;
+ double stepLat;
+ int sortOrder;
+ int isCoordinatesPresent;
+ int nvValPerNode;
+ int isPrecisionCodePresent;
+ int translationApplied;
+ int isGridValid;
+ char *name;
+ double **grid;
+}raf09_grid_t;
+static raf09_grid_t raf09_grid;
+
/* bilinear interpolation ----------------------------------------------------*/
static double interpb(const double *y, double a, double b)
{
@@ -46,6 +69,270 @@ static double geoidh_emb(const double *pos)
y[3]=geoid[i2][j2];
return interpb(y,a,b);
}
+
+void free_raf09_grid() {
+ if(raf09_grid.name != NULL) {
+ free(raf09_grid.name);
+ }
+ if (raf09_grid.grid != NULL) {
+ int i;
+ for (i = 0; i < raf09_grid.lonSize; ++i) {
+ free(raf09_grid.grid[i]);
+ }
+ free(raf09_grid.grid);
+ }
+ raf09_grid.latSize = 0;
+ raf09_grid.lonSize = 0;
+ raf09_grid.isGridValid = 0;
+}
+/*------------------------------------------------------------------------------
+ * ign raf09 support
+ */
+size_t raf09_getline(char **lineptr, size_t *n, FILE *stream)
+{
+#ifdef _HAVE_FGETLN_ /* fgetln is faster */
+ char *ptr;
+ size_t len;
+ ptr = fgetln(stream, n);
+ if (ptr == NULL) {
+ return -1;
+ }
+
+ if (*lineptr != NULL) free(*lineptr); /*ensure that the pointer supplied is free*/
+ /* Add one more space for '\0' */
+ len = n[0] + 1;
+ n[0] = len;
+ /* Allocate a new buffer */
+ *lineptr = malloc(len);
+ /* Copy over the string */
+ memcpy(*lineptr, ptr, len-1);
+ /* Write the NULL character */
+ (*lineptr)[len-1] = '\0';
+ /* Return the length of the new buffer */
+ return len;
+#else
+#define OPTIMISTIC_LINE_SIZE 80 /* because a standard text file is 80 chars wide */
+ char *bufptr = NULL;
+ char *p = bufptr;
+ size_t size;
+ int offset;
+ int c;
+
+ if ((lineptr == NULL) || (stream == NULL) || (n == NULL)) {
+ return -1;
+ }
+
+ bufptr = *lineptr;
+ size = *n;
+
+ c = fgetc(stream);
+ if (c == EOF) {
+ return -1;
+ }
+ if (bufptr == NULL) {
+ bufptr = (char *)malloc(OPTIMISTIC_LINE_SIZE); /* alocate an initial buffer */
+ if (bufptr == NULL) {
+ return -1;
+ }
+ size = OPTIMISTIC_LINE_SIZE;
+ }
+ p = bufptr;
+ while(c != EOF) {
+ offset = p - bufptr;
+ if ((p - bufptr) > ((int)size - 1)) { /* there is not enough place in bufptr for storing another char so expand bufptr */
+ size = size + OPTIMISTIC_LINE_SIZE;
+ bufptr = (char *)realloc(bufptr, size);
+ p = bufptr + offset; /* bufptr may have a new location, so be sure to relocate p to its new position */
+ if (bufptr == NULL) {
+ return -1;
+ }
+ }
+ *p++ = c;
+ if (c == '\n') {
+ break;
+ }
+ c = fgetc(stream);
+ }
+
+ *p++ = '\0';
+ *lineptr = bufptr;
+ *n = size;
+
+ return p - bufptr - 1; /*posix 2008 getline does not count termination char*/
+#endif
+}
+void get_raf09_grid(FILE *fp)
+{
+ const char SPACE_CHAR = 0x20;
+ const char NULL_CHAR = 0x0;
+
+ char buffer[RAF09_READ_BUFFER_SIZE];
+ int cFieldCounter;
+ char *geoid_text = NULL;
+ char *pGeoidText;
+ char *pCurrentValue; /*Working pointer on current scanned value*/
+ char *pFirstElement; /*working pointer on the first element*/
+ char *pText; /*Pointer a the end of the line*/
+ size_t read;
+ size_t geoid_text_len=0;
+ int it; /*iterator on longitude*/
+ int nbElementsTotal = 0; /*number of different element in geoid line*/
+ int nbElementsAltitude = 0; /*number of altitude element in geoid line*/
+ int nbElLon,nbElLat,nbElPerNode;
+ int i,j,kLon,lLat; /*i is for counting down from end to start of the line*/
+ /*j is counting up (faster than subtracting), */
+ /*kLon, lLat are indexes in grid grid[kLon][lLat]*/
+
+ raf09_grid.latSize=0;
+ raf09_grid.lonSize=0;
+ raf09_grid.isGridValid=0;
+ raf09_grid.grid = NULL;
+ raf09_grid.name = NULL;
+ if (fp != NULL) {
+ if (feof(fp)==0) {
+ /*read ign mnt header*/
+ /*an IGN type 2 mnt geoid model file is composed of 2 lines:*/
+ /* - first line is the header*/
+ /* - second line is the geoid*/
+ /* see http://geodesie.ign.fr/contenu/fichiers/documentation/grilles/notices/Grilles-MNT-TXT_Formats.pdf*/
+ /* for explanation of format in french*/
+ /*11 numeric fields + 1 string field*/
+ /*the first 11 fields are separated by one space*/
+ /*lines are ended with \r\n*/
+ if (fgets(buffer,RAF09_READ_BUFFER_SIZE,fp) != NULL) {
+ char *pBuffer = buffer;
+ char *fields[12];
+ fields[0]=pBuffer;
+ cFieldCounter = 1;
+ while(pBuffer&&(cFieldCounter<12))
+ {
+ if (*pBuffer==SPACE_CHAR) {
+ *pBuffer = NULL_CHAR;
+ fields[cFieldCounter]=pBuffer+1;
+ cFieldCounter++;
+ }
+ pBuffer++;
+ }
+ if (cFieldCounter == 12) { /* seems OK */
+
+ fields[11][strlen(fields[11])-2]=NULL_CHAR;
+ raf09_grid.name = (char*)malloc( (strlen(fields[11])+1)*sizeof(*fields[11]));
+ strcpy(raf09_grid.name,fields[11]);
+ raf09_grid.minLon=atof(fields[0]);
+ raf09_grid.maxLon=atof(fields[1]);
+ raf09_grid.minLat=atof(fields[2]);
+ raf09_grid.maxLat=atof(fields[3]);
+ raf09_grid.stepLon=atof(fields[4]);
+ raf09_grid.stepLat=atof(fields[5]);
+ raf09_grid.sortOrder = atoi(fields[6]); /* if atoi failed (ie file is wrong) sortOrder = 0 so we will not parse */
+ raf09_grid.isCoordinatesPresent = atoi(fields[7]);
+ raf09_grid.nvValPerNode = atoi(fields[8]);
+ raf09_grid.isPrecisionCodePresent = atoi(fields[9]);
+ raf09_grid.translationApplied = atoi(fields[10]);
+
+ nbElLon=floor((raf09_grid.maxLon-raf09_grid.minLon)/raf09_grid.stepLon)+1; /*do not forget +1 for extremities*/
+ nbElLat=floor((raf09_grid.maxLat-raf09_grid.minLat)/raf09_grid.stepLat)+1;
+
+ /*explanation of nbElPerNode:*/
+ /* minimum 1 element (the altitude component)*/
+ /* if coordinates are present for each node +2 elements*/
+ /* if precision code is present +1 element*/
+ /* maybe 0 if atoi and atof failed in parsing (strange file) so we will stop parsing */
+ nbElPerNode=raf09_grid.nvValPerNode+raf09_grid.isPrecisionCodePresent+(raf09_grid.isCoordinatesPresent?2:0);
+
+ raf09_grid.latSize=nbElLat;
+ raf09_grid.lonSize=nbElLon;
+
+ if ((nbElPerNode>0) && (raf09_grid.sortOrder==2)){
+ read = raf09_getline(&geoid_text,&geoid_text_len,fp);
+ if (read > 0) { /*be sure that we have something to parse*/
+ pGeoidText = geoid_text; /*working pointer on the geoid line*/
+ while(*pGeoidText){ /*during counting we split the geoid line*/
+ if (*pGeoidText==SPACE_CHAR){
+ *pGeoidText=NULL_CHAR;
+ nbElementsTotal++;
+ }
+ pGeoidText++;
+ }
+ nbElementsAltitude = nbElementsTotal/nbElPerNode;
+ if (nbElementsAltitude == (raf09_grid.latSize*raf09_grid.lonSize)){ /*Grid seems valid so we will do parsing*/
+ /*so we need to allocate an in memory grid*/
+ raf09_grid.grid = malloc(raf09_grid.lonSize*sizeof(double*));
+ for(it=0;it<raf09_grid.lonSize;it++)
+ raf09_grid.grid[it] = malloc(raf09_grid.latSize*sizeof(double));
+ pFirstElement = geoid_text;
+ pText = geoid_text+read; /*Pointer a the end of the line*/
+ j=0;kLon=nbElLon-1;lLat=0; /*i is for counting down from end to start of the line*/
+ /*j is counting up (faster than subtracting)*/
+ /*kLon, lLat are indexes in grid grid[kLon][lLat]*/
+ for (i=read;i>0;i--){
+ if(!*pText && *(pText+1)){ /*only if pointer is on \0 and pointer+1 is on a real character*/
+ if( (j+1)%nbElPerNode == 0) /*we keep only the altitude component*/
+ {
+ pCurrentValue = pText+1;
+ raf09_grid.grid[kLon][lLat]=atof(pCurrentValue); /*placing it into the grid*/
+ if(kLon==0){ /*we are going from SE to NW*/
+ kLon=nbElLon-1;
+ lLat++;
+ }else{
+ kLon--;
+ }
+ }
+ j++;
+ }
+ pText--;
+ }
+ /*last value*/
+ raf09_grid.grid[kLon][lLat]=atof(pFirstElement);
+ /*wonderful now we have a double[lon][lat] grid*/
+ raf09_grid.isGridValid = 1;
+ trace(3,"get_raf09_grid: '%s' was parsed correctly\n",raf09_grid.name);
+ }
+ free(geoid_text);
+ } /* if second line was correctly read */
+ }else{ /*if sort order is not ==2*/
+ trace(2,"sort order %d of IGN mnt geoid model is not yet supported\n",raf09_grid.sortOrder);
+ }
+ } /* test 12 fields */
+ } /* test if fgets of first line succeed */
+ }
+ } else { /*if fopen failed*/
+ trace(2,"Geoid model fopen failed\n");
+ }
+
+}
+
+static double geoidh_raf09(const double *pos)
+{
+ double a,b,y[4],posd[2];
+ int lat1,lat2,lon1,lon2;
+
+ if (raf09_grid.isGridValid == 0) {
+ trace(2,"RAF09 grid is invalid get RAF09.mnt from http://geodesie.ign.fr/contenu/fichiers/documentation/grilles/metropole/RAF09.mnt");
+ return 0.0;
+ }
+
+ posd[0]=pos[0];
+ posd[1]=(pos[1]>180?pos[1]-360:pos[1]);
+
+ if (posd[1]>raf09_grid.maxLon||
+ posd[1]<raf09_grid.minLon||
+ posd[0]>raf09_grid.maxLat||
+ posd[0]<raf09_grid.minLat) {
+ trace(2,"out of geoid model range: lat=%.3f lon=%.3f\n",pos[0],posd[1]);
+ return 0.0;
+ }
+
+ a=(posd[0]-raf09_grid.minLat)/raf09_grid.stepLat;
+ b=(posd[1]-raf09_grid.minLon)/raf09_grid.stepLon;
+ lat1=(int)a; a-=lat1; lat2=lat1<raf09_grid.latSize-1?lat1+1:lat1;
+ lon1=(int)b; b-=lon1; lon2=lon1<raf09_grid.lonSize-1?lon1+1:lon1;
+ y[0]=raf09_grid.grid[lon1][lat1];
+ y[1]=raf09_grid.grid[lon2][lat1];
+ y[2]=raf09_grid.grid[lon1][lat2];
+ y[3]=raf09_grid.grid[lon2][lat2];
+ return interpb(y,a,b);
+}
/* get 2 byte signed integer from file ---------------------------------------*/
static short fget2b(FILE *fp, long off)
{
@@ -64,7 +351,7 @@ static double geoidh_egm96(const double *pos)
long i1,i2,j1,j2;
if (!fp_geoid) return 0.0;
-
+
a=(pos[1]-lon0)/dlon;
b=(pos[0]-lat0)/dlat;
i1=(long)a; a-=i1; i2=i1<nlon-1?i1+1:0;
@@ -92,9 +379,9 @@ static double geoidh_egm08(const double *pos, int model)
double a,b,y[4];
long i1,i2,j1,j2;
int nlon,nlat;
-
+
if (!fp_geoid) return 0.0;
-
+
if (model==GEOID_EGM2008_M25) { /* 2.5 x 2.5" grid */
dlon= 2.5/60.0;
dlat=-2.5/60.0;
@@ -203,7 +490,8 @@ extern int opengeoid(int model, const char *file)
return 1;
}
if (model!=GEOID_EGM96_M150 &&model!=GEOID_EGM2008_M25&&
- model!=GEOID_EGM2008_M10&&model!=GEOID_GSI2000_M15) {
+ model!=GEOID_EGM2008_M10&&model!=GEOID_GSI2000_M15&&
+ model!=GEOID_RAF09) {
trace(2,"invalid geoid model: model=%d file=%s\n",model,file);
return 0;
}
@@ -211,6 +499,9 @@ extern int opengeoid(int model, const char *file)
trace(2,"geoid model file open error: model=%d file=%s\n",model,file);
return 0;
}
+ if (model==GEOID_RAF09) {
+ get_raf09_grid(fp_geoid);
+ }
model_geoid=model;
return 1;
}
@@ -222,8 +513,11 @@ extern int opengeoid(int model, const char *file)
extern void closegeoid(void)
{
trace(3,"closegoid:\n");
-
+
if (fp_geoid) fclose(fp_geoid);
+ if (model_geoid==GEOID_RAF09) {
+ free_raf09_grid();
+ }
fp_geoid=NULL;
model_geoid=GEOID_EMBEDDED;
}
@@ -238,9 +532,9 @@ extern void closegeoid(void)
extern double geoidh(const double *pos)
{
double posd[2],h;
-
+
posd[1]=pos[1]*R2D; posd[0]=pos[0]*R2D; if (posd[1]<0.0) posd[1]+=360.0;
-
+
if (posd[1]<0.0||360.0-1E-12<posd[1]||posd[0]<-90.0||90.0<posd[0]) {
trace(2,"out of range for geoid model: lat=%.3f lon=%.3f\n",posd[0],posd[1]);
return 0.0;
@@ -251,6 +545,7 @@ extern double geoidh(const double *pos)
case GEOID_EGM2008_M25: h=geoidh_egm08(posd,model_geoid); break;
case GEOID_EGM2008_M10: h=geoidh_egm08(posd,model_geoid); break;
case GEOID_GSI2000_M15: h=geoidh_gsi (posd); break;
+ case GEOID_RAF09: h=geoidh_raf09(posd); break;
default: return 0.0;
}
if (fabs(h)>200.0) {
@@ -259,6 +554,7 @@ extern double geoidh(const double *pos)
}
return h;
}
+
/*------------------------------------------------------------------------------
* embedded geoid model
* notes : geoid heights are derived from EGM96 (1 x 1 deg grid)
--
GitLab