00001 #ifndef TRANSFORMMATRIX_H
00002 #define TRANSFORMMATRIX_H
00003
00004 #include <valarray>
00005 #include <iostream>
00006 #include <cmath>
00007 #include <cassert>
00008
00009 #ifndef size_t
00010 typedef unsigned int size_t;
00011 #endif
00012
00013 using namespace std;
00014
00023 template<class T> class TransformMatrix{
00024 public:
00026 TransformMatrix() : matrixdata(16) { for(size_t k=0; k<16;k+=5) matrixdata[k]=T(1.0); }
00027
00031 TransformMatrix(const TransformMatrix<T> &src) : matrixdata(src.matrixdata) {}
00032
00034 virtual ~TransformMatrix() { }
00035
00039 void reset() { matrixdata=0; for(size_t k=0; k<16;k+=5) matrixdata[k]=T(1.0); }
00040
00048 template<class Vector> void transform(const Vector &v,Vector &vt) const;
00049
00057 template<class Vector> void transformHomogen(const Vector &v,Vector &vt) const;
00058
00065 void translate(T x, T y, T z);
00066
00071 void rotateAroundZ(T phi);
00072
00079 T operator()(size_t row,size_t column) const { assert(row < 4); assert(column <4 ); return matrixdata[column+4*row]; }
00080
00087 T &operator()(size_t row,size_t column) { assert(row < 4); assert(column <4 ); return matrixdata[column+4*row]; }
00088
00094 bool operator==(const TransformMatrix<T> &other) const { for(size_t k=0; k<16;k++) if(matrixdata[k]!=other.matrixdata[k]) return false; return true; }
00095
00101 bool operator!=(const TransformMatrix<T> &other) { for(size_t k=0; k<16;k++) if(matrixdata[k]!=other.matrixdata[k]) return true; return false; }
00102
00107 void operator+=(const TransformMatrix<T> &other) { matrixdata += other.matrixdata; }
00108
00113 void operator-=(const TransformMatrix<T> &other) { matrixdata -= other.matrixdata; }
00114
00119 inline void operator*=(const TransformMatrix<T> &b);
00120
00126 void operator*=(T scale) { matrixdata *= scale; }
00127
00128 private:
00130 valarray<T> matrixdata;
00131 };
00132
00133
00140 template<class T> TransformMatrix<T> operator+(const TransformMatrix<T> &a,const TransformMatrix<T> &b)
00141 {
00142 TransformMatrix<T> c(a);
00143 c+=b;
00144 return c;
00145 }
00146
00153 template<class T> TransformMatrix<T> operator-(const TransformMatrix<T> &a,const TransformMatrix<T> &b)
00154 {
00155 TransformMatrix<T> c(a);
00156 c-=b;
00157 return c;
00158 }
00159
00166 template<class T> TransformMatrix<T> operator*(const TransformMatrix<T> &a,const TransformMatrix<T> &b)
00167 {
00168 TransformMatrix<T> c(a);
00169 c*=b;
00170 return c;
00171 }
00172
00179 template<class T> TransformMatrix<T> operator*(const TransformMatrix<T> &a,T b)
00180 {
00181 TransformMatrix<T> c(a);
00182 c*=b;
00183 return b;
00184 }
00185
00192
00193
00194
00195
00196
00197
00198
00199 template<class T> void TransformMatrix<T>::operator*=(const TransformMatrix<T> &b)
00200 {
00201 TransformMatrix<T> a(*this);
00202 matrixdata[0]=a.matrixdata[0]*b.matrixdata[0]+a.matrixdata[1]*b.matrixdata[4]+a.matrixdata[2]*b.matrixdata[8]+a.matrixdata[3]*b.matrixdata[12];
00203 matrixdata[1]=a.matrixdata[0]*b.matrixdata[1]+a.matrixdata[1]*b.matrixdata[5]+a.matrixdata[2]*b.matrixdata[9]+a.matrixdata[3]*b.matrixdata[13];
00204 matrixdata[2]=a.matrixdata[0]*b.matrixdata[2]+a.matrixdata[1]*b.matrixdata[6]+a.matrixdata[2]*b.matrixdata[10]+a.matrixdata[3]*b.matrixdata[14];
00205 matrixdata[3]=a.matrixdata[0]*b.matrixdata[3]+a.matrixdata[1]*b.matrixdata[7]+a.matrixdata[2]*b.matrixdata[11]+a.matrixdata[3]*b.matrixdata[15];
00206
00207 matrixdata[4]=a.matrixdata[4]*b.matrixdata[0]+a.matrixdata[5]*b.matrixdata[4]+a.matrixdata[6]*b.matrixdata[8]+a.matrixdata[7]*b.matrixdata[12];
00208 matrixdata[5]=a.matrixdata[4]*b.matrixdata[1]+a.matrixdata[5]*b.matrixdata[5]+a.matrixdata[6]*b.matrixdata[9]+a.matrixdata[7]*b.matrixdata[13];
00209 matrixdata[6]=a.matrixdata[4]*b.matrixdata[2]+a.matrixdata[5]*b.matrixdata[6]+a.matrixdata[6]*b.matrixdata[10]+a.matrixdata[7]*b.matrixdata[14];
00210 matrixdata[7]=a.matrixdata[4]*b.matrixdata[3]+a.matrixdata[5]*b.matrixdata[7]+a.matrixdata[6]*b.matrixdata[11]+a.matrixdata[7]*b.matrixdata[15];
00211
00212 matrixdata[8]=a.matrixdata[8]*b.matrixdata[0]+a.matrixdata[9]*b.matrixdata[4]+a.matrixdata[10]*b.matrixdata[8]+a.matrixdata[11]*b.matrixdata[12];
00213 matrixdata[9]=a.matrixdata[8]*b.matrixdata[1]+a.matrixdata[9]*b.matrixdata[5]+a.matrixdata[10]*b.matrixdata[9]+a.matrixdata[11]*b.matrixdata[13];
00214 matrixdata[10]=a.matrixdata[8]*b.matrixdata[2]+a.matrixdata[9]*b.matrixdata[6]+a.matrixdata[10]*b.matrixdata[10]+a.matrixdata[11]*b.matrixdata[14];
00215 matrixdata[11]=a.matrixdata[8]*b.matrixdata[3]+a.matrixdata[9]*b.matrixdata[7]+a.matrixdata[10]*b.matrixdata[11]+a.matrixdata[11]*b.matrixdata[15];
00216
00217 matrixdata[12]=a.matrixdata[12]*b.matrixdata[0]+a.matrixdata[13]*b.matrixdata[4]+a.matrixdata[14]*b.matrixdata[8]+a.matrixdata[15]*b.matrixdata[12];
00218 matrixdata[13]=a.matrixdata[12]*b.matrixdata[1]+a.matrixdata[13]*b.matrixdata[5]+a.matrixdata[14]*b.matrixdata[9]+a.matrixdata[15]*b.matrixdata[13];
00219 matrixdata[14]=a.matrixdata[12]*b.matrixdata[2]+a.matrixdata[13]*b.matrixdata[6]+a.matrixdata[14]*b.matrixdata[10]+a.matrixdata[15]*b.matrixdata[14];
00220 matrixdata[15]=a.matrixdata[12]*b.matrixdata[3]+a.matrixdata[13]*b.matrixdata[7]+a.matrixdata[14]*b.matrixdata[11]+a.matrixdata[15]*b.matrixdata[15];
00221 }
00222
00223 template<class T> ostream &operator<<(ostream &output,const TransformMatrix<T> &matrix)
00224 {
00225 for(size_t row=0;row < 4;row++)
00226 {
00227 for(size_t column=0; column < 4; column++)
00228 {
00229 output << matrix(row,column) << " ";
00230 }
00231 output << endl;
00232 }
00233 return output;
00234 }
00235
00236 template<class T> template<class Vector> void TransformMatrix<T>::transform(const Vector &v,Vector &vt) const
00237 {
00238 vt[0]=v[0]*matrixdata[0]+v[1]*matrixdata[4]+v[2]*matrixdata[8]+matrixdata[12];
00239 vt[1]=v[0]*matrixdata[1]+v[1]*matrixdata[5]+v[2]*matrixdata[9]+matrixdata[13];
00240 vt[2]=v[0]*matrixdata[2]+v[1]*matrixdata[6]+v[2]*matrixdata[10]+matrixdata[14];
00241 }
00242
00243 template<class T> template<class Vector> void TransformMatrix<T>::transformHomogen(const Vector &v,Vector &vt) const
00244 {
00245 vt[0]=v[0]*matrixdata[0]+v[1]*matrixdata[4]+v[2]*matrixdata[8]+matrixdata[12];
00246 vt[1]=v[0]*matrixdata[1]+v[1]*matrixdata[5]+v[2]*matrixdata[9]+matrixdata[13];
00247 vt[2]=v[0]*matrixdata[2]+v[1]*matrixdata[6]+v[2]*matrixdata[10]+matrixdata[14];
00248 vt[3]=v[0]*matrixdata[3]+v[1]*matrixdata[7]+v[2]*matrixdata[11]+matrixdata[15];
00249 }
00250
00251
00252 template<class T> void TransformMatrix<T>::translate(T x, T y, T z)
00253 {
00254 matrixdata[0]=matrixdata[0]+x*matrixdata[3];
00255 matrixdata[1]=matrixdata[1]+y*matrixdata[3];
00256 matrixdata[2]=matrixdata[2]+z*matrixdata[3];
00257
00258 matrixdata[4]=matrixdata[4]+x*matrixdata[7];
00259 matrixdata[5]=matrixdata[5]+y*matrixdata[7];
00260 matrixdata[6]=matrixdata[6]+z*matrixdata[7];
00261
00262 matrixdata[8]=matrixdata[8]+x*matrixdata[11];
00263 matrixdata[9]=matrixdata[9]+y*matrixdata[11];
00264 matrixdata[10]=matrixdata[10]+z*matrixdata[11];
00265
00266 matrixdata[12]=matrixdata[12]+x*matrixdata[15];
00267 matrixdata[13]=matrixdata[13]+y*matrixdata[15];
00268 matrixdata[14]=matrixdata[14]+z*matrixdata[15];
00269 }
00270
00271 template<class T> void TransformMatrix<T>::rotateAroundZ(T phi)
00272 {
00273 T c=cos(phi);
00274 T s=sin(phi);
00275
00276 T a0=matrixdata[0];
00277 matrixdata[0]=a0*c-matrixdata[1]*s;
00278 matrixdata[1]=matrixdata[1]*c+a0*s;
00279
00280 a0=matrixdata[4];
00281 matrixdata[4]=a0*c-matrixdata[5]*s;
00282 matrixdata[5]=matrixdata[5]*c+a0*s;
00283
00284 a0=matrixdata[8];
00285 matrixdata[8]=a0*c-matrixdata[9]*s;
00286 matrixdata[9]=matrixdata[9]*c+a0*s;
00287
00288 a0=matrixdata[12];
00289 matrixdata[12]=a0*c-matrixdata[13]*s;
00290 matrixdata[13]=matrixdata[13]*c+a0*s;
00291 }
00292 #endif // TRANSFORMMATRIX_H