c_cpp ST_Segmentize.c

Posted 2021-05-14

/**
* Given a starting location r, a distance and an azimuth
* to the new point, compute the location of the projected point on the unit sphere.
*/
int sphere_project(const GEOGRAPHIC_POINT *r, double distance, double azimuth, GEOGRAPHIC_POINT *n)
{
	double d = distance;
	double lat1 = r->lat;
	double lon1 = r->lon;
	double lat2, lon2;

	lat2 = asin(sin(lat1)*cos(d) + cos(lat1)*sin(d)*cos(azimuth));

	/* If we're going straight up or straight down, we don't need to calculate the longitude */
	/* TODO: this isn't quite true, what if we're going over the pole? */
	if ( FP_EQUALS(azimuth, M_PI) || FP_EQUALS(azimuth, 0.0) )
	{
		lon2 = r->lon;
	}
	else
	{
		lon2 = lon1 + atan2(sin(azimuth)*sin(d)*cos(lat1), cos(d)-sin(lat1)*sin(lat2));
	}
	
	if ( isnan(lat2) || isnan(lon2) )
		return LW_FAILURE;

	n->lat = lat2;
	n->lon = lon2;

	return LW_SUCCESS;
}

/**
* Create a new point array with no segment longer than the input segment length (expressed in radians!)
* @param pa_in - input point array pointer
* @param max_seg_length - maximum output segment length in radians
*/
static POINTARRAY*
ptarray_segmentize_sphere(const POINTARRAY *pa_in, double max_seg_length)
{
	POINTARRAY *pa_out;
	int hasz = ptarray_has_z(pa_in);
	int hasm = ptarray_has_m(pa_in);
	int pa_in_offset = 0; /* input point offset */
	POINT4D p1, p2, p;
	GEOGRAPHIC_POINT g1, g2, g;
	double d;
	
	/* Just crap out on crazy input */
	if ( ! pa_in )
		lwerror("ptarray_segmentize_sphere: null input pointarray");
	if ( max_seg_length <= 0.0 )	
		lwerror("ptarray_segmentize_sphere: maximum segment length must be positive");

	/* Empty starting array */
	pa_out = ptarray_construct_empty(hasz, hasm, pa_in->npoints);

	/* Add first point */
	getPoint4d_p(pa_in, pa_in_offset, &p1);
	ptarray_append_point(pa_out, &p1, LW_FALSE);
	geographic_point_init(p1.x, p1.y, &g1);
	pa_in_offset++;
	
	while ( pa_in_offset < pa_in->npoints )
	{
		getPoint4d_p(pa_in, pa_in_offset, &p2);
		geographic_point_init(p2.x, p2.y, &g2);
		
		/* Skip duplicate points (except in case of 2-point lines!) */
		if ( (pa_in->npoints > 2) && p4d_same(&p1, &p2) )
		{
			/* Move one offset forward */
			p1 = p2;
			g1 = g2;
			pa_in_offset++;
			continue;
		}

		/* How long is this edge? */
		d = sphere_distance(&g1, &g2);
		
		/* We need to segmentize this edge */
		if ( d > max_seg_length )
		{
			int nsegs = 1 + d / max_seg_length;
			int i;
			double dzz = 0, dmm = 0;
			double delta = d / nsegs;

			/* The independent Z/M values on the ptarray */
			if ( hasz ) dzz = (p2.z - p1.z) / nsegs;
			if ( hasm ) dmm = (p2.m - p1.m) / nsegs;
			
			g = g1;
			p = p1;
			for ( i = 0; i < nsegs - 1; i++ )
			{
				GEOGRAPHIC_POINT gn;
				double heading;

				/* Compute the current heading to the destination */
				heading = sphere_direction(&g, &g2, (nsegs-i) * delta);
				/* Move one increment forwards */
				sphere_project(&g, delta, heading, &gn);
				g = gn;

				p.x = rad2deg(g.lon);
				p.y = rad2deg(g.lat);
				if ( hasz )
					p.z += dzz;
				if ( hasm )
					p.m += dmm;
				ptarray_append_point(pa_out, &p, LW_FALSE);
			}
			
			ptarray_append_point(pa_out, &p2, LW_FALSE);
		}
		/* This edge is already short enough */
		else
		{
			ptarray_append_point(pa_out, &p2, (pa_in->npoints==2)?LW_TRUE:LW_FALSE);
		}

		/* Move one offset forward */
		p1 = p2;
		g1 = g2;
		pa_in_offset++;
	}
	
	return pa_out;	
}