#include #include #include #include "matrixmath.h" //// matrix manipulation package void identityMatrix(double m[16]) { int i; for(i=1;i<15;i++) m[i]=0.0; m[0]=m[5]=m[10]=m[15]=1.0; } // r=aXb void crossV3(double a[3],double b[3],double r[3]) { r[0]=-(a[2]*b[1]) + a[1]*b[2]; r[1]=a[2]*b[0] - a[0]*b[2]; r[2]=-(a[1]*b[0]) + a[0]*b[1]; } // r=a-b void subV3(double a[3],double b[3],double r[3]) { r[0]=a[0]-b[0]; r[1]=a[1]-b[1]; r[2]=a[2]-b[2]; } int rotationMatrix(double a,double n[3],double r[16]) { double u,v,w,u2,v2,w2,ca,sa,uvw2,suvw2; u=n[0]; v=n[1]; w=n[2]; u2=n[0]*n[0]; v2=n[1]*n[1]; w2=n[2]*n[2]; ca=cos(a); sa=sin(a); uvw2=u2+v2+w2; suvw2=sqrt(uvw2); if( a==0.0 ) { identityMatrix(r); return(0); } if( uvw2==0.0 ) { identityMatrix(r); return(-1); } r[0]=(u2 + ca*(v2 + w2))/uvw2; r[1]=(u*v - ca*u*v - sa*suvw2*w)/uvw2; r[2]=(sa*suvw2*v + u*w - ca*u*w)/uvw2; r[3]=0; r[4]=(u*v - ca*u*v + sa*suvw2*w)/uvw2; r[5]=(v2 + ca*(u2 + w2))/uvw2; r[6]=-((sa*u*suvw2 + (-1 + ca)*v*w)/uvw2); r[7]=0; r[8]=-((sa*suvw2*v + (-1 + ca)*u*w)/uvw2); r[9]=(sa*u*suvw2 + v*w - ca*v*w)/ uvw2; r[10]=(ca*(u2 + v2) + w2)/uvw2; r[11]=0; r[12]=0; r[13]=0; r[14]=0; r[15]=1; return(0); } void translationMatrix(double t[3],double m[16]) { m[0]=1.0; m[1]=0.0; m[2]=0.0; m[3]=t[0]; m[4]=0.0; m[5]=1.0; m[6]=0.0; m[7]=t[1]; m[8]=0.0; m[9]=0.0; m[10]=1.0; m[11]=t[2]; m[12]=0.0; m[13]=0.0; m[14]=0.0; m[15]=1.0; } void scaleMatrix(double s[3],double m[16]) { m[0]=s[0]; m[1]=0.0; m[2]=0.0; m[3]=0.0; m[4]=0.0; m[5]=s[1]; m[6]=0.0; m[7]=0.0; m[8]=0.0; m[9]=0.0; m[10]=s[2]; m[11]=0.0; m[12]=0.0; m[13]=0.0; m[14]=0.0; m[15]=1.0; } void internalMatrix(double fx,double fy,double cx,double cy,double m[16]) { m[0]=fx;m[1]=0.0;m[2]=cx;m[3]=0.0; m[4]=0.0;m[5]=fy;m[6]=cy;m[7]=0.0; m[8]=0.0;m[9]=0.0;m[10]=1.0;m[11]=0.0; m[12]=0.0;m[13]=0.0;m[14]=0.0;m[15]=1.0; } double norm2(double *w,int len) { double s; int i; s=0.0; for(i=0;i m(4x4) void multMM(double a[16],double b[16],double m[16]) { m[0]=a[0]*b[0] + a[3]*b[12] + a[1]*b[4] + a[2]*b[8]; m[1]=a[0]*b[1] + a[3]*b[13] + a[1]*b[5] + a[2]*b[9]; m[2]=a[2]*b[10] + a[3]*b[14] + a[0]*b[2] + a[1]*b[6]; m[3]=a[2]*b[11] + a[3]*b[15] + a[0]*b[3] + a[1]*b[7]; m[4]=a[4]*b[0] + a[7]*b[12] + a[5]*b[4] + a[6]*b[8]; m[5]=a[4]*b[1] + a[7]*b[13] + a[5]*b[5] + a[6]*b[9]; m[6]=a[6]*b[10] + a[7]*b[14] + a[4]*b[2] + a[5]*b[6]; m[7]=a[6]*b[11] + a[7]*b[15] + a[4]*b[3] + a[5]*b[7]; m[8]=a[8]*b[0] + a[11]*b[12] + a[9]*b[4] + a[10]*b[8]; m[9]=a[8]*b[1] + a[11]*b[13] + a[9]*b[5] + a[10]*b[9]; m[10]=a[10]*b[10] + a[11]*b[14] + a[8]*b[2] + a[9]*b[6]; m[11]=a[10]*b[11] + a[11]*b[15] + a[8]*b[3] + a[9]*b[7]; m[12]=a[12]*b[0] + a[15]*b[12] + a[13]*b[4] + a[14]*b[8]; m[13]=a[12]*b[1] + a[15]*b[13] + a[13]*b[5] + a[14]*b[9]; m[14]=a[14]*b[10] + a[15]*b[14] + a[12]*b[2] + a[13]*b[6]; m[15]=a[14]*b[11] + a[15]*b[15] + a[12]*b[3] + a[13]*b[7]; } // w : axis + magnitude of rot void externalMatrix(double t[3],double w[3],double m[16]) { double tm[16],wm[16],L; L=norm(w,3); translationMatrix(t,tm); rotationMatrix(L,w,wm); //multMM(tm,wm,m); multMM(wm,tm,m); } void copyM(double a[16],double b[16]) { int i; for(i=0;i<16;i++) b[i]=a[i]; } void copyV(double a[4],double b[4]) { b[0]=a[0]; b[1]=a[1]; b[2]=a[2]; b[3]=a[3]; } /// une matrice complete // contient dans un vecteur p: fx fy cx cy tx ty tz wx wy wz (10 params) // min et mext sont les matrices internes/externes, si desire... void cameraMatrix(double p[10],double m[16],double min[16],double mex[16]) { double internM[16],externM[16]; internalMatrix(p[0],p[1],p[2],p[3],internM); externalMatrix(p+4,p+7,externM); multMM(internM,externM,m); if( min ) copyM(internM,min); if( mex ) copyM(externM,mex); } // suppose que m[12..15] = {0,0,0,1} int inverseMaffine(double m[16],double im[16]) { double det; int i; det=-(m[1]*m[10]*m[4]) + m[0]*m[10]*m[5] - m[2]*m[5]*m[8] + m[1]*m[6]*m[8] + m[2]*m[4]*m[9] - m[0]*m[6]*m[9]; if( det==0.0 ) return(-1); im[0]=m[10]*m[5] - m[6]*m[9]; im[1]=-(m[1]*m[10]) + m[2]*m[9]; im[2]=-(m[2]*m[5]) + m[1]*m[6]; im[3]=m[11]*m[2]*m[5] - m[10]*m[3]*m[5] - m[1]*m[11]*m[6] + m[1]*m[10]*m[7] + m[3]*m[6]*m[9] - m[2]*m[7]*m[9]; im[4]=-(m[10]*m[4]) + m[6]*m[8]; im[5]=m[0]*m[10] - m[2]*m[8]; im[6]=m[2]*m[4] - m[0]*m[6]; im[7]=-(m[11]*m[2]*m[4]) + m[10]*m[3]*m[4] + m[0]*m[11]*m[6] - m[0]*m[10]*m[7] - m[3]*m[6]*m[8] + m[2]*m[7]*m[8]; im[8]= -(m[5]*m[8]) + m[4]*m[9]; im[9]=m[1]*m[8] - m[0]*m[9]; im[10]=-(m[1]*m[4]) + m[0]*m[5]; im[11]=m[1]*m[11]*m[4] - m[0]*m[11]*m[5] + m[3]*m[5]*m[8] - m[1]*m[7]*m[8] - m[3]*m[4]*m[9] + m[0]*m[7]*m[9]; im[12]=0.0; im[13]=0.0; im[14]=0.0; im[15]=1.0; for(i=0;i<=11;i++) im[i]/=det; return(0); } // inverse general int inverse(double m[16],double im[16]) { double det; int i; det=m[1]*m[11]*m[14]*m[4] - m[1]*m[10]*m[15]*m[4] - m[11]*m[13]*m[2]*m[4] + m[10]*m[13]*m[3]*m[4] - m[0]*m[11]*m[14]*m[5] + m[0]*m[10]*m[15]*m[5] + m[11]*m[12]*m[2]*m[5] - m[10]*m[12]*m[3]*m[5] - m[1]*m[11]*m[12]*m[6] + m[0]*m[11]*m[13]*m[6] + m[1]*m[10]*m[12]*m[7] - m[0]*m[10]*m[13]*m[7] - m[15]*m[2]*m[5]*m[8] + m[14]*m[3]*m[5]*m[8] + m[1]*m[15]*m[6]*m[8] - m[13]*m[3]*m[6]*m[8] - m[1]*m[14]*m[7]*m[8] + m[13]*m[2]*m[7]*m[8] + m[15]*m[2]*m[4]*m[9] - m[14]*m[3]*m[4]*m[9] - m[0]*m[15]*m[6]*m[9] + m[12]*m[3]*m[6]*m[9] + m[0]*m[14]*m[7]*m[9] - m[12]*m[2]*m[7]*m[9]; if( det v(4x1) void multMV(double a[16],double b[4],double v[4]) { v[0]=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]+a[3]*b[3]; v[1]=a[4]*b[0]+a[5]*b[1]+a[6]*b[2]+a[7]*b[3]; v[2]=a[8]*b[0]+a[9]*b[1]+a[10]*b[2]+a[11]*b[3]; v[3]=a[12]*b[0]+a[13]*b[1]+a[14]*b[2]+a[15]*b[3]; } // homogenize a 3D projective point, so you have [x,y,z,1] int homogene3d(double v[4]) { int k=0; if( v[3]==0.0 ) { v[3]=1e-8;k=-1; } v[0]/=v[3]; v[1]/=v[3]; v[2]/=v[3]; v[3]=1.0; return(k); } // homogenize a 2D projective point, so you have [x,y,1,w] int homogene2d(double v[4]) { int k=0; if( v[2]==0.0 ) { v[2]=1e-8;k=-1; } v[0]/=v[2]; v[1]/=v[2]; v[3]/=v[2]; v[2]=1.0; return(k); } ///// DEBUG ////// #ifdef SKIP int main(int argc,char *argv[]) { double r1[16],r2[16],n[3]; double s1[16],is1[16]; double cam[16],icam[16]; double camIn[16],camEx[16]; double p[10],qa[4],pa[4],pb[4]; double L; printf("allo\n"); n[0]=1.0; n[1]=0.0; n[2]=0.0; rotationMatrix(M_PI/4.0,n,r1); n[0]=1.0; n[1]=1.0; n[2]=1.0; rotationMatrix(-M_PI/4.0,n,r2); n[0]=2.0; n[1]=3.0; n[2]=4.0; scaleMatrix(n,r2); multMM(r1,r2,s1); dump4x4(r1); dump4x4(r2); dump4x4(s1); inverseMaffine(s1,is1); dump4x4(is1); inverseMaffine(is1,s1); dump4x4(s1); internalMatrix(256.0,128.0,64.0,64.0,s1); dump4x4(s1); // interne p[0]=512.0; p[1]=512.0; p[2]=512.0; p[3]=512.0; // translation p[4]=1.0; p[5]=2.0; p[6]=3.0; // rotation p[7]=1.0; p[8]=1.0; p[9]=1.0; normalize(p+7,3); // make p[7,8,9] into an axis L=10*1.5*M_PI/180.0; // rotation magnitude p[7]*=L;p[8]*=L;p[9]*=L; cameraMatrix(p,cam,camIn,camEx); dump4x4(cam); inverseMaffine(cam,icam); dump4x4(icam); qa[0]=10.0; qa[1]=15.0; qa[2]=20.0; qa[3]=1.0; multMV(cam,qa,pa); dump4x1(qa);printf(" -> ");dump4x1(pa); homogene2d(pa); printf(" -H-> ");dump4x1(pa); printf("\n"); multMV(icam,pa,pb); printf("in 3d -> ");dump4x1(pb); homogene3d(pb); printf(" -H-> ");dump4x1(pb); printf("\n"); pa[3]/=2; multMV(icam,pa,pb); printf("in 3d -> ");dump4x1(pb); homogene3d(pb); printf(" -H-> ");dump4x1(pb); printf("\n"); pa[0]=1.0; pa[1]=0.0; pa[2]=0.0; pa[3]=0.0; multMV(camEx,pa,pb); printf("NORMALE: ");dump4x1(pa);printf(" --> ");dump4x1(pb);printf("\n"); } #endif