math.c

一个用于智能手机的多媒体库适合S60 WinCE的跨平台开发库

}

GF_EXPORT
void gf_mx_apply_rect(GF_Matrix *mat, GF_Rect *rc)
{
	GF_Matrix2D mat2D;
	gf_mx2d_from_mx(&mat2D, mat);
	gf_mx2d_apply_rect(&mat2D, rc);
}

GF_EXPORT
void gf_mx_add_matrix(GF_Matrix *mat, GF_Matrix *mul)
{
    GF_Matrix tmp;
	gf_mx_init(tmp);

    tmp.m[0] = gf_mulfix(mat->m[0],mul->m[0]) + gf_mulfix(mat->m[4],mul->m[1]) + gf_mulfix(mat->m[8],mul->m[2]);
    tmp.m[4] = gf_mulfix(mat->m[0],mul->m[4]) + gf_mulfix(mat->m[4],mul->m[5]) + gf_mulfix(mat->m[8],mul->m[6]);
    tmp.m[8] = gf_mulfix(mat->m[0],mul->m[8]) + gf_mulfix(mat->m[4],mul->m[9]) + gf_mulfix(mat->m[8],mul->m[10]);
    tmp.m[12]= gf_mulfix(mat->m[0],mul->m[12]) + gf_mulfix(mat->m[4],mul->m[13]) + gf_mulfix(mat->m[8],mul->m[14]) + mat->m[12];
    tmp.m[1] = gf_mulfix(mat->m[1],mul->m[0]) + gf_mulfix(mat->m[5],mul->m[1]) + gf_mulfix(mat->m[9],mul->m[2]);
    tmp.m[5] = gf_mulfix(mat->m[1],mul->m[4]) + gf_mulfix(mat->m[5],mul->m[5]) + gf_mulfix(mat->m[9],mul->m[6]);
    tmp.m[9] = gf_mulfix(mat->m[1],mul->m[8]) + gf_mulfix(mat->m[5],mul->m[9]) + gf_mulfix(mat->m[9],mul->m[10]);
    tmp.m[13]= gf_mulfix(mat->m[1],mul->m[12]) + gf_mulfix(mat->m[5],mul->m[13]) + gf_mulfix(mat->m[9],mul->m[14]) + mat->m[13];
    tmp.m[2] = gf_mulfix(mat->m[2],mul->m[0]) + gf_mulfix(mat->m[6],mul->m[1]) + gf_mulfix(mat->m[10],mul->m[2]);
    tmp.m[6] = gf_mulfix(mat->m[2],mul->m[4]) + gf_mulfix(mat->m[6],mul->m[5]) + gf_mulfix(mat->m[10],mul->m[6]);
    tmp.m[10]= gf_mulfix(mat->m[2],mul->m[8]) + gf_mulfix(mat->m[6],mul->m[9]) + gf_mulfix(mat->m[10],mul->m[10]);
    tmp.m[14]= gf_mulfix(mat->m[2],mul->m[12]) + gf_mulfix(mat->m[6],mul->m[13]) + gf_mulfix(mat->m[10],mul->m[14]) + mat->m[14];
	memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
}

GF_EXPORT
void gf_mx_add_translation(GF_Matrix *mat, Fixed tx, Fixed ty, Fixed tz)
{
	Fixed tmp[3];
	u32 i;
	tmp[0] = mat->m[12];
	tmp[1] = mat->m[13];
	tmp[2] = mat->m[14];
	for (i=0; i<3; i++)
		tmp[i] += (gf_mulfix(tx,mat->m[i]) + gf_mulfix(ty,mat->m[i+4]) + gf_mulfix(tz, mat->m[i + 8]));
	mat->m[12] = tmp[0];
	mat->m[13] = tmp[1];
	mat->m[14] = tmp[2];
}

GF_EXPORT
void gf_mx_add_scale(GF_Matrix *mat, Fixed sx, Fixed sy, Fixed sz)
{
	Fixed tmp[3];
	u32 i, j;

	tmp[0] = sx; 
	tmp[1] = sy; 
	tmp[2] = sz; 

	for (i=0; i<3; i++) {
		for (j=0; j<3; j++) {
			mat->m[i*4 + j] = gf_mulfix(mat->m[j+4 * i], tmp[i]);
		}
	}
}

GF_EXPORT
void gf_mx_add_rotation(GF_Matrix *mat, Fixed angle, Fixed x, Fixed y, Fixed z)
{
	GF_Matrix tmp;
	Fixed xx, yy, zz, xy, xz, yz;
	Fixed nor = gf_sqrt(gf_mulfix(x,x) + gf_mulfix(y,y) + gf_mulfix(z,z));
	Fixed cos_a = gf_cos(angle);
	Fixed sin_a = gf_sin(angle);
	Fixed icos_a = FIX_ONE - cos_a;

	if (nor && (nor!=FIX_ONE)) { 
		x = gf_divfix(x, nor); 
		y = gf_divfix(y, nor); 
		z = gf_divfix(z, nor); 
	}
	xx = gf_mulfix(x,x); yy = gf_mulfix(y,y); zz = gf_mulfix(z,z); xy = gf_mulfix(x,y); xz = gf_mulfix(x,z); yz = gf_mulfix(y,z);
	gf_mx_init(tmp);
    tmp.m[0] = gf_mulfix(icos_a,xx) + cos_a;
    tmp.m[1] = gf_mulfix(xy,icos_a) + gf_mulfix(z,sin_a);
    tmp.m[2] = gf_mulfix(xz,icos_a) - gf_mulfix(y,sin_a);
    
	tmp.m[4] = gf_mulfix(xy,icos_a) - gf_mulfix(z,sin_a);
    tmp.m[5] = gf_mulfix(icos_a,yy) + cos_a;
    tmp.m[6] = gf_mulfix(yz,icos_a) + gf_mulfix(x,sin_a);

	tmp.m[8] = gf_mulfix(xz,icos_a) + gf_mulfix(y,sin_a);
    tmp.m[9] = gf_mulfix(yz,icos_a) - gf_mulfix(x,sin_a);
    tmp.m[10]= gf_mulfix(icos_a,zz) + cos_a;

	gf_mx_add_matrix(mat, &tmp);
}

GF_EXPORT
void gf_mx_from_mx2d(GF_Matrix *mat, GF_Matrix2D *mat2D)
{
	gf_mx_init(*mat);
	mat->m[0] = mat2D->m[0];
	mat->m[4] = mat2D->m[1];
	mat->m[12] = mat2D->m[2];
	mat->m[1] = mat2D->m[3];
	mat->m[5] = mat2D->m[4];
	mat->m[13] = mat2D->m[5];
}

GF_EXPORT
Bool gf_mx_equal(GF_Matrix *mx1, GF_Matrix *mx2)
{
	if (mx1->m[0] != mx2->m[0]) return 0;
	if (mx1->m[1] != mx2->m[1]) return 0;
	if (mx1->m[2] != mx2->m[2]) return 0;
	if (mx1->m[4] != mx2->m[4]) return 0;
	if (mx1->m[5] != mx2->m[5]) return 0;
	if (mx1->m[6] != mx2->m[6]) return 0;
	if (mx1->m[8] != mx2->m[8]) return 0;
	if (mx1->m[9] != mx2->m[9]) return 0;
	if (mx1->m[10] != mx2->m[10]) return 0;
	if (mx1->m[12] != mx2->m[12]) return 0;
	if (mx1->m[13] != mx2->m[13]) return 0;
	if (mx1->m[14] != mx2->m[14]) return 0;
	return 1;
}


GF_EXPORT
void gf_mx_inverse(GF_Matrix *mx)
{
    Fixed det;
	GF_Matrix rev;
	gf_mx_init(rev);

	assert(! ((mx->m[3] != 0) || (mx->m[7] != 0) || (mx->m[11] != 0) || (mx->m[15] != FIX_ONE)) );

	det = gf_mulfix(gf_mulfix(mx->m[0], mx->m[5]) , mx->m[10]);
	det += gf_mulfix(gf_mulfix(mx->m[1], mx->m[6]) , mx->m[8]);
	det += gf_mulfix(gf_mulfix(mx->m[2], mx->m[4]) , mx->m[9]);
	det -= gf_mulfix(gf_mulfix(mx->m[2], mx->m[5]) , mx->m[8]);
	det -= gf_mulfix(gf_mulfix(mx->m[1], mx->m[4]) , mx->m[10]);
	det -= gf_mulfix(gf_mulfix(mx->m[0], mx->m[6]) , mx->m[9]);

//	if (gf_mulfix(det) < FIX_EPSILON) return;

	/* Calculate inverse(A) = adj(A) / det(A) */
	rev.m[0] =  gf_muldiv(mx->m[5], mx->m[10], det) - gf_muldiv(mx->m[6], mx->m[9], det);
	rev.m[4] = -gf_muldiv(mx->m[4], mx->m[10], det) + gf_muldiv(mx->m[6], mx->m[8], det);
	rev.m[8] =  gf_muldiv(mx->m[4], mx->m[9], det) - gf_muldiv(mx->m[5], mx->m[8], det);
	rev.m[1] = -gf_muldiv(mx->m[1], mx->m[10], det) + gf_muldiv(mx->m[2], mx->m[9], det);
	rev.m[5] =  gf_muldiv(mx->m[0], mx->m[10], det) - gf_muldiv(mx->m[2], mx->m[8], det);
	rev.m[9] = -gf_muldiv(mx->m[0], mx->m[9], det) + gf_muldiv(mx->m[1], mx->m[8], det);
	rev.m[2] =  gf_muldiv(mx->m[1], mx->m[6], det) - gf_muldiv(mx->m[2], mx->m[5], det);
	rev.m[6] = -gf_muldiv(mx->m[0], mx->m[6], det) + gf_muldiv(mx->m[2], mx->m[4], det);
	rev.m[10] = gf_muldiv(mx->m[0], mx->m[5], det) - gf_muldiv(mx->m[1], mx->m[4], det);

	/* do translation part*/
	rev.m[12] = -( gf_mulfix(mx->m[12], rev.m[0]) + gf_mulfix(mx->m[13], rev.m[4]) + gf_mulfix(mx->m[14], rev.m[8]) );
	rev.m[13] = -( gf_mulfix(mx->m[12], rev.m[1]) + gf_mulfix(mx->m[13], rev.m[5]) + gf_mulfix(mx->m[14], rev.m[9]) );
	rev.m[14] = -( gf_mulfix(mx->m[12], rev.m[2]) + gf_mulfix(mx->m[13], rev.m[6]) + gf_mulfix(mx->m[14], rev.m[10]) );
	gf_mx_copy(*mx, rev);
}


GF_EXPORT
void gf_mx_apply_vec(GF_Matrix *mx, GF_Vec *pt)
{
	GF_Vec res;
	res.x = gf_mulfix(pt->x, mx->m[0]) + gf_mulfix(pt->y, mx->m[4]) + gf_mulfix(pt->z, mx->m[8]) + mx->m[12];
	res.y = gf_mulfix(pt->x, mx->m[1]) + gf_mulfix(pt->y, mx->m[5]) + gf_mulfix(pt->z, mx->m[9]) + mx->m[13];
	res.z = gf_mulfix(pt->x, mx->m[2]) + gf_mulfix(pt->y, mx->m[6]) + gf_mulfix(pt->z, mx->m[10]) + mx->m[14];
	*pt = res;
}

GF_EXPORT
void gf_mx_ortho(GF_Matrix *mx, Fixed left, Fixed right, Fixed bottom, Fixed top, Fixed z_near, Fixed z_far)
{
	gf_mx_init(*mx);
	mx->m[0] = gf_divfix(2*FIX_ONE, right-left);
	mx->m[5] = gf_divfix(2*FIX_ONE, top-bottom);
	mx->m[10] = gf_divfix(-2*FIX_ONE, z_far-z_near);
	mx->m[12] = gf_divfix(right+left, right-left);
	mx->m[13] = gf_divfix(top+bottom, top-bottom);
	mx->m[14] = gf_divfix(z_far+z_near, z_far-z_near);
	mx->m[15] = FIX_ONE;
}

GF_EXPORT
void gf_mx_perspective(GF_Matrix *mx, Fixed fieldOfView, Fixed aspectRatio, Fixed z_near, Fixed z_far)
{
	Fixed f = gf_divfix(gf_cos(fieldOfView/2), gf_sin(fieldOfView/2));
	gf_mx_init(*mx);
	mx->m[0] = gf_divfix(f, aspectRatio);
	mx->m[5] = f;
	mx->m[10] = gf_divfix(z_far+z_near, z_near-z_far);
	mx->m[11] = -FIX_ONE;
	mx->m[14] = 2*gf_muldiv(z_near, z_far, z_near-z_far);
	mx->m[15] = 0;
}

GF_EXPORT
void gf_mx_lookat(GF_Matrix *mx, GF_Vec eye, GF_Vec center, GF_Vec upVector)
{
	GF_Vec f, s, u;
	
	gf_vec_diff(f, center, eye);
	gf_vec_norm(&f);
	gf_vec_norm(&upVector);

	s = gf_vec_cross(f, upVector);
	u = gf_vec_cross(s, f);
	gf_mx_init(*mx);
	
	mx->m[0] = s.x;
	mx->m[1] = u.x;
	mx->m[2] = -f.x;
	mx->m[4] = s.y;
	mx->m[5] = u.y;
	mx->m[6] = -f.y;
	mx->m[8] = s.z;
	mx->m[9] = u.z;
	mx->m[10] = -f.z;

	gf_mx_add_translation(mx, -eye.x, -eye.y, -eye.z);
}

GF_Vec4 gf_quat_from_matrix(GF_Matrix *mx);

GF_EXPORT
void gf_mx_decompose(GF_Matrix *mx, GF_Vec *translate, GF_Vec *scale, GF_Vec4 *rotate, GF_Vec *shear)
{
	u32 i, j;
	GF_Vec4 quat;
	Fixed locmat[16];
	GF_Matrix tmp;
	GF_Vec row0, row1, row2;
	Fixed shear_xy, shear_xz, shear_yz; 
	assert(mx->m[15]);

	memcpy(locmat, mx->m, sizeof(Fixed)*16);
	/*no perspective*/
    locmat[3] = locmat[7] = locmat[11] = 0;
	/*normalize*/
    for (i=0; i<4; i++) {
        for (j=0; j<4; j++) {
            locmat[4*i+j] = gf_divfix(locmat[4*i+j], locmat[15]);
        }
    }
    translate->x = locmat[12];
    translate->y = locmat[13];
    translate->z = locmat[14];
    locmat[12] = locmat[13] = locmat[14] = 0;
    row0.x = locmat[0]; row0.y = locmat[1]; row0.z = locmat[2];
    row1.x = locmat[4]; row1.y = locmat[5]; row1.z = locmat[6];
    row2.x = locmat[8]; row2.y = locmat[9]; row2.z = locmat[10];

    scale->x = gf_vec_len(row0);
    gf_vec_norm(&row0);
    shear_xy = gf_vec_dot(row0, row1);
    row1.x -= gf_mulfix(row0.x, shear_xy);
    row1.y -= gf_mulfix(row0.y, shear_xy);
    row1.z -= gf_mulfix(row0.z, shear_xy);

    scale->y = gf_vec_len(row1);
    gf_vec_norm(&row1);
    shear->x = gf_divfix(shear_xy, scale->y);

    shear_xz = gf_vec_dot(row0, row2);
    row2.x -= gf_mulfix(row0.x, shear_xz);
    row2.y -= gf_mulfix(row0.y, shear_xz);
    row2.z -= gf_mulfix(row0.z, shear_xz);
    shear_yz = gf_vec_dot(row1, row2);
    row2.x -= gf_mulfix(row1.x, shear_yz);
    row2.y -= gf_mulfix(row1.y, shear_yz);
    row2.z -= gf_mulfix(row1.z, shear_yz);

    scale->z = gf_vec_len(row2);
    gf_vec_norm(&row2);
    shear->y = gf_divfix(shear_xz, scale->z);
    shear->z = gf_divfix(shear_yz, scale->z);

	locmat[0] = row0.x; locmat[4] = row1.x; locmat[8] = row2.x;
	locmat[1] = row0.y; locmat[5] = row1.y; locmat[9] = row2.y;
	locmat[2] = row0.z; locmat[6] = row1.z; locmat[10] = row2.z;

	memcpy(tmp.m, locmat, sizeof(Fixed)*16);
	quat = gf_quat_from_matrix(&tmp);
	*rotate = gf_quat_to_rotation(&quat);
}

GF_EXPORT
void gf_mx_apply_bbox(GF_Matrix *mx, GF_BBox *b)
{
	Fixed var;
	gf_mx_apply_vec(mx, &b->min_edge);
	gf_mx_apply_vec(mx, &b->max_edge);

	if (b->min_edge.x > b->max_edge.x) 
	{
		var = b->min_edge.x; b->min_edge.x = b->max_edge.x; b->max_edge.x = var;
	}
	if (b->min_edge.y > b->max_edge.y) 
	{
		var = b->min_edge.y; b->min_edge.y = b->max_edge.y; b->max_edge.y = var;
	}
	if (b->min_edge.z > b->max_edge.z) 
	{
		var = b->min_edge.z; b->min_edge.z = b->max_edge.z; b->max_edge.z = var;
	}
	gf_bbox_refresh(b);
}


// Apply the rotation portion of a matrix to a vector.
GF_EXPORT
void gf_mx_rotate_vector(GF_Matrix *mx, GF_Vec *pt)
{
	GF_Vec res;
	Fixed den;
	res.x = gf_mulfix(pt->x, mx->m[0]) + gf_mulfix(pt->y, mx->m[4]) + gf_mulfix(pt->z, mx->m[8]);
	res.y = gf_mulfix(pt->x, mx->m[1]) + gf_mulfix(pt->y, mx->m[5]) + gf_mulfix(pt->z, mx->m[9]);
	res.z = gf_mulfix(pt->x, mx->m[2]) + gf_mulfix(pt->y, mx->m[6]) + gf_mulfix(pt->z, mx->m[10]);
	den = gf_mulfix(pt->x, mx->m[3]) + gf_mulfix(pt->y, mx->m[7]) + gf_mulfix(pt->z, mx->m[11]) + mx->m[15];
	if (den == 0) return;
	res.x = gf_divfix(res.x, den);
	res.y = gf_divfix(res.y, den);
	res.z = gf_divfix(res.z, den);
	*pt = res;
}

GF_EXPORT
void gf_mx_rotation_matrix_from_vectors(GF_Matrix *mx, GF_Vec x, GF_Vec y, GF_Vec z)
{
    mx->m[0] = x.x; mx->m[1] = y.x; mx->m[2] = z.x; mx->m[3] = 0;
    mx->m[4] = x.y; mx->m[5] = y.y; mx->m[6] = z.y; mx->m[7] = 0;
    mx->m[8] = x.z; mx->m[9] = y.z; mx->m[10] = z.z; mx->m[11] = 0;
    mx->m[12] = 0; mx->m[13] = 0; mx->m[14] = 0; mx->m[15] = FIX_ONE;
}


/*we should only need a full matrix product for frustrum setup*/
GF_EXPORT
void gf_mx_add_matrix_4x4(GF_Matrix *mat, GF_Matrix *mul)
{
    GF_Matrix tmp;
	gf_mx_init(tmp);
    tmp.m[0] = gf_mulfix(mat->m[0],mul->m[0]) + gf_mulfix(mat->m[4],mul->m[1]) + gf_mulfix(mat->m[8],mul->m[2]) + gf_mulfix(mat->m[12],mul->m[3]);
    tmp.m[1] = gf_mulfix(mat->m[1],mul->m[0]) + gf_mulfix(mat->m[5],mul->m[1]) + gf_mulfix(mat->m[9],mul->m[2]) + gf_mulfix(mat->m[13],mul->m[3]);
    tmp.m[2] = gf_mulfix(mat->m[2],mul->m[0]) + gf_mulfix(mat->m[6],mul->m[1]) + gf_mulfix(mat->m[10],mul->m[2]) + gf_mulfix(mat->m[14],mul->m[3]);
    tmp.m[3] = gf_mulfix(mat->m[3],mul->m[0]) + gf_mulfix(mat->m[7],mul->m[1]) + gf_mulfix(mat->m[11],mul->m[2]) + gf_mulfix(mat->m[15],mul->m[3]);
    tmp.m[4] = gf_mulfix(mat->m[0],mul->m[4]) + gf_mulfix(mat->m[4],mul->m[5]) + gf_mulfix(mat->m[8],mul->m[6]) + gf_mulfix(mat->m[12],mul->m[7]);
    tmp.m[5] = gf_mulfix(mat->m[1],mul->m[4]) + gf_mulfix(mat->m[5],mul->m[5]) + gf_mulfix(mat->m[9],mul->m[6]) + gf_mulfix(mat->m[13],mul->m[7]);
    tmp.m[6] = gf_mulfix(mat->m[2],mul->m[4]) + gf_mulfix(mat->m[6],mul->m[5]) + gf_mulfix(mat->m[10],mul->m[6]) + gf_mulfix(mat->m[14],mul->m[7]);
    tmp.m[7] = gf_mulfix(mat->m[3],mul->m[4]) + gf_mulfix(mat->m[7],mul->m[5]) + gf_mulfix(mat->m[11],mul->m[6]) + gf_mulfix(mat->m[15],mul->m[7]);
    tmp.m[8] = gf_mulfix(mat->m[0],mul->m[8]) + gf_mulfix(mat->m[4],mul->m[9]) + gf_mulfix(mat->m[8],mul->m[10]) + gf_mulfix(mat->m[12],mul->m[11]);
    tmp.m[9] = gf_mulfix(mat->m[1],mul->m[8]) + gf_mulfix(mat->m[5],mul->m[9]) + gf_mulfix(mat->m[9],mul->m[10]) + gf_mulfix(mat->m[13],mul->m[11]);
    tmp.m[10] = gf_mulfix(mat->m[2],mul->m[8]) + gf_mulfix(mat->m[6],mul->m[9]) + gf_mulfix(mat->m[10],mul->m[10]) + gf_mulfix(mat->m[14],mul->m[11]);
    tmp.m[11] = gf_mulfix(mat->m[3],mul->m[8]) + gf_mulfix(mat->m[7],mul->m[9]) + gf_mulfix(mat->m[11],mul->m[10]) + gf_mulfix(mat->m[15],mul->m[11]);
    tmp.m[12] = gf_mulfix(mat->m[0],mul->m[12]) + gf_mulfix(mat->m[4],mul->m[13]) + gf_mulfix(mat->m[8],mul->m[14]) + gf_mulfix(mat->m[12],mul->m[15]);
    tmp.m[13] = gf_mulfix(mat->m[1],mul->m[12]) + gf_mulfix(mat->m[5],mul->m[13]) + gf_mulfix(mat->m[9],mul->m[14]) + gf_mulfix(mat->m[13],mul->m[15]);
    tmp.m[14] = gf_mulfix(mat->m[2],mul->m[12]) + gf_mulfix(mat->m[6],mul->m[13]) + gf_mulfix(mat->m[10],mul->m[14]) + gf_mulfix(mat->m[14],mul->m[15]);
    tmp.m[15] = gf_mulfix(mat->m[3],mul->m[12]) + gf_mulfix(mat->m[7],mul->m[13]) + gf_mulfix(mat->m[11],mul->m[14]) + gf_mulfix(mat->m[15],mul->m[15]);
	memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
}


GF_EXPORT
void gf_mx_apply_vec_4x4(GF_Matrix *mx, GF_Vec4 *vec)
{
	GF_Vec4 res;
	res.x = gf_mulfix(mx->m[0], vec->x) + gf_mulfix(mx->m[4], vec->y) + gf_mulfix(mx->m[8], vec->z) + gf_mulfix(mx->m[12], vec->q);
	res.y = gf_mulfix(mx->m[1], vec->x) + gf_mulfix(mx->m[5], vec->y) + gf_mulfix(mx->m[9], vec->z) + gf_mulfix(mx->m[13], vec->q);
	res.z = gf_mulfix(mx->m[2], vec->x) + gf_mulfix(mx->m[6], vec->y) + gf_mulfix(mx->m[10], vec->z) + gf_mulfix(mx->m[14], vec->q);
	res.q = gf_mulfix(mx->m[3], vec->x) + gf_mulfix(mx->m[7], vec->y) + gf_mulfix(mx->m[11], vec->z) + gf_mulfix(mx->m[15], vec->q);
	*vec = res;
}

/*
 *	Taken from MESA/GLU (LGPL)
 *
 * Compute inverse of 4x4 transformation matrix.
 * Code contributed by Jacques Leroy [e-mail]
 * Return 1 for success, 0 for failure (singular matrix)
 */

GF_EXPORT
Bool gf_mx_inverse_4x4(GF_Matrix *mx)
{

#define SWAP_ROWS(a, b) { Fixed *_tmp = a; (a)=(b); (b)=_tmp; }
	Fixed wtmp[4][8];
	Fixed m0, m1, m2, m3, s;
	Fixed *r0, *r1, *r2, *r3;
	GF_Matrix res;
	r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
	r0[0] = mx->m[0]; r0[1] = mx->m[4]; r0[2] = mx->m[8]; r0[3] = mx->m[12]; r0[4] = FIX_ONE; r0[5] = r0[6] = r0[7] = 0;
	r1[0] = mx->m[1]; r1[1] = mx->m[5]; r1[2] = mx->m[9]; r1[3] = mx->m[13]; r1[5] = FIX_ONE; r1[4] = r1[6] = r1[7] = 0;
	r2[0] = mx->m[2]; r2[1] = mx->m[6]; r2[2] = mx->m[10]; r2[3] = mx->m[14]; r2[6] = FIX_ONE; r2[4] = r2[5] = r2[7] = 0;
	r3[0] = mx->m[3]; r3[1] = mx->m[7]; r3[2] = mx->m[11]; r3[3] = mx->m[15]; r3[7] = FIX_ONE; r3[4] = r3[5] = r3[6] = 0;

	/* choose pivot - or die */
	if (ABS(r3[0]) > ABS(r2[0])) SWAP_ROWS(r3, r2);
	if (ABS(r2[0]) > ABS(r1[0])) SWAP_ROWS(r2, r1);
	if (ABS(r1[0]) > ABS(r0[0])) SWAP_ROWS(r1, r0);
	if (r0[0]==0) return 0;
	
	/*eliminate first variable*/
	m1 = gf_divfix(r1[0], r0[0]);
	m2 = gf_divfix(r2[0], r0[0]);
	m3 = gf_divfix(r3[0], r0[0]);
	s = r0[1];
	r1[1] -= gf_mulfix(m1, s);
	r2[1] -= gf_mulfix(m2, s);
	r3[1] -= gf_mulfix(m3, s);
	s = r0[2];
	r1[2] -= gf_mulfix(m1, s);
	r2[2] -= gf_mulfix(m2, s);
	r3[2] -= gf_mulfix(m3, s);
	s = r0[3];
	r1[3] -= gf_mulfix(m1, s);
	r2[3] -= gf_mulfix(m2, s);
	r3[3] -= gf_mulfix(m3, s);
	s = r0[4];
	if (s != 0) {
		r1[4] -= gf_mulfix(m1, s);
		r2[4] -= gf_mulfix(m2, s);
		r3[4] -= gf_mulfix(m3, s);
	}
	s = r0[5];
	if (s != 0) {
		r1[5] -= gf_mulfix(m1, s);
		r2[5] -= gf_mulfix(m2, s);
		r3[5] -= gf_mulfix(m3, s);
	}
	s = r0[6];
	if (s != 0) {
		r1[6] -= gf_mulfix(m1, s);
		r2[6] -= gf_mulfix(m2, s);
		r3[6] -= gf_mulfix(m3, s);
	}
	s = r0[7];
	if (s != 0) {
		r1[7] -= gf_mulfix(m1, s);
		r2[7] -= gf_mulfix(m2, s);
		r3[7] -= gf_mulfix(m3, s);
	}

	/* choose pivot - or die */
	if (fabs(r3[1]) > fabs(r2[1])) SWAP_ROWS(r3, r2);
	if (fabs(r2[1]) > fabs(r1[1])) SWAP_ROWS(r2, r1);
	if (r1[1]==0) return 0;
	
	/* eliminate second variable */
	m2 = gf_divfix(r2[1], r1[1]);
	m3 = gf_divfix(r3[1], r1[1]);
	r2[2] -= gf_mulfix(m2, r1[2]);
	r3[2] -= gf_mulfix(m3, r1[2]);
	r2[3] -= gf_mulfix(m2, r1[3]);
	r3[3] -= gf_mulfix(m3, r1[3]);
	s = r1[4];
	if (s!=0) {
		r2[4] -= gf_mulfix(m2, s);
		r3[4] -= gf_mulfix(m3, s);
	}
	s = r1[5];
	if (s!=0) {
		r2[5] -= gf_mulfix(m2, s);
		r3[5] -= gf_mulfix(m3, s);
	}
	s = r1[6];
	if (s!=0) {
		r2[6] -= gf_mulfix(m2, s);
		r3[6] -= gf_mulfix(m3, s);
	}
	s = r1[7];
	if (s!=0) {
		r2[7] -= gf_mulfix(m2, s);
		r3[7] -= gf_mulfix(m3, s);
	}

	/* choose pivot - or die */
	if (fabs(r3[2]) > fabs(r2[2])) SWAP_ROWS(r3, r2);
	if (r2[2]==0) return 0;

	/* eliminate third variable */
	m3 = gf_divfix(r3[2], r2[2]);
	r3[3] -= gf_mulfix(m3, r2[3]); r3[4] -= gf_mulfix(m3, r2[4]); r3[5] -= gf_mulfix(m3, r2[5]);
	r3[6] -= gf_mulfix(m3, r2[6]); r3[7] -= gf_mulfix(m3, r2[7]);
	/* last check */
	if (r3[3]==0) return 0;

	s = gf_invfix(r3[3]);		/* now back substitute row 3 */
	r3[4] = gf_mulfix(r3[4], s);
	r3[5] = gf_mulfix(r3[5], s);
	r3[6] = gf_mulfix(r3[6], s);
	r3[7] = gf_mulfix(r3[7], s);
	
	m2 = r2[3];			/* now back substitute row 2 */
	s = gf_invfix(r2[2]);
	r2[4] = gf_mulfix(s, r2[4] - gf_mulfix(r3[4], m2));
	r2[5] = gf_mulfix(s, r2[5] - gf_mulfix(r3[5], m2));
	r2[6] = gf_mulfix(s, r2[6] - gf_mulfix(r3[6], m2));
	r2[7] = gf_mulfix(s, r2[7] - gf_mulfix(r3[7], m2));
	m1 = r1[3]; 
	r1[4] -= gf_mulfix(r3[4], m1); r1[5] -= gf_mulfix(r3[5], m1); r1[6] -= gf_mulfix(r3[6], m1); r1[7] -= gf_mulfix(r3[7], m1);
	m0 = r0[3];
	r0[4] -= gf_mulfix(r3[4], m0); r0[5] -= gf_mulfix(r3[5], m0); r0[6] -= gf_mulfix(r3[6], m0); r0[7] -= gf_mulfix(r3[7], m0);

	m1 = r1[2];			/* now back substitute row 1 */
	s = gf_invfix(r1[1]);
	r1[4] = gf_mulfix(s, r1[4] - gf_mulfix(r2[4], m1));
	r1[5] = gf_mulfix(s, r1[5] - gf_mulfix(r2[5], m1));
	r1[6] = gf_mulfix(s, r1[6] - gf_mulfix(r2[6], m1));
	r1[7] = gf_mulfix(s, r1[7] - gf_mulfix(r2[7], m1));
	m0 = r0[2];
	r0[4] -= gf_mulfix(r2[4], m0); r0[5] -= gf_mulfix(r2[5], m0); r0[6] -= gf_mulfix(r2[6], m0); r0[7] -= gf_mulfix(r2[7], m0);

	m0 = r0[1];			/* now back substitute row 0 */
	s = gf_invfix(r0[0]);
	r0[4] = gf_mulfix(s, r0[4] - gf_mulfix(r1[4], m0));
	r0[5] = gf_mulfix(s, r0[5] - gf_mulfix(r1[5], m0)); 
	r0[6] = gf_mulfix(s, r0[6] - gf_mulfix(r1[6], m0));
	r0[7] = gf_mulfix(s, r0[7] - gf_mulfix(r1[7], m0));

	gf_mx_init(res)
	res.m[0] = r0[4]; res.m[4] = r0[5]; res.m[8] = r0[6]; res.m[12] = r0[7];
	res.m[1] = r1[4]; res.m[5] = r1[5], res.m[9] = r1[6]; res.m[13] = r1[7];
	res.m[2] = r2[4]; res.m[6] = r2[5]; res.m[10] = r2[6]; res.m[14] = r2[7];
	res.m[3] = r3[4]; res.m[7] = r3[5]; res.m[11] = r3[6]; res.m[15] = r3[7];
	gf_mx_copy(*mx, res);
	return 1;
#undef SWAP_ROWS

}


GF_EXPORT
Bool gf_plane_exists_intersection(GF_Plane *plane, GF_Plane *with)
{
	GF_Vec cross;
	cross = gf_vec_cross(with->normal, plane->normal);
	return gf_vec_lensq(cross) > FIX_EPSILON;
}

GF_EXPORT
Bool gf_plane_intersect_line(GF_Plane *plane, GF_Vec *linepoint, GF_Vec *linevec, GF_Vec *outPoint)
{
	Fixed t, t2;
	t2 = gf_vec_dot(plane->normal, *linevec);
	if (t2 == 0) return 0;
	t = - gf_divfix((gf_vec_dot(plane->normal, *linepoint) + plane->d) , t2);
	if (t<0) return 0;
	*outPoint = gf_vec_scale(*linevec, t);
	gf_vec_add(*outPoint, *linepoint, *outPoint);
	return 1;
}

GF_EXPORT
Bool gf_plane_intersect_plane(GF_Plane *plane, GF_Plane *with, GF_Vec *linepoint, GF_Vec *linevec)
{
	Fixed fn00 = gf_vec_len(plane->normal);
	Fixed fn01 = gf_vec_dot(plane->normal, with->normal);
	Fixed fn11 = gf_vec_len(with->normal);
	Fixed det = gf_mulfix(fn00,fn11) - gf_mulfix(fn01,fn01);