src/Translate.jl

######################################
# Image subpixel accuracy translation
######################################

module Translate

##########
# Exports
##########

export interpolate2d,
       interpolate2d_quadrants,
       extract_subimage!,
       translate_image!,
       DisplacementFull,
       DisplacementConstant,
       Displacement,
       Image

##################
# Types
##################

# Two different types of displacement data supported:
#  a) given in each pixel
#  b) constant in space
Image = Array{Float64,2}
DisplacementFull = Array{Float64,3}
DisplacementConstant = Array{Float64,1}
Displacement = Union{DisplacementFull,DisplacementConstant}

#############################
# Base interpolation routine
#############################

@inline function interpolate2d_quadrants(v, (x, y))
    (m, n) = size(v)
    clipx = xʹ -> max(1, min(xʹ, m))
    clipy = yʹ -> max(1, min(yʹ, n))

    xfℤ = clipx(floor(Int, x))
    xcℤ = clipx(ceil(Int, x))
    yfℤ = clipy(floor(Int, y))
    ycℤ = clipy(ceil(Int, y))
       
    xf = convert(Float64, xfℤ)
    xc = convert(Float64, xcℤ)
    yf = convert(Float64, yfℤ)
    yc = convert(Float64, ycℤ)
    xm = (xf+xc)/2
    ym = (yf+yc)/2

    vff = @inbounds v[xfℤ, yfℤ]
    vfc = @inbounds v[xfℤ, ycℤ]
    vcf = @inbounds v[xcℤ, yfℤ]
    vcc = @inbounds v[xcℤ, ycℤ]
    vmm = (vff+vfc+vcf+vcc)/4

    if xfℤ==xcℤ
        if yfℤ==ycℤ
            # Completely degenerate case
            v = vmm
        else
            # Degenerate x
            v = vff+(y-yf)/(yc-yf)*(vfc-vff)
        end
    elseif yfℤ==ycℤ
        # Degenerate y
        v = vff + (x-xf)/(xc-xf)*(vcf-vff)
    elseif y-ym ≥ x-xm
        # top-left half
        if (y-ym) + (x-xm) ≥ 0
            # top quadrant
            v = vfc + (x-xf)/(xc-xf)*(vcc-vfc) + (y-yc)/(ym-yc)*(vmm-(vcc+vfc)/2)
        else
            # left quadrant
            v = vff + (y-yf)/(yc-yf)*(vfc-vff) + (x-xf)/(xm-xf)*(vmm-(vfc+vff)/2)
        end
    else
        # bottom-left half
        if (y-ym) + (x-xm) ≥ 0
            # right quadrant
            v = vcf + (y-yf)/(yc-yf)*(vcc-vcf) + (x-xc)/(xm-xc)*(vmm-(vcc+vcf)/2)
        else
            # bottom quadrant
            v = vff + (x-xf)/(xc-xf)*(vcf-vff) + (y-yf)/(ym-yf)*(vmm-(vcf+vff)/2)
        end
    end

    return v
end

interpolate2d = interpolate2d_quadrants

##############
# Translation
##############

@polly function translate_image!(x, z, u::DisplacementFull)
    @assert(size(u, 1)==2 && size(x)==size(u)[2:end] && size(x)==size(z))

    @inbounds @simd for i=1:size(x, 1)
        @simd for j=1:size(x, 2)
            pt = (i - u[1, i, j], j - u[2, i, j])
            x[i, j] = interpolate2d(z, pt)
        end
    end
end

@polly function translate_image!(x, z, u::DisplacementConstant)
    @assert(size(u)==(2,) && size(x)==size(z))

    @inbounds @simd for i=1:size(x, 1)
        @simd for j=1:size(x, 2)
            pt = (i - u[1], j - u[2])
            x[i, j] = interpolate2d(z, pt)
        end
    end
end

######################
# Subimage extraction
######################

@polly function extract_subimage!(b, im, v::DisplacementConstant)
    (imx, imy) = size(im)
    (bx, by) = size(b)

    # Translation from target to source coordinates
    vxʹ = (imx-bx)/2 - v[1]
    vyʹ = (imy-by)/2 - v[2]

    # Target image indices within source image
    px = ceil(Int, max(1, vxʹ + 1) - vxʹ)
    py = ceil(Int, max(1, vyʹ + 1) - vyʹ)
    qx = floor(Int, min(imx, vxʹ + bx) - vxʹ)
    qy = floor(Int, min(imy, vyʹ + by) - vyʹ)
    
    b .= 0

    @inbounds @simd for i=px:qx
        for j=py:qy
            b[i, j] = interpolate2d(im, (i+vxʹ, j+vyʹ))
        end
    end
end

end