sys/src/libgeometry/matrix.c

   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 }