diff --git a/t.cc b/t.cc index 6a26ada..6e8ecae 100644 --- a/t.cc +++ b/t.cc @@ -2792,6 +2792,25 @@ if(sizeof(int)!=read(f,&seed,sizeof(int))) laerror("cannot read /dev/random"); close(f); srand(seed); +Mat3 a; +a.randomize(10.); +cout< b; +b.randomize(10.); +cout < x = a.linear_solve(b); +cout < a; a.randomize(10.); cout<::transposeme() {T t; t=q[0][1]; q[0][1]=q[1][0]; q[1][0]=t; t=q[0][2]; q[0][2]=q[2][0]; q[2][0]=t; t=q[1][2]; q[1][2]=q[2][1]; q[2][1]=t;}; template -const Mat3 Mat3::inverse() const +const Mat3 Mat3::inverse(T *det) const { Mat3 r; r[0][0]= q[2][2]*q[1][1]-q[2][1]*q[1][2]; @@ -219,7 +219,27 @@ const Mat3 Mat3::inverse() const r[2][0]= q[2][1]*q[1][0]-q[2][0]*q[1][1]; r[2][1]= -q[2][1]*q[0][0]+q[2][0]*q[0][1]; r[2][2]= q[1][1]*q[0][0]-q[1][0]*q[0][1]; - return r/determinant(); + T d=determinant(); + if(det) *det=d; + return r/d; + } + +template +const Vec3 Mat3::linear_solve(const Vec3 &rhs, T *det) const + { + Vec3 r; + r[0]= ( q[2][2]*q[1][1]-q[2][1]*q[1][2]) * rhs[0] + +( -q[2][2]*q[0][1]+q[2][1]*q[0][2]) * rhs[1] + +( +q[1][2]*q[0][1]-q[1][1]*q[0][2]) * rhs[2]; + r[1]= (-q[2][2]*q[1][0]+q[2][0]*q[1][2]) * rhs[0] + +( +q[2][2]*q[0][0]-q[2][0]*q[0][2]) * rhs[1] + +( -q[1][2]*q[0][0]+q[1][0]*q[0][2]) * rhs[2]; + r[2]= ( q[2][1]*q[1][0]-q[2][0]*q[1][1]) * rhs[0] + +( -q[2][1]*q[0][0]+q[2][0]*q[0][1]) *rhs[1] + +( q[1][1]*q[0][0]-q[1][0]*q[0][1]) * rhs[2]; + T d=determinant(); + if(det) *det=d; + return r/d; } template diff --git a/vecmat3.h b/vecmat3.h index d6c4461..b1b2eda 100644 --- a/vecmat3.h +++ b/vecmat3.h @@ -172,7 +172,8 @@ public: T determinant() const; void transposeme(); const Mat3 transpose() const {Mat3 r(*this); r.transposeme(); return r;}; - const Mat3 inverse() const; + const Mat3 inverse(T *det = NULL) const; + const Vec3 linear_solve(const Vec3 &rhs, T *det = NULL) const; //C-style IO int fprintf(FILE *f, const char *format) const {int n= ::fprintf(f,format,q[0][0],q[0][1],q[0][2]); n+=::fprintf(f,format,q[1][0],q[1][1],q[1][2]); n+=::fprintf(f,format,q[2][0],q[2][1],q[2][2]); return n;}; int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);};