shithub: imagetools

ref: fc8a7899256f517ee57df789abb880592cdfe6c8
dir: /warp.c/

#include <u.h>
#include <libc.h>
#include <draw.h>
#include <memdraw.h>
#include <geometry.h>
#include "fns.h"

typedef struct Mstk Mstk;
struct Mstk
{
	Matrix3 *items;
	ulong size;
};

static double γ;	/* roll */

static void
pushmat(Mstk *stk, Matrix3 m)
{
	if(stk->size % 4 == 0)
		stk->items = erealloc(stk->items, (stk->size + 4)*sizeof(Matrix3));
	memmove(stk->items[stk->size++], m, sizeof(Matrix3));
}

static void
popmat(Mstk *stk, Matrix3 m)
{
	memmove(m, stk->items[--stk->size], sizeof(Matrix3));
	if(stk->size == 0){
		free(stk->items);
		stk->items = nil;
	}
}

static void
mkrotation(Matrix3 m, double θ, double φ, double γ)
{
	Matrix3 Rx = {
		1, 0, 0, 0,
		0, cos(θ), -sin(θ), 0,
		0, sin(θ),  cos(θ), 0,
		0, 0, 0, 1,
	}, Ry = {
		 cos(φ), 0, sin(φ), 0,
		0, 1, 0, 0,
		-sin(φ), 0, cos(φ), 0,
		0, 0, 0, 1,
	}, Rz = {
		cos(γ), -sin(γ), 0, 0,
		sin(γ),  cos(γ), 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1,
	};
	mulm3(Ry, Rx);
	mulm3(Rz, Ry);
	memmove(m, Rz, sizeof(Matrix3));
}

static void
mkscale(Matrix3 m, double sx, double sy, double sz)
{
	sx = sx == 0? 1: 1/sx;
	sy = sy == 0? 1: 1/sy;
	sz = sz == 0? 1: 1/sz;
	Matrix3 S = {
		sx, 0, 0, 0,
		0, sy, 0, 0,
		0, 0, sz, 0,
		0, 0, 0, 1,
	};
	memmove(m, S, sizeof(Matrix3));
}

static void
mktranslation(Matrix3 m, double tx, double ty, double tz)
{
	Matrix3 T = {
		1, 0, 0, tx,
		0, 1, 0, ty,
		0, 0, 1, tz,
		0, 0, 0, 1,
	};
	memmove(m, T, sizeof(Matrix3));
}

static void
mkshear(Matrix3 m, double shx, double shy, double shz)
{
	Matrix3 Sxy = {
		1, 0, 0, shx,
		0, 1, 0, shy,
		0, 0, 1, 0,
		0, 0, 0, 1,
	}, Syz = {
		1,   0, 0, 0,
		shy, 1, 0, 0,
		shz, 0, 1, 0,
		0,   0, 0, 1,
	}, Sxz = {
		1, shx, 0, 0,
		0,   1, 0, 0,
		0, shz, 1, 0,
		0,   0, 0, 1,
	};
	mulm3(Syz, Sxy);
	mulm3(Sxz, Syz);
	memmove(m, Sxz, sizeof(Matrix3));
}

/*
 * references:
 *	- http://stackoverflow.com/questions/17087446/how-to-calculate-perspective-transform-for-opencv-from-rotation-angles
 * 	- http://jepsonsblog.blogspot.tw/2012/11/rotation-in-3d-using-opencvs.html
 */
static void
mkxform(Matrix m, Mstk *stk, Memimage *s)
{
	double d, focal;
	int w, h, i, j;
	Matrix3 U, t;

	w = Dx(s->r);
	h = Dy(s->r);
	d = hypot(w, h);
	focal = γ == 0? d: d / 2*sin(γ);

	identity3(U);
	while(stk->size > 0){
		popmat(stk, t);
		mulm3(U, t);
	}

	Matrix3 T = {
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, focal,
		0, 0, 0, 1,
	}, A1 = {
		1, 0, -w/2, 0,
		0, 1, -h/2, 0,
		0, 0, 0, 0,
		0, 0, 1, 0,
	}, A2 = {
		focal, 0, w/2, 0,
		0, focal, h/2, 0,
		0, 0, 1, 0,
		0, 0, 0, 0,
	};

	mulm3(U, A1);
	mulm3(T, U);
	mulm3(A2, T);

	for(j = 0; j < 3; j++)
	for(i = 0; i < 3; i++)
		m[j][i] = A2[j][i];
}

static uchar
sample(Memimage *i, Point p, int off)
{
	if(p.x < i->r.min.x || p.y < i->r.min.y
	|| p.x >= i->r.max.x || p.y >= i->r.max.y)
		return 0;	/* edge handler: constant */
	return *(byteaddr(i, p) + off);
}

static Memimage *
imgxform(Memimage *s, Mstk *stk)
{
	Memimage *d;
	Matrix m;
	Point p, p0;
	Point2 p2;
	double Δx, Δy, c[2][2];
	int i;

	d = eallocmemimage(s->r, s->chan);
	mkxform(m, stk, s);

	for(p.y = s->r.min.y; p.y < s->r.max.y; p.y++)
	for(p.x = s->r.min.x; p.x < s->r.max.x; p.x++){
		p2 = xform((Point2){p.x, p.y, 1}, m);
		p2 = mulpt2(p2, p2.w == 0? 1: 1/p2.w);

		p0.x = p2.x;
		p0.y = p2.y;
		Δx = p2.x - p0.x;
		Δy = p2.y - p0.y;

		for(i = 0; i < s->nchan; i++){
			c[0][0] = sample(s, p0, i);
			c[0][1] = sample(s, addpt(p0, (Point){1,0}), i);
			c[1][0] = sample(s, addpt(p0, (Point){0,1}), i);
			c[1][1] = sample(s, addpt(p0, (Point){1,1}), i);

			c[0][0] = flerp(c[0][0], c[0][1], Δx);
			c[0][1] = flerp(c[1][0], c[1][1], Δx);
			c[0][0] = flerp(c[0][0], c[0][1], Δy);

			*(byteaddr(d, p) + i) = clamp(c[0][0], 0, 0xFF);
		}
	}
	return d;
}

static void
usage(void)
{
	fprint(2, "usage: %s [[-s x y z] [-r x y z] [-t x y z] [-S x y z]]...\n", argv0);
	exits(nil);
}

void
main(int argc, char *argv[])
{
	Memimage *img, *warp;
	Mstk stk;
	Matrix3 m;
	double x, y, z;

	memset(&stk, 0, sizeof stk);
	identity3(m);
	ARGBEGIN{
	case 's':
		x = strtod(EARGF(usage()), nil);
		y = strtod(EARGF(usage()), nil);
		z = strtod(EARGF(usage()), nil);
		mkscale(m, x, y, z);
		pushmat(&stk, m);
		break;
	case 'r':
		x = strtod(EARGF(usage()), nil)*DEG;
		y = strtod(EARGF(usage()), nil)*DEG;
		z = strtod(EARGF(usage()), nil)*DEG;
		γ += z;
		mkrotation(m, x, y, z);
		pushmat(&stk, m);
		break;
	case 't':
		x = strtod(EARGF(usage()), nil);
		y = strtod(EARGF(usage()), nil);
		z = strtod(EARGF(usage()), nil);
		mktranslation(m, x, y, z);
		pushmat(&stk, m);
		break;
	case 'S':
		x = strtod(EARGF(usage()), nil);
		y = strtod(EARGF(usage()), nil);
		z = strtod(EARGF(usage()), nil);
		mkshear(m, x, y, z);
		pushmat(&stk, m);
		break;
	default: usage();
	}ARGEND;
	if(argc != 0)
		usage();

	img = ereadmemimage(0);
	warp = imgxform(img, &stk);
	freememimage(img);

	ewritememimage(1, warp);
	freememimage(warp);

	exits(nil);
}