Add supports for RAF09 geoid

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)