gistfile1.c

void mmul(float* m, const float* a, const float* b)
{
	for (int r = 0; r < 4; r++)
		for (int c = 0; c < 4; c++)
			m[r*4+c] = a[r*4+0] * b[0*4+c] + a[r*4+1] * b[1*4+c] + a[r*4+2] * b[2*4+c] + a[r*4+3] * b[3*4+c];
}

void midentity(float* m)
{
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			m[i*4+j] = i == j ? 1 : 0;
}

void minverse(float* inv, const float* src)
{
	int i, j, k;
	float t;
	float temp[4][4];
	
	for (i = 0; i < 4; i++)
		for (j = 0; j  <4; j++)
			temp[i][j] = src[i*4+j];
	midentity(inv);
	
	for (i = 0; i < 4; i++)
	{
		if (temp[i][i] == 0.0f)
		{
			for (j = i + 1; j < 4; j++)
				if (temp[j][i] != 0.0f)
					break;
			
			if (j != 4)
			{
				for (k = 0; k < 4; k++)
				{
					t = temp[i][k];
					temp[i][k] = temp[j][k];
					temp[j][k] = t;
					t = inv[i*4+k];
					inv[i*4+k] = inv[j*4+k];
					inv[j*4+k] = t;
				}
			}
			else
			{
				return;
			}
		}
		
		t = 1.0f / temp[i][i];
		for (k = 0; k < 4; k++)
		{
			temp[i][k] *= t;
			inv[i*4+k] *= t;
		}
		for (j = 0; j < 4; j++)
		{
			if (j != i)
			{
				t = temp[j][i];
				for (k = 0; k < 4; k++)
				{
					temp[j][k] -= temp[i][k]*t;
					inv[j*4+k] -= inv[i*4+k]*t;
				}
			}
		}
	}
}

inline void transpoint4(float* r, const float* m, const float* v)
{
	r[0] = v[0]*m[0] + v[1]*m[4] + v[2]*m[8] + v[3]*m[12];
	r[1] = v[0]*m[1] + v[1]*m[5] + v[2]*m[9] + v[3]*m[13];
	r[2] = v[0]*m[2] + v[1]*m[6] + v[2]*m[10] + v[3]*m[14];
	r[3] = v[0]*m[3] + v[1]*m[7] + v[2]*m[11] + v[3]*m[15];
}

void project(float x, float y, float z, const float* model, const float* proj,
			 int w, int h, float* pos)
{
	float A[16];
	float in[4], out[4];
	in[0]=x; in[1]=y; in[2]=z; in[3]=1;
	mmul(A, model, proj);
	transpoint4(out, A, in);
	pos[0] = ((out[0]/out[3])+1)*w/2;
	pos[1] = ((out[1]/out[3])+1)*h/2;
	pos[2] = (out[2]/out[3]);
}

void unproject(float wx, float wy, float wz,
			   const float* model, const float* proj,
			   int w, int h, float* pos)
{
	float m[16], A[16];
	float in[4], out[4];
	
	in[0] = (wx/w)*2 - 1;
	in[1] = (wy/h)*2 - 1;
	in[2] = wz*2 - 1;
	in[3] = 1;
	
	mmul(A, model, proj);
	minverse(m, A);
	
	transpoint4(out, m, in);
	
	if (out[3] == 0.0f)
		return;
	pos[0] = out[0] / out[3];
	pos[1] = out[1] / out[3];
	pos[2] = out[2] / out[3];
}