c_cpp LWGEOM_line_interpolate_point.c

Posted 2021-05-14

/*
 * Given a point, returns the location of closest point on pointarray
 * and, optionally, it's actual distance from the point array.
 */
double
ptarray_locate_point(const POINTARRAY *pa, const POINT4D *p4d, double *mindistout, POINT4D *proj4d)
{
	double mindist=-1;
	double tlen, plen;
	int t, seg=-1;
	POINT4D	start4d, end4d, projtmp;
	POINT2D proj, p;
	const POINT2D *start = NULL, *end = NULL;

	/* Initialize our 2D copy of the input parameter */
	p.x = p4d->x;
	p.y = p4d->y;

	if ( ! proj4d ) proj4d = &projtmp;

	start = getPoint2d_cp(pa, 0);

	/* If the pointarray has only one point, the nearest point is */
	/* just that point */
	if ( pa->npoints == 1 )
	{
		getPoint4d_p(pa, 0, proj4d);
		if ( mindistout )
			*mindistout = distance2d_pt_pt(&p, start);
		return 0.0;
	}

	/* Loop through pointarray looking for nearest segment */
	for (t=1; t<pa->npoints; t++)
	{
		double dist;
		end = getPoint2d_cp(pa, t);
		dist = distance2d_pt_seg(&p, start, end);

		if (t==1 || dist < mindist )
		{
			mindist = dist;
			seg=t-1;
		}

		if ( mindist == 0 )
		{
			LWDEBUG(3, "Breaking on mindist=0");
			break;
		}

		start = end;
	}

	if ( mindistout ) *mindistout = mindist;

	LWDEBUGF(3, "Closest segment: %d", seg);
	LWDEBUGF(3, "mindist: %g", mindist);

	/*
	 * We need to project the
	 * point on the closest segment.
	 */
	getPoint4d_p(pa, seg, &start4d);
	getPoint4d_p(pa, seg+1, &end4d);
	closest_point_on_segment(p4d, &start4d, &end4d, proj4d);

	/* Copy 4D values into 2D holder */
	proj.x = proj4d->x;
	proj.y = proj4d->y;

	LWDEBUGF(3, "Closest segment:%d, npoints:%d", seg, pa->npoints);

	/* For robustness, force 1 when closest point == endpoint */
	if ( (seg >= (pa->npoints-2)) && p2d_same(&proj, end) )
	{
		return 1.0;
	}

	LWDEBUGF(3, "Closest point on segment: %g,%g", proj.x, proj.y);

	tlen = ptarray_length_2d(pa);

	LWDEBUGF(3, "tlen %g", tlen);

	/* Location of any point on a zero-length line is 0 */
	/* See http://trac.osgeo.org/postgis/ticket/1772#comment:2 */
	if ( tlen == 0 ) return 0;

	plen=0;
	start = getPoint2d_cp(pa, 0);
	for (t=0; t<seg; t++, start=end)
	{
		end = getPoint2d_cp(pa, t+1);
		plen += distance2d_pt_pt(start, end);

		LWDEBUGF(4, "Segment %d made plen %g", t, plen);
	}

	plen+=distance2d_pt_pt(&proj, start);

	LWDEBUGF(3, "plen %g, tlen %g", plen, tlen);

	return plen/tlen;
}

ptarray_length_2d(const POINTARRAY *pts)
{
	double dist = 0.0;
	int i;
	const POINT2D *frm;
	const POINT2D *to;

	if ( pts->npoints < 2 ) return 0.0;

	frm = getPoint2d_cp(pts, 0);

	for ( i=1; i < pts->npoints; i++ )
	{
		to = getPoint2d_cp(pts, i);

		dist += sqrt( ((frm->x - to->x)*(frm->x - to->x))  +
		              ((frm->y - to->y)*(frm->y - to->y)) );

		frm = to;
	}
	return dist;
}

/**
 * Find interpolation point I
 * between point A and point B
 * so that the len(AI) == len(AB)*F
 * and I falls on AB segment.
 *
 * Example:
 *
 *   F=0.5  :    A----I----B
 *   F=1    :    A---------B==I
 *   F=0    : A==I---------B
 *   F=.2   :    A-I-------B
 */
void
interpolate_point4d(POINT4D *A, POINT4D *B, POINT4D *I, double F)
{
#if PARANOIA_LEVEL > 0
	double absF=fabs(F);
	if ( absF < 0 || absF > 1 )
	{
		lwerror("interpolate_point4d: invalid F (%g)", F);
	}
#endif
	I->x=A->x+((B->x-A->x)*F);
	I->y=A->y+((B->y-A->y)*F);
	I->z=A->z+((B->z-A->z)*F);
	I->m=A->m+((B->m-A->m)*F);
}

Datum LWGEOM_line_locate_point(PG_FUNCTION_ARGS);
PG_FUNCTION_INFO_V1(LWGEOM_line_locate_point);
Datum LWGEOM_line_locate_point(PG_FUNCTION_ARGS)
{
	GSERIALIZED *geom1 = PG_GETARG_GSERIALIZED_P(0);
	GSERIALIZED *geom2 = PG_GETARG_GSERIALIZED_P(1);
	LWLINE *lwline;
	LWPOINT *lwpoint;
	POINTARRAY *pa;
	POINT4D p, p_proj;
	double ret;

	if ( gserialized_get_type(geom1) != LINETYPE )
	{
		elog(ERROR,"line_locate_point: 1st arg isn't a line");
		PG_RETURN_NULL();
	}
	if ( gserialized_get_type(geom2) != POINTTYPE )
	{
		elog(ERROR,"line_locate_point: 2st arg isn't a point");
		PG_RETURN_NULL();
	}

	error_if_srid_mismatch(gserialized_get_srid(geom1), gserialized_get_srid(geom2));

	lwline = lwgeom_as_lwline(lwgeom_from_gserialized(geom1));
	lwpoint = lwgeom_as_lwpoint(lwgeom_from_gserialized(geom2));

	pa = lwline->points;
	lwpoint_getPoint4d_p(lwpoint, &p);

	ret = ptarray_locate_point(pa, &p, NULL, &p_proj);

	PG_RETURN_FLOAT8(ret);
}

/***********************************************************************
 * [e-mail];
 ***********************************************************************/

/***********************************************************************
 * Interpolate a point along a line, useful for Geocoding applications
 * SELECT line_interpolate_point( 'LINESTRING( 0 0, 2 2'::geometry, .5 )
 * returns POINT( 1 1 ).
 * Works in 2d space only.
 *
 * Initially written by: [e-mail]
 * Ported to LWGEOM by: [e-mail]
 ***********************************************************************/

Datum LWGEOM_line_interpolate_point(PG_FUNCTION_ARGS);

PG_FUNCTION_INFO_V1(LWGEOM_line_interpolate_point);
Datum LWGEOM_line_interpolate_point(PG_FUNCTION_ARGS)
{
	GSERIALIZED *gser = PG_GETARG_GSERIALIZED_P(0);
	GSERIALIZED *result;
	double distance = PG_GETARG_FLOAT8(1);
	LWLINE *line;
	LWGEOM *geom;
	LWPOINT *point;
	POINTARRAY *ipa, *opa;
	POINT4D pt;
	int nsegs, i;
	double length, slength, tlength;

	if ( distance < 0 || distance > 1 )
	{
		elog(ERROR,"line_interpolate_point: 2nd arg isn't within [0,1]");
		PG_RETURN_NULL();
	}

	if ( gserialized_get_type(gser) != LINETYPE )
	{
		elog(ERROR,"line_interpolate_point: 1st arg isn't a line");
		PG_RETURN_NULL();
	}

	/* Empty.InterpolatePoint == Point Empty */
	if ( gserialized_is_empty(gser) )
	{
		point = lwpoint_construct_empty(gserialized_get_srid(gser), gserialized_has_z(gser), gserialized_has_m(gser));
		result = geometry_serialize(lwpoint_as_lwgeom(point));
		lwpoint_free(point);
		PG_RETURN_POINTER(result);
	}

	geom = lwgeom_from_gserialized(gser);
	line = lwgeom_as_lwline(geom);
	ipa = line->points;

	/* If distance is one of the two extremes, return the point on that
	 * end rather than doing any expensive computations
	 */
	if ( distance == 0.0 || distance == 1.0 )
	{
		if ( distance == 0.0 )
			getPoint4d_p(ipa, 0, &pt);
		else
			getPoint4d_p(ipa, ipa->npoints-1, &pt);

		opa = ptarray_construct(lwgeom_has_z(geom), lwgeom_has_m(geom), 1);
		ptarray_set_point4d(opa, 0, &pt);
		
		point = lwpoint_construct(line->srid, NULL, opa);
		PG_RETURN_POINTER(geometry_serialize(lwpoint_as_lwgeom(point)));
	}

	/* Interpolate a point on the line */
	nsegs = ipa->npoints - 1;
	length = ptarray_length_2d(ipa);
	tlength = 0;
	for ( i = 0; i < nsegs; i++ )
	{
		POINT4D p1, p2;
		POINT4D *p1ptr=&p1, *p2ptr=&p2; /* don't break
						                                 * strict-aliasing rules
						                                 */

		getPoint4d_p(ipa, i, &p1);
		getPoint4d_p(ipa, i+1, &p2);

		/* Find the relative length of this segment */
		slength = distance2d_pt_pt((POINT2D*)p1ptr, (POINT2D*)p2ptr)/length;

		/* If our target distance is before the total length we've seen
		 * so far. create a new point some distance down the current
		 * segment.
		 */
		if ( distance < tlength + slength )
		{
			double dseg = (distance - tlength) / slength;
			interpolate_point4d(&p1, &p2, &pt, dseg);
			opa = ptarray_construct(lwgeom_has_z(geom), lwgeom_has_m(geom), 1);
			ptarray_set_point4d(opa, 0, &pt);
			point = lwpoint_construct(line->srid, NULL, opa);
			PG_RETURN_POINTER(geometry_serialize(lwpoint_as_lwgeom(point)));
		}
		tlength += slength;
	}

	/* Return the last point on the line. This shouldn't happen, but
	 * could if there's some floating point rounding errors. */
	getPoint4d_p(ipa, ipa->npoints-1, &pt);
	opa = ptarray_construct(lwgeom_has_z(geom), lwgeom_has_m(geom), 1);
	ptarray_set_point4d(opa, 0, &pt);
	point = lwpoint_construct(line->srid, NULL, opa);
	PG_FREE_IF_COPY(gser, 0);
	PG_RETURN_POINTER(geometry_serialize(lwpoint_as_lwgeom(point)));
}