matrix.c (3725B)
1 /* 2 * ident(m) store identity matrix in m 3 * matmul(a, b) matrix multiply a*=b 4 * matmulr(a, b) matrix multiply a=b*a 5 * determinant(m) returns det(m) 6 * adjoint(m, minv) minv=adj(m) 7 * invertmat(m, minv) invert matrix m, result in minv, returns det(m) 8 * if m is singular, minv=adj(m) 9 */ 10 #include <u.h> 11 #include <libc.h> 12 #include <draw.h> 13 #include <geometry.h> 14 void ident(Matrix m){ 15 register double *s=&m[0][0]; 16 *s++=1;*s++=0;*s++=0;*s++=0; 17 *s++=0;*s++=1;*s++=0;*s++=0; 18 *s++=0;*s++=0;*s++=1;*s++=0; 19 *s++=0;*s++=0;*s++=0;*s=1; 20 } 21 void matmul(Matrix a, Matrix b){ 22 int i, j, k; 23 double sum; 24 Matrix tmp; 25 for(i=0;i!=4;i++) for(j=0;j!=4;j++){ 26 sum=0; 27 for(k=0;k!=4;k++) 28 sum+=a[i][k]*b[k][j]; 29 tmp[i][j]=sum; 30 } 31 for(i=0;i!=4;i++) for(j=0;j!=4;j++) 32 a[i][j]=tmp[i][j]; 33 } 34 void matmulr(Matrix a, Matrix b){ 35 int i, j, k; 36 double sum; 37 Matrix tmp; 38 for(i=0;i!=4;i++) for(j=0;j!=4;j++){ 39 sum=0; 40 for(k=0;k!=4;k++) 41 sum+=b[i][k]*a[k][j]; 42 tmp[i][j]=sum; 43 } 44 for(i=0;i!=4;i++) for(j=0;j!=4;j++) 45 a[i][j]=tmp[i][j]; 46 } 47 /* 48 * Return det(m) 49 */ 50 double determinant(Matrix m){ 51 return m[0][0]*(m[1][1]*(m[2][2]*m[3][3]-m[2][3]*m[3][2])+ 52 m[1][2]*(m[2][3]*m[3][1]-m[2][1]*m[3][3])+ 53 m[1][3]*(m[2][1]*m[3][2]-m[2][2]*m[3][1])) 54 -m[0][1]*(m[1][0]*(m[2][2]*m[3][3]-m[2][3]*m[3][2])+ 55 m[1][2]*(m[2][3]*m[3][0]-m[2][0]*m[3][3])+ 56 m[1][3]*(m[2][0]*m[3][2]-m[2][2]*m[3][0])) 57 +m[0][2]*(m[1][0]*(m[2][1]*m[3][3]-m[2][3]*m[3][1])+ 58 m[1][1]*(m[2][3]*m[3][0]-m[2][0]*m[3][3])+ 59 m[1][3]*(m[2][0]*m[3][1]-m[2][1]*m[3][0])) 60 -m[0][3]*(m[1][0]*(m[2][1]*m[3][2]-m[2][2]*m[3][1])+ 61 m[1][1]*(m[2][2]*m[3][0]-m[2][0]*m[3][2])+ 62 m[1][2]*(m[2][0]*m[3][1]-m[2][1]*m[3][0])); 63 } 64 /* 65 * Store the adjoint (matrix of cofactors) of m in madj. 66 * Works fine even if m and madj are the same matrix. 67 */ 68 void adjoint(Matrix m, Matrix madj){ 69 double m00=m[0][0], m01=m[0][1], m02=m[0][2], m03=m[0][3]; 70 double m10=m[1][0], m11=m[1][1], m12=m[1][2], m13=m[1][3]; 71 double m20=m[2][0], m21=m[2][1], m22=m[2][2], m23=m[2][3]; 72 double m30=m[3][0], m31=m[3][1], m32=m[3][2], m33=m[3][3]; 73 madj[0][0]=m11*(m22*m33-m23*m32)+m21*(m13*m32-m12*m33)+m31*(m12*m23-m13*m22); 74 madj[0][1]=m01*(m23*m32-m22*m33)+m21*(m02*m33-m03*m32)+m31*(m03*m22-m02*m23); 75 madj[0][2]=m01*(m12*m33-m13*m32)+m11*(m03*m32-m02*m33)+m31*(m02*m13-m03*m12); 76 madj[0][3]=m01*(m13*m22-m12*m23)+m11*(m02*m23-m03*m22)+m21*(m03*m12-m02*m13); 77 madj[1][0]=m10*(m23*m32-m22*m33)+m20*(m12*m33-m13*m32)+m30*(m13*m22-m12*m23); 78 madj[1][1]=m00*(m22*m33-m23*m32)+m20*(m03*m32-m02*m33)+m30*(m02*m23-m03*m22); 79 madj[1][2]=m00*(m13*m32-m12*m33)+m10*(m02*m33-m03*m32)+m30*(m03*m12-m02*m13); 80 madj[1][3]=m00*(m12*m23-m13*m22)+m10*(m03*m22-m02*m23)+m20*(m02*m13-m03*m12); 81 madj[2][0]=m10*(m21*m33-m23*m31)+m20*(m13*m31-m11*m33)+m30*(m11*m23-m13*m21); 82 madj[2][1]=m00*(m23*m31-m21*m33)+m20*(m01*m33-m03*m31)+m30*(m03*m21-m01*m23); 83 madj[2][2]=m00*(m11*m33-m13*m31)+m10*(m03*m31-m01*m33)+m30*(m01*m13-m03*m11); 84 madj[2][3]=m00*(m13*m21-m11*m23)+m10*(m01*m23-m03*m21)+m20*(m03*m11-m01*m13); 85 madj[3][0]=m10*(m22*m31-m21*m32)+m20*(m11*m32-m12*m31)+m30*(m12*m21-m11*m22); 86 madj[3][1]=m00*(m21*m32-m22*m31)+m20*(m02*m31-m01*m32)+m30*(m01*m22-m02*m21); 87 madj[3][2]=m00*(m12*m31-m11*m32)+m10*(m01*m32-m02*m31)+m30*(m02*m11-m01*m12); 88 madj[3][3]=m00*(m11*m22-m12*m21)+m10*(m02*m21-m01*m22)+m20*(m01*m12-m02*m11); 89 } 90 /* 91 * Store the inverse of m in minv. 92 * If m is singular, minv is instead its adjoint. 93 * Returns det(m). 94 * Works fine even if m and minv are the same matrix. 95 */ 96 double invertmat(Matrix m, Matrix minv){ 97 double d, dinv; 98 int i, j; 99 d=determinant(m); 100 adjoint(m, minv); 101 if(d!=0.){ 102 dinv=1./d; 103 for(i=0;i!=4;i++) for(j=0;j!=4;j++) minv[i][j]*=dinv; 104 } 105 return d; 106 }