Index: math.c
===================================================================
--- math.c	(revision 1157)
+++ math.c	(revision 1158)
@@ -39,6 +39,13 @@
 res[2]= (p[2]*raydium_math_cos(rx)+ p[1]*raydium_math_sin(rx))*raydium_math_cos(ry)-p[0]*raydium_math_sin(ry);
 }
 
+void raydium_math_normalize_vector4 (GLfloat * vector){
+	GLfloat length;
+	length=1.0/sqrtf(vector[0]*vector[0]+vector[1]*vector[1]+vector[2]*vector[2]);
+	vector[0]*=length;
+	vector[1]*=length;
+	vector[2]*=length;
+}
 
 // pos: GLfloat[3], m: GLfloat[16]
 void raydium_math_pos_to_matrix(GLfloat *pos, GLfloat *m)
@@ -49,6 +56,22 @@
 m[3+4*0] = 0; m[3+4*1] = 0; m[3+4*2] = 0; m[3+4*3] = 1;
 }
 
+void raydium_math_multiply_matrix4(GLfloat *matrix1,GLfloat *matrix2,GLfloat *result ){
+
+	unsigned int i, j, k;
+	for(i=0; i<4; i++)
+	{
+		for(j=0; j<4; j++)
+		{
+            GLfloat acc = 0;
+		    for (k=0; k<4; k++)
+		        acc += matrix1[i+k*4] * matrix2[k+j*4];
+
+		    result[i+j*4] = acc;
+		}
+	}
+}
+
 // res: GLfloat[3]
 void raydium_math_pos_get_modelview(GLfloat *res)
 {
@@ -152,6 +175,14 @@
 }
 #endif
 
+void raydium_math_matrix4_identity_set(GLfloat * mati){
+    mati[1]=mati[2]=mati[3]=
+    mati[4]=mati[6]=mati[7]=
+    mati[8]=mati[9]=mati[11]=
+    mati[12]=mati[13]=mati[14]=0.0f;
+    mati[0]=mati[5]=mati[10]=mati[15]=1.0f;
+}
+
 // Our matrix_inverse seems broken.
 // This code works, thanks to Alexander Zaprjagaev (frustum@public.tsu.ru)
 int _raydium_math_MatrixInverse(const float *m,float *out) {
@@ -183,6 +214,182 @@
     return 1;
 }
 
+void raydium_math_invert_matrix4(GLfloat * matin, GLfloat * matout){
+
+float det;
+
+#define a(i,j) matin[(i-1)+(j-1)*4]
+#define b(i,j) matout[(i-1)+(j-1)*4]
+
+//http://gskinner.com/RegExr/
+//regexp: a([1-9])([1-9])
+//replace: a($1,$2)
+// wxMaxima
+// a:matrix([a11,a12,a13,a14],[a21,a22,a23,a24],[a31,a32,a33,a34],[a41,a42,a43,a44]);
+// ia:a^^-1;
+// det=
+//subst(det,...
+
+det=((a(1,1)*a(2,2)-a(1,2)*a(2,1))*a(3,3)+(a(1,3)*a(2,1)-a(1,1)*a(2,3))*a(3,2)+(a(1,2)*a(2,3)-a(1,3)*a(2,2))*a(3,1))*a(4,4)
+    +((a(1,2)*a(2,1)-a(1,1)*a(2,2))*a(3,4)+(a(1,1)*a(2,4)-a(1,4)*a(2,1))*a(3,2)+(a(1,4)*a(2,2)-a(1,2)*a(2,4))*a(3,1))*a(4,3)
+    +((a(1,1)*a(2,3)-a(1,3)*a(2,1))*a(3,4)+(a(1,4)*a(2,1)-a(1,1)*a(2,4))*a(3,3)+(a(1,3)*a(2,4)-a(1,4)*a(2,3))*a(3,1))*a(4,2)
+    +((a(1,3)*a(2,2)-a(1,2)*a(2,3))*a(3,4)+(a(1,2)*a(2,4)-a(1,4)*a(2,2))*a(3,3)+(a(1,4)*a(2,3)-a(1,3)*a(2,4))*a(3,2))*a(4,1);
+
+
+b(1,1)= ((a(2,2)*a(3,3)-a(2,3)*a(3,2))*a(4,4)+(a(2,4)*a(3,2)-a(2,2)*a(3,4))*a(4,3)+(a(2,3)*a(3,4)-a(2,4)*a(3,3))*a(4,2))/det;
+b(1,2)= -((a(1,2)*a(3,3)-a(1,3)*a(3,2))*a(4,4)+(a(1,4)*a(3,2)-a(1,2)*a(3,4))*a(4,3)+(a(1,3)*a(3,4)-a(1,4)*a(3,3))*a(4,2))/det;
+b(1,3)= ((a(1,2)*a(2,3)-a(1,3)*a(2,2))*a(4,4)+(a(1,4)*a(2,2)-a(1,2)*a(2,4))*a(4,3)+(a(1,3)*a(2,4)-a(1,4)*a(2,3))*a(4,2))/det;
+b(1,4)= -((a(1,2)*a(2,3)-a(1,3)*a(2,2))*a(3,4)+(a(1,4)*a(2,2)-a(1,2)*a(2,4))*a(3,3)+(a(1,3)*a(2,4)-a(1,4)*a(2,3))*a(3,2))/det;
+
+b(2,1)= -((a(2,1)*a(3,3)-a(2,3)*a(3,1))*a(4,4)+(a(2,4)*a(3,1)-a(2,1)*a(3,4))*a(4,3)+(a(2,3)*a(3,4)-a(2,4)*a(3,3))*a(4,1))/det;
+b(2,2)= ((a(1,1)*a(3,3)-a(1,3)*a(3,1))*a(4,4)+(a(1,4)*a(3,1)-a(1,1)*a(3,4))*a(4,3)+(a(1,3)*a(3,4)-a(1,4)*a(3,3))*a(4,1))/det;
+b(2,3)= -((a(1,1)*a(2,3)-a(1,3)*a(2,1))*a(4,4)+(a(1,4)*a(2,1)-a(1,1)*a(2,4))*a(4,3)+(a(1,3)*a(2,4)-a(1,4)*a(2,3))*a(4,1))/det;
+b(2,4)= ((a(1,1)*a(2,3)-a(1,3)*a(2,1))*a(3,4)+(a(1,4)*a(2,1)-a(1,1)*a(2,4))*a(3,3)+(a(1,3)*a(2,4)-a(1,4)*a(2,3))*a(3,1))/det;
+
+b(3,1)= ((a(2,1)*a(3,2)-a(2,2)*a(3,1))*a(4,4)+(a(2,4)*a(3,1)-a(2,1)*a(3,4))*a(4,2)+(a(2,2)*a(3,4)-a(2,4)*a(3,2))*a(4,1))/det;
+b(3,2)= -((a(1,1)*a(3,2)-a(1,2)*a(3,1))*a(4,4)+(a(1,4)*a(3,1)-a(1,1)*a(3,4))*a(4,2)+(a(1,2)*a(3,4)-a(1,4)*a(3,2))*a(4,1))/det;
+b(3,3)= ((a(1,1)*a(2,2)-a(1,2)*a(2,1))*a(4,4)+(a(1,4)*a(2,1)-a(1,1)*a(2,4))*a(4,2)+(a(1,2)*a(2,4)-a(1,4)*a(2,2))*a(4,1))/det;
+b(3,4)= -((a(1,1)*a(2,2)-a(1,2)*a(2,1))*a(3,4)+(a(1,4)*a(2,1)-a(1,1)*a(2,4))*a(3,2)+(a(1,2)*a(2,4)-a(1,4)*a(2,2))*a(3,1))/det;
+
+b(4,1)= -((a(2,1)*a(3,2)-a(2,2)*a(3,1))*a(4,3)+(a(2,3)*a(3,1)-a(2,1)*a(3,3))*a(4,2)+(a(2,2)*a(3,3)-a(2,3)*a(3,2))*a(4,1))/det;
+b(4,2)= ((a(1,1)*a(3,2)-a(1,2)*a(3,1))*a(4,3)+(a(1,3)*a(3,1)-a(1,1)*a(3,3))*a(4,2)+(a(1,2)*a(3,3)-a(1,3)*a(3,2))*a(4,1))/det;
+b(4,3)= -((a(1,1)*a(2,2)-a(1,2)*a(2,1))*a(4,3)+(a(1,3)*a(2,1)-a(1,1)*a(2,3))*a(4,2)+(a(1,2)*a(2,3)-a(1,3)*a(2,2))*a(4,1))/det;
+b(4,4)= ((a(1,1)*a(2,2)-a(1,2)*a(2,1))*a(3,3)+(a(1,3)*a(2,1)-a(1,1)*a(2,3))*a(3,2)+(a(1,2)*a(2,3)-a(1,3)*a(2,2))*a(3,1))/det;
+
+#undef a
+#undef b
+
+}
+
+void raydium_math_translate_matrix4_3f(GLfloat x, GLfloat y, GLfloat z,GLfloat *matrix){
+	matrix[12]=matrix[0]*x+matrix[4]*y+matrix[8]*z+matrix[12];
+	matrix[13]=matrix[1]*x+matrix[5]*y+matrix[9]*z+matrix[13];
+	matrix[14]=matrix[2]*x+matrix[6]*y+matrix[10]*z+matrix[14];
+	matrix[15]=matrix[3]*x+matrix[7]*y+matrix[11]*z+matrix[15];
+}
+
+void raydium_math_rotate_matrix4_vector_angle(GLfloat angleInRadians, GLfloat x, GLfloat y, GLfloat z,GLfloat *matrix){
+	//See page 191 of Elementary Linear Algebra by Howard Anton
+	GLfloat m[16], rotate[16];
+	GLfloat OneMinusCosAngle, CosAngle, SinAngle;
+	GLfloat A_OneMinusCosAngle, C_OneMinusCosAngle;
+	CosAngle=cosf(angleInRadians);			//Some stuff for optimizing code
+	OneMinusCosAngle=1.0-CosAngle;
+	SinAngle=sinf(angleInRadians);
+	A_OneMinusCosAngle=x*OneMinusCosAngle;
+	C_OneMinusCosAngle=z*OneMinusCosAngle;
+	//Make a copy
+	m[0]=matrix[0];
+	m[1]=matrix[1];
+	m[2]=matrix[2];
+	m[3]=matrix[3];
+	m[4]=matrix[4];
+	m[5]=matrix[5];
+	m[6]=matrix[6];
+	m[7]=matrix[7];
+	m[8]=matrix[8];
+	m[9]=matrix[9];
+	m[10]=matrix[10];
+	m[11]=matrix[11];
+	m[12]=matrix[12];
+	m[13]=matrix[13];
+	m[14]=matrix[14];
+	m[15]=matrix[15];
+
+	rotate[ 0]=x*A_OneMinusCosAngle+CosAngle;
+	rotate[ 1]=y*A_OneMinusCosAngle+z*SinAngle;
+	rotate[ 2]=z*A_OneMinusCosAngle-y*SinAngle;
+	rotate[ 3]=0.0;
+
+	rotate[ 4]=y*A_OneMinusCosAngle-z*SinAngle;
+	rotate[ 5]=y*y*OneMinusCosAngle+CosAngle;
+	rotate[ 6]=y*C_OneMinusCosAngle+x*SinAngle;
+	rotate[ 7]=0.0;
+
+	rotate[ 8]=x*C_OneMinusCosAngle+y*SinAngle;
+	rotate[ 9]=y*C_OneMinusCosAngle-x*SinAngle;
+	rotate[10]=z*C_OneMinusCosAngle+CosAngle;
+	rotate[11]=0.0;
+	//The last column of rotate[] is {0 0 0 1}
+
+	matrix[0]=m[0]*rotate[0]+
+		m[4]*rotate[1]+
+		m[8]*rotate[2];
+
+	matrix[4]=m[0]*rotate[4]+
+		m[4]*rotate[5]+
+		m[8]*rotate[6];
+
+	matrix[8]=m[0]*rotate[8]+
+		m[4]*rotate[9]+
+		m[8]*rotate[10];
+
+	//matrix[12]=matrix[12];
+
+	matrix[1]=m[1]*rotate[0]+
+		m[5]*rotate[1]+
+		m[9]*rotate[2];
+
+	matrix[5]=m[1]*rotate[4]+
+		m[5]*rotate[5]+
+		m[9]*rotate[6];
+
+	matrix[9]=m[1]*rotate[8]+
+		m[5]*rotate[9]+
+		m[9]*rotate[10];
+
+	//matrix[13]=matrix[13];
+
+	matrix[2]=m[2]*rotate[0]+
+		m[6]*rotate[1]+
+		m[10]*rotate[2];
+
+	matrix[6]=m[2]*rotate[4]+
+		m[6]*rotate[5]+
+		m[10]*rotate[6];
+
+	matrix[10]=m[2]*rotate[8]+
+		m[6]*rotate[9]+
+		m[10]*rotate[10];
+
+	//matrix[14]=matrix[14];
+
+	matrix[3]=m[3]*rotate[0]+
+		m[7]*rotate[1]+
+		m[11]*rotate[2];
+
+	matrix[7]=m[3]*rotate[4]+
+		m[7]*rotate[5]+
+		m[11]*rotate[6];
+
+	matrix[11]=m[3]*rotate[8]+
+		m[7]*rotate[9]+
+		m[11]*rotate[10];
+
+	//matrix[15]=matrix[15];
+}
+
+void raydium_math_crossproduct (GLfloat *pvector1, GLfloat *pvector2,GLfloat *normal){
+	normal[0]=(pvector1[1]*pvector2[2])-(pvector1[2]*pvector2[1]);
+	normal[1]=(pvector1[2]*pvector2[0])-(pvector1[0]*pvector2[2]);
+	normal[2]=(pvector1[0]*pvector2[1])-(pvector1[1]*pvector2[0]);
+}
+
+void raydium_math_multiply_matrix4_vectors_array(GLfloat *matrix1,GLfloat *vect, int n_elems,GLfloat *result){
+	unsigned int i, j, k;
+	for(i=0; i<4; i++)
+	{
+		for(j=0; j<n_elems; j++)
+		{
+		    GLfloat acc = 0;
+		    for (k=0; k<4; k++)
+		        acc += matrix1[i+k*4] * vect[k+j*4];
+
+		    result[i+j*4] = acc;
+		}
+	}
+}
+
 void raydium_math_quaternion_normalize(float *quat)
 {
 float magnitude;