1 files changed, 233 insertions, 0 deletions
diff --git a/sys/mwcs/wfsamp.x b/sys/mwcs/wfsamp.x
new file mode 100644
index 00000000..992f3211
--- /dev/null
+++ b/sys/mwcs/wfsamp.x
@@ -0,0 +1,233 @@
+# Copyright(c) 1986 Association of Universities for Research in Astronomy Inc.
+
+include	"mwcs.h"
+
+.help WFSAMP
+.nf -------------------------------------------------------------------------
+WFSAMP -- WCS function driver for the one dimensional sampled wcs function.
+For this driver, the function P<->W (physical to/from world) is defined by
+a sampled WCS curve.
+
+Driver routines:
+
+	FN_INIT		    wf_smp_init (fc, dir)
+	FN_DESTROY			(none)
+	FN_FWD		   wf_smp_ctran (fc, v1, v2)
+	FN_INV				(same)
+
+In this initial implementation, only linear interpolation of the sampled
+curve is provided, but the driver is easily extended to provide additional
+interpolators.  NOTE that this entire driver assumes that the sampled function
+is monotonic.
+.endhelp --------------------------------------------------------------------
+
+# Driver specific fields of function call (FC) descriptor.
+define	FC_NPTS		Memi[$1+FCU]		# number of points in curve
+define	FC_LOC		Memi[$1+FCU+1]		# location in IN vector
+define	FC_V1		Memi[$1+FCU+2]		# pointer to IN vector
+define	FC_V2		Memi[$1+FCU+3]		# pointer to OUT vector
+define	FC_W		Memd[P2D($1+FCU+4)]	# W value (CRVAL)
+define	FC_DIR		Memi[$1+FCU+6]		# direction of transform
+
+
+# WF_SMP_INIT -- Initialize the function call descriptor for the indicated
+# type of transform (forward or inverse).
+
+procedure wf_smp_init (fc, dir)
+
+pointer	fc			#I pointer to FC descriptor
+int	dir			#I type of transformation
+
+int	axis, npts
+pointer	wp, mw, sp, emsg, pv, wv
+
+begin
+	# Enforce the current restriction to 1-dim sampled functions.
+	if (FC_NAXES(fc) != 1)
+	    call error (1, "Sampled wcs functions must be 1-dimensional")
+
+	wp = FC_WCS(fc)
+	mw = CT_MW(FC_CT(fc))
+	axis = CT_AXIS(FC_CT(fc),1)
+
+	# Get pointers to the input and output sample vectors.  For our
+	# purposes there is no difference between the forward and inverse
+	# transform; we just swap the vectors for the inverse transform.
+	# The use of direct pointers here assumes that the DBUF is not
+	# reallocated while the CTRAN is being used.
+
+	npts = WCS_NPTS(wp,axis)
+	pv   = WCS_PV(wp,axis)
+	wv   = WCS_WV(wp,axis)
+
+	# Verify that we have a sampled WCS for this axis.
+	if (npts <= 0 || pv == NULL || wv == NULL) {
+	    call smark (sp)
+	    call salloc (emsg, SZ_LINE, TY_CHAR)
+	    call sprintf (Memc[emsg], SZ_LINE,
+		"No sampled wcs entered for axis %d")
+		call pargi (axis)
+	    call error (2, Memc[emsg])
+	    call sfree (sp)
+	}
+
+	if (dir == FORWARD) {
+	    FC_V1(fc) = MI_DBUF(mw) + pv - 1
+	    FC_V2(fc) = MI_DBUF(mw) + wv - 1
+	} else {
+	    FC_V1(fc) = MI_DBUF(mw) + wv - 1
+	    FC_V2(fc) = MI_DBUF(mw) + pv - 1
+	}
+
+	FC_NPTS(fc) = npts
+	if (WCS_W(wp) == NULL)
+	    FC_W(fc) = 0.0
+	else
+	    FC_W(fc) = D(mw,WCS_W(wp)+axis-1)
+
+	FC_LOC(fc) = 1
+	FC_DIR(fc) = dir
+end
+
+
+# WF_SMP_CTRAN -- Given the coordinates of a point X in the input curve,
+# locate the sample interval containing the point, and return the coordinate
+# of the same point in the output curve using simple linear interpolation
+# (currently) to evaluate the WCS function value.
+
+procedure wf_smp_ctran (fc, a_x, a_y)
+
+pointer	fc			#I pointer to FC descriptor
+double	a_x			#I point to sample WCS at
+double	a_y			#O value of WCS at that point
+
+int	index, i, step
+double	frac, x, y
+pointer	ip, op, i1, i2
+int	wf_smp_binsearch()
+define	sample_ 91
+define	oor_ 92
+
+begin
+	# Get the input X value.
+	if (FC_DIR(fc) == FORWARD)
+	    x = a_x
+	else
+	    x = a_x - FC_W(fc)
+
+	# Check for out of bounds and set step.
+	i1 = FC_V1(fc)
+	i2 = i1 + FC_NPTS(fc) - 1
+	if (Memd[i1] <= Memd[i2]) {
+	    if (x < Memd[i1] || x > Memd[i2])
+		goto oor_
+	    step = 1
+	} else {
+	    if (x < Memd[i2] || x > Memd[i1])
+		goto oor_
+	    step = -1
+	}
+
+	# Check the endpoints and the last inverval to optimize the case of
+	# repeated samplings of the same region of the curve.
+
+	if (x == Memd[i1])
+	    ip = i1 - min (0, step)
+	else if (x == Memd[i2])
+	    ip = i2 - max (0, step)
+	else
+	    ip = FC_LOC(fc) + i1 - 1
+	if (Memd[ip] <= x && x <= Memd[ip+step])
+	    goto sample_
+
+	# Next check several intervals to either side.
+	if (x < Memd[ip]) {
+	    do i = 1, 5 {
+		ip = ip - step
+		if (Memd[ip] <= x)
+		    goto sample_
+	    }
+	} else {
+	    do i = 1, 5 {
+		if (Memd[ip+step] >= x)
+		    goto sample_
+		ip = ip + step
+	    }
+	}
+
+	# Give up and do a full binary search!
+	index = wf_smp_binsearch (x, Memd[i1], FC_NPTS(fc))
+	if (index == 0)
+	    goto oor_
+	else
+	    ip = i1 + index - 1
+
+	# Having found the proper interval, compute the function value by
+	# interpolating the output vector.
+sample_
+	op = FC_V2(fc) + ip-i1
+	frac = (x - Memd[ip]) / (Memd[ip+step] - Memd[ip])
+	y = (Memd[op+step] - Memd[op]) * frac + Memd[op]
+
+	# Get the output Y value.
+	if (FC_DIR(fc) == FORWARD)
+	    a_y = y
+	else
+	    a_y = y + FC_W(fc)
+
+	# Save last location.
+	FC_LOC(fc) = ip - i1 + 1
+
+	return
+oor_
+	# Given X value is not in the region covered by the sampled curve,
+	# or at least we couldn't find it with a binary search.
+
+	call error (2, "Out of bounds reference on sampled WCS curve")
+end
+
+
+# WF_SMP_BINSEARCH -- Perform a binary search of a sorted array for the
+# interval containing the given point.
+
+int procedure wf_smp_binsearch (x, v, npts)
+
+double	x				#I point we want interval for
+double	v[ARB]				#I array to be searched
+int	npts				#I number of points in array
+
+int	low, high, pos, i
+
+begin
+	low = 1
+	high = max (1, npts)
+
+	# Cut range of search in half until interval is found, or until range
+	# vanishes (high - low <= 1).
+
+	if (v[1] < v[npts]) {
+	    do i = 1, npts {
+		pos = min ((high - low) / 2 + low, npts-1)
+		if (pos == low)
+		    return (0)				# not found
+		else if (v[pos] <= x && x <= v[pos+1])
+		    return (pos)
+		else if (x < v[pos])
+		    high = pos
+		else
+		    low = pos
+	    }
+	} else {
+	    do i = 1, npts {
+		pos = min ((high - low) / 2 + low, npts-1)
+		if (pos == low)
+		    return (0)				# not found
+		else if (v[pos+1] <= x && x <= v[pos])
+		    return (pos+1)
+		else if (x > v[pos])
+		    high = pos
+		else
+		    low = pos
+	    }
+	}
+end