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