| 2 |
- |
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 |
register 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 |
register 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 |
}
|