Perspective correcting a quad

gistfile1.py

import array

# these routines copied shamelessly from http://threeblindmiceandamonkey.com/?p=16

# multiply matrix: c = a * b
cdef inline void multiplyMatrix(double a[3][3], double b[3][3], double c[3][3]):
    c[0][0] = a[0][0]*b[0][0] + a[0][1]*b[1][0] + a[0][2]*b[2][0]
    c[0][1] = a[0][0]*b[0][1] + a[0][1]*b[1][1] + a[0][2]*b[2][1]
    c[0][2] = a[0][0]*b[0][2] + a[0][1]*b[1][2] + a[0][2]*b[2][2]
    c[1][0] = a[1][0]*b[0][0] + a[1][1]*b[1][0] + a[1][2]*b[2][0]
    c[1][1] = a[1][0]*b[0][1] + a[1][1]*b[1][1] + a[1][2]*b[2][1]
    c[1][2] = a[1][0]*b[0][2] + a[1][1]*b[1][2] + a[1][2]*b[2][2]
    c[2][0] = a[2][0]*b[0][0] + a[2][1]*b[1][0] + a[2][2]*b[2][0]
    c[2][1] = a[2][0]*b[0][1] + a[2][1]*b[1][1] + a[2][2]*b[2][1]
    c[2][2] = a[2][0]*b[0][2] + a[2][1]*b[1][2] + a[2][2]*b[2][2]

# transform point p by matrix a: q = p*a 
cdef inline void transformMatrix(double p[3], double q[3], double a[3][3]):
    q[0] = p[0]*a[0][0] + p[1]*a[1][0] + p[2]*a[2][0]
    q[1] = p[0]*a[0][1] + p[1]*a[1][1] + p[2]*a[2][1]
    q[2] = p[0]*a[0][2] + p[1]*a[1][2] + p[2]*a[2][2]

# determinant of a 2x2 matrix
cdef inline double det2(double a, double b, double c, double d):
    return( a*d - b*c)

# adjoint matrix: b = adjoint(a) returns determinant(a)
cdef inline double adjointMatrix(double a[3][3], double b[3][3]):
    b[0][0] = det2(a[1][1], a[1][2], a[2][1], a[2][2])
    b[1][0] = det2(a[1][2], a[1][0], a[2][2], a[2][0])
    b[2][0] = det2(a[1][0], a[1][1], a[2][0], a[2][1])
    b[0][1] = det2(a[2][1], a[2][2], a[0][1], a[0][2])
    b[1][1] = det2(a[2][2], a[2][0], a[0][2], a[0][0])
    b[2][1] = det2(a[2][0], a[2][1], a[0][0], a[0][1])
    b[0][2] = det2(a[0][1], a[0][2], a[1][1], a[1][2])
    b[1][2] = det2(a[0][2], a[0][0], a[1][2], a[1][0])
    b[2][2] = det2(a[0][0], a[0][1], a[1][0], a[1][1])
    return a[0][0]*b[0][0] + a[0][1]*b[0][1] + a[0][2]*b[0][2]

cdef inline bool ZERO(double x):
    return (x < 1e-13) and (x > -1e-13)

# calculate matrix for unit square to quad mapping
cdef inline bool mapSquareToQuad(\
    double quad[4][2],  # vertices of quadrilateral
    double SQ[3][3]):   # square->quad transform

    cdef double px, py
    cdef double dx1, dx2, dy1, dy2, det

    px = quad[0][0]-quad[1][0]+quad[2][0]-quad[3][0]
    py = quad[0][1]-quad[1][1]+quad[2][1]-quad[3][1]

    if ZERO(px) and ZERO(py):
        SQ[0][0] = quad[1][0]-quad[0][0]
        SQ[1][0] = quad[2][0]-quad[1][0]
        SQ[2][0] = quad[0][0]
        SQ[0][1] = quad[1][1]-quad[0][1]
        SQ[1][1] = quad[2][1]-quad[1][1]
        SQ[2][1] = quad[0][1]
        SQ[0][2] = 0.
        SQ[1][2] = 0.
        SQ[2][2] = 1.
    else:
        dx1 = quad[1][0]-quad[2][0]
        dx2 = quad[3][0]-quad[2][0]
        dy1 = quad[1][1]-quad[2][1]
        dy2 = quad[3][1]-quad[2][1]

        det = det2(dx1,dx2, dy1,dy2)
                
        if det==0.:
            return False

        SQ[0][2] = det2(px,dx2, py,dy2)/det
        SQ[1][2] = det2(dx1,px, dy1,py)/det
        SQ[2][2] = 1.
        SQ[0][0] = quad[1][0]-quad[0][0]+SQ[0][2]*quad[1][0]
        SQ[1][0] = quad[3][0]-quad[0][0]+SQ[1][2]*quad[3][0]
        SQ[2][0] = quad[0][0]
        SQ[0][1] = quad[1][1]-quad[0][1]+SQ[0][2]*quad[1][1]
        SQ[1][1] = quad[3][1]-quad[0][1]+SQ[1][2]*quad[3][1]
        SQ[2][1] = quad[0][1]

    return True
    
# calculate matrix for general quad to quad mapping
cdef inline bool mapQuadToQuad(\
    double src[4][2],    # starting quad
    double dest[4][2],   # target quad
    double ST[3][3]):   # the matrix (returned)
    
    cdef double quad[4][2], MS[3][3]
    cdef double SM[3][3], MT[3][3]

    quad[0][0] = src[0][0]; quad[0][1] = src[0][1]
    quad[1][0] = src[1][0]; quad[1][1] = src[1][1]
    quad[2][0] = src[2][0]; quad[2][1] = src[2][1]
    quad[3][0] = src[3][0]; quad[3][1] = src[3][1]
    if not mapSquareToQuad(quad, MS):
        return False

    adjointMatrix(MS, SM)

    quad[0][0] = dest[0][0]; quad[0][1] = dest[0][1] 
    quad[1][0] = dest[1][0]; quad[1][1] = dest[1][1] 
    quad[2][0] = dest[2][0]; quad[2][1] = dest[2][1] 
    quad[3][0] = dest[3][0]; quad[3][1] = dest[3][1] 
    if not mapSquareToQuad(quad, MT):
        return False

    multiplyMatrix(SM, MT, ST)

    return True
    
# bits below original:

cdef extern from "stdlib.h":
    long c_libc_random "random"()
    void c_libc_srandom "srandom"(unsigned int seed)
    
cdef inline void transform_point(double pt[2],double trans[3][3]):
    cdef double src[3]
    cdef double dest[3]
    src[0], src[1], src[2] = pt[0], pt[1], 1
    transformMatrix(src,dest,trans)
    pt[0] = dest[0] / dest[2]
    pt[1] = dest[1] / dest[2]
    
cdef inline void linear_interop(double a[2],double b[2],double pt[2],double factor):
    pt[0] = a[0] + (b[0]-a[0]) * factor
    pt[1] = a[1] + (b[1]-a[1]) * factor

cdef int do_extract_rect(\
    size_t src_addr,
    int sw,int sstride,int sh,
    double quad[4][2],
    size_t dest_addr,
    int dw,int dstride,int dh,
    int nsamples) except -1:
    cdef char* src = <char*>src_addr
    cdef char* dest = <char*>dest_addr
    # http://alvyray.com/memos/6_pixel.pdf is a fun read, which we'll ignore ;)
    # this makes some poor quality and slow choices:
        # pixels are rectangles, so find the quad in the source for each dest pixel
        # randomly sample it some number of times
        # mean average the samples
    cdef int x, y
    cdef double dest_rect[4][2]
    cdef double homo[3][3]
    cdef double tl[2], tr[2], bl[2], br[2]
    cdef int r, g, b, samples
    cdef double xf, yf
    cdef double left[2], right[2], pt[2]
    cdef int ptx, pty
    cdef char* sptr, *sentinal = src+sstride*sh
    cdef int masked = 0
    dest_rect[0][0] = 0;  dest_rect[0][1] = 0
    dest_rect[1][0] = dw; dest_rect[1][1] = 0
    dest_rect[2][0] = dw; dest_rect[2][1] = dh
    dest_rect[3][0] = 0;  dest_rect[3][1] = dh
    if not mapQuadToQuad(dest_rect,quad,homo):
        raise Exception("mapQuadToQuad failed")
    if nsamples == 1:
        # special case fast version with first-pixel
        for y in range(0,dh):
            for x in range(0,dw):
                pt[0], pt[1] = x, y
                transform_point(pt,homo)
                if (pt[0] >= 0) and (pt[0] < sw) and \
                    (pt[1] >= 0) and (pt[1] < sh):
                    sptr = src + (<int>pt[1]*sstride) + (<int>pt[0]*4)
                    if (sptr >= sentinal) or (sptr < src):
                        raise Exception("access (%s,%s) out of bounds"%(pt[0],pt[1]))
                    dest[0] = sptr[0]
                    dest[1] = sptr[1]
                    dest[2] = sptr[2]
                    dest[3] = 0xff
                else:
                    dest[3] = 0
                    masked = 1
                dest += 4
            dest += (dstride-(dw*4))
        return masked
    for y in range(0,dh):
        # work out the left side of the first pixel, put it into the right
        tr[0], tr[1] = 0, y
        transform_point(tr,homo)
        br[0], br[1] = 1, y+1
        transform_point(br,homo)
        for x in range(0,dw):
            # reuse the right of the previous pixel as the left
            tl[0], tl[1] = tr[0], tr[1]
            tr[0], tr[1] = x+1, y
            transform_point(tr,homo)
            bl[0], bl[1] = br[0], br[1]
            br[0], br[1] = x+1, y+1
            transform_point(br,homo)
            # sum the source pixels for this destination
            r = g = b = samples = 0
            for sample in range(0,nsamples):
                # pick a random sample
                xf = (c_libc_random() & 0xff) / 0xff
                yf = (c_libc_random() & 0xff) / 0xff
                linear_interop(tl,bl,left,yf)
                linear_interop(tr,br,right,yf)
                linear_interop(left,right,pt,xf)
                # find and mix the source pixel
                if (pt[0] >= 0) and (pt[0] < sw) and \
                    (pt[1] >= 0) and (pt[1] < sh):
                    sptr = src + (<int>pt[1]*sstride) + (<int>pt[0]*4)
                    if (sptr >= sentinal) or (sptr < src):
                        raise Exception("access (%s,%s) out of bounds"%(pt[0],pt[1]))
                    r += sptr[0]
                    g += sptr[1]
                    b += sptr[2]
                    samples += 1
            # mean average
            if samples > 0:
                dest[0] = r / samples
                dest[1] = g / samples
                dest[2] = b / samples
                dest[3] = 0xff
            else:
                dest[3] = 0
                masked = 1
            dest += 4
        dest += (dstride-(dw*4))
    return masked

cdef inline int xassert(expr,message=None) except -1:
    if not expr:
        raise Exception(message if message is not None else "XAssertion failed")
    return 0
    
def extract_rect(src,src_width,src_stride,src_height,quad,dest,dest_width,dest_stride,dest_height,nsamples=7):
    cdef double src_quad[4][2]
    xassert(isinstance(src,array.array))
    xassert(src_stride >= (src_width*4))
    xassert(len(src) == (src_stride*src_height))
    xassert(isinstance(dest,array.array))
    xassert(dest_stride >= (dest_width*4))
    xassert(len(dest) == (dest_stride*dest_height))
    for y in range(0,4):
        for x in range(0,2):
            src_quad[y][x] = quad[y][x]
    return do_extract_rect(\
        src.buffer_info()[0],src_width,src_stride,src_height,
        src_quad,
        dest.buffer_info()[0],dest_width,dest_stride,dest_height,
        nsamples)