00001 00002 template<typename Real> 00003 m4x4<Real>& m4x4<Real>::set( 00004 const Real a, const Real b, const Real c, const Real d, 00005 const Real e, const Real f, const Real g, const Real h, 00006 const Real i, const Real j, const Real k, const Real l, 00007 const Real m, const Real n, const Real o, const Real p) 00008 { 00009 this->m[0][0]=a; this->m[0][1]=b; this->m[0][2]=c; this->m[0][3]=d; 00010 this->m[1][0]=e; this->m[1][1]=f; this->m[1][2]=g; this->m[1][3]=h; 00011 this->m[2][0]=i; this->m[2][1]=j; this->m[2][2]=k; this->m[2][3]=l; 00012 this->m[3][0]=m; this->m[3][1]=n; this->m[3][2]=o; this->m[3][3]=p; 00013 return *this; 00014 } 00015 00016 template<typename Real> 00017 m4x4<Real>& m4x4<Real>::get( 00018 Real& a, Real& b, Real& c, Real& d, 00019 Real& e, Real& f, Real& g, Real& h, 00020 Real& i, Real& j, Real& k, Real& l, 00021 Real& m, Real& n, Real& o, Real& p) 00022 { 00023 a=this->m[0][0]; b=this->m[0][1]; c=this->m[0][2]; d=this->m[0][3]; 00024 e=this->m[1][0]; f=this->m[1][1]; g=this->m[1][2]; h=this->m[1][3]; 00025 i=this->m[2][0]; j=this->m[2][1]; k=this->m[2][2]; l=this->m[2][3]; 00026 m=this->m[3][0]; n=this->m[3][1]; o=this->m[3][2]; p=this->m[3][3]; 00027 return *this; 00028 } 00029 00030 template<typename Real> 00031 m4x4<Real>& m4x4<Real>::zero(void){ 00032 m[0][0]=0.0f; m[0][1]=0.0f; m[0][2]=0.0f; m[0][3]=0.0f; 00033 m[1][0]=0.0f; m[1][1]=0.0f; m[1][2]=0.0f; m[1][3]=0.0f; 00034 m[2][0]=0.0f; m[2][1]=0.0f; m[2][2]=0.0f; m[2][3]=0.0f; 00035 m[3][0]=0.0f; m[3][1]=0.0f; m[3][2]=0.0f; m[3][3]=0.0f; 00036 return *this; 00037 } 00038 00039 template<typename Real> 00040 m4x4<Real>& m4x4<Real>::identity(void){ 00041 m[0][0]=1.0f; m[0][1]=0.0f; m[0][2]=0.0f; m[0][3]=0.0f; 00042 m[1][0]=0.0f; m[1][1]=1.0f; m[1][2]=0.0f; m[1][3]=0.0f; 00043 m[2][0]=0.0f; m[2][1]=0.0f; m[2][2]=1.0f; m[2][3]=0.0f; 00044 m[3][0]=0.0f; m[3][1]=0.0f; m[3][2]=0.0f; m[3][3]=1.0f; 00045 return *this; 00046 } 00047 00048 template<typename Real> 00049 Real m4x4<Real>::det(void){ 00050 const Real fA0 = m16[ 0]*m16[ 5] - m16[ 1]*m16[ 4]; 00051 const Real fA1 = m16[ 0]*m16[ 6] - m16[ 2]*m16[ 4]; 00052 const Real fA2 = m16[ 0]*m16[ 7] - m16[ 3]*m16[ 4]; 00053 const Real fA3 = m16[ 1]*m16[ 6] - m16[ 2]*m16[ 5]; 00054 const Real fA4 = m16[ 1]*m16[ 7] - m16[ 3]*m16[ 5]; 00055 const Real fA5 = m16[ 2]*m16[ 7] - m16[ 3]*m16[ 6]; 00056 const Real fB0 = m16[ 8]*m16[13] - m16[ 9]*m16[12]; 00057 const Real fB1 = m16[ 8]*m16[14] - m16[10]*m16[12]; 00058 const Real fB2 = m16[ 8]*m16[15] - m16[11]*m16[12]; 00059 const Real fB3 = m16[ 9]*m16[14] - m16[10]*m16[13]; 00060 const Real fB4 = m16[ 9]*m16[15] - m16[11]*m16[13]; 00061 const Real fB5 = m16[10]*m16[15] - m16[11]*m16[14]; 00062 00063 return fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0; 00064 } 00065 00066 template<typename Real> 00067 m4x4<Real>& m4x4<Real>::transpose(void){ 00068 register Real temp; 00069 temp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = temp; 00070 temp = m[0][2]; m[0][2] = m[2][0]; m[2][0] = temp; 00071 temp = m[0][3]; m[0][3] = m[3][0]; m[3][0] = temp; 00072 temp = m[1][2]; m[1][2] = m[2][1]; m[2][1] = temp; 00073 temp = m[1][3]; m[1][3] = m[3][1]; m[3][1] = temp; 00074 temp = m[2][3]; m[2][3] = m[3][2]; m[3][2] = temp; 00075 return *this; 00076 } 00077 00078 template<typename Real> 00079 m4x4<Real>& m4x4<Real>::invert(void){ 00080 return *this = inverse(*this); 00081 } 00082 00083 template<typename Real> 00084 m4x4<Real>& m4x4<Real>::asTranslation(const Real x, const Real y,const Real z){ 00085 m[0][0]=1.0f; m[0][1]=0.0f; m[0][2]=0.0f; m[0][3]=x; 00086 m[1][0]=0.0f; m[1][1]=1.0f; m[1][2]=0.0f; m[1][3]=y; 00087 m[2][0]=0.0f; m[2][1]=0.0f; m[2][2]=1.0f; m[2][3]=z; 00088 m[3][0]=0.0f; m[3][1]=0.0f; m[3][2]=0.0f; m[3][3]=1.0f; 00089 return *this; 00090 } 00091 00092 template<typename Real> 00093 m4x4<Real>& m4x4<Real>::asRotation(const Real x, const Real y, const Real z){ 00094 const Real sx = sin(x); 00095 const Real cx = cos(x); 00096 const Real sy = sin(y); 00097 const Real cy = cos(y); 00098 const Real sz = sin(z); 00099 const Real cz = cos(z); 00100 const Real sxsy = sx*sy; 00101 const Real cxsy = cx*sy; 00102 const Real cxcy = cx*cy; 00103 00104 m[0][0] = cy*cz; m[0][1] = cy*sz; m[0][2] = -sy; m[0][3] = 0.0f; 00105 m[1][0] = sxsy*cz-cx*sz; m[1][1] = sxsy*sz+cxcy; m[1][2] = sx*cy; m[1][3] = 0.0f; 00106 m[2][0] = cxsy*cz+sx*sz; m[2][1] = cxsy*sz-sx*cz; m[2][2] = cxcy; m[2][3] = 0.0f; 00107 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 00108 00109 return *this; 00110 } 00111 00112 template<typename Real> 00113 m4x4<Real>& m4x4<Real>::asRotationX(const Real angle){ 00114 const Real s = sin(angle); 00115 const Real c = cos(angle); 00116 m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f; 00117 m[1][0] = 0.0f; m[1][1] = c; m[1][2] = s; m[1][3] = 0.0f; 00118 m[2][0] = 0.0f; m[2][1] = -s; m[2][2] = c; m[2][3] = 0.0f; 00119 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 00120 return *this; 00121 } 00122 00123 template<typename Real> 00124 m4x4<Real>& m4x4<Real>::asRotationY(const Real angle){ 00125 const Real s = sin(angle); 00126 const Real c = cos(angle); 00127 m[0][0] = c; m[0][1] = 0.0f; m[0][2] = -s; m[0][3] = 0.0f; 00128 m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f; 00129 m[2][0] = s; m[2][1] = 0.0f; m[2][2] = c; m[2][3] = 0.0f; 00130 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 00131 return *this; 00132 } 00133 00134 template<typename Real> 00135 m4x4<Real>& m4x4<Real>::asRotationZ(const Real angle){ 00136 const Real s = sin(angle); 00137 const Real c = cos(angle); 00138 m[0][0] = c; m[0][1] = s; m[0][2] = 0.0f; m[0][3] = 0.0f; 00139 m[1][0] = -s; m[1][1] = c; m[1][2] = 0.0f; m[1][3] = 0.0f; 00140 m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f; 00141 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 00142 return *this; 00143 } 00144 00145 template<typename Real> 00146 m4x4<Real>& m4x4<Real>::asScale(const Real x, const Real y, const Real z){ 00147 m[0][0] = x; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f; 00148 m[1][0] = 0.0f; m[1][1] = y; m[1][2] = 0.0f; m[1][3] = 0.0f; 00149 m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = z; m[2][3] = 0.0f; 00150 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 00151 return *this; 00152 } 00153 00154 template<typename Real> 00155 m3x3<Real> m4x4<Real>::as3x3(void){ 00156 return m3x3<Real>( 00157 m[0][0], m[0][1], m[0][2], 00158 m[1][0], m[1][1], m[1][2], 00159 m[2][0], m[2][1], m[2][2]); 00160 } 00161 00162 template<typename Real> 00163 const m3x3<Real> m4x4<Real>::as3x3(void) const{ 00164 return m3x3<Real>( 00165 m[0][0], m[0][1], m[0][2], 00166 m[1][0], m[1][1], m[1][2], 00167 m[2][0], m[2][1], m[2][2]); 00168 } 00169 00170 template<typename Real> 00171 m4x4<Real>& m4x4<Real>::projection(const int width, const int height, const Real Near, const Real Far, const Real FOV){ 00172 const Real zero(0.0); 00173 const Real h = Real(1.0) / tan(FOV * (PI / Real(360.0))); 00174 const Real w = (h * height) / width; 00175 const Real dz = Near-Far; 00176 this->m[0][0] = w; this->m[0][1] = zero; this->m[0][2] = zero; this->m[0][3] = zero; 00177 this->m[1][0] = zero; this->m[1][1] = h; this->m[1][2] = zero; this->m[1][3] = zero; 00178 this->m[2][0] = zero; this->m[2][1] = zero; this->m[2][2] = (Far+Near)/dz; this->m[2][3] = (2*Far*Near)/dz; 00179 this->m[3][0] = zero; this->m[3][1] = zero; this->m[3][2] = -Real(1.0); this->m[3][3] = zero; 00180 return *this; 00181 } 00182 00183 template<typename Real> 00184 m4x4<Real>& m4x4<Real>::lookAt(const v3<Real>& camPos, const v3<Real>& camDirUnion, const v3<Real>& camUp){ 00185 const Real zero(0.0); 00186 const v3<Real> z = -camDirUnion; 00187 const v3<Real> x = normalize(cross(camUp, z)); 00188 const v3<Real> y = cross(z, x); 00189 this->m[0][0] = x[0]; this->m[0][1] = x[1]; this->m[0][2] = x[2]; this->m[0][3] = -(x*camPos); 00190 this->m[1][0] = y[0]; this->m[1][1] = y[1]; this->m[1][2] = y[2]; this->m[1][3] = -(y*camPos); 00191 this->m[2][0] = z[0]; this->m[2][1] = z[1]; this->m[2][2] = z[2]; this->m[2][3] = -(z*camPos); 00192 this->m[3][0] = zero; this->m[3][1] = zero; this->m[3][2] = zero; this->m[3][3] = Real(1.0); 00193 return *this; 00194 } 00195 00196 template<typename Real> 00197 m4x4<Real>& m4x4<Real>::lookAt(const Real eyex, const Real eyey, const Real eyez, 00198 const Real centerx, const Real centery, const Real centerz, 00199 const Real upx, const Real upy, const Real upz) 00200 { 00201 v3<Real> dir(eyex-centerx, eyey-centery, eyez-centerz); 00202 dir.normalize(); 00203 v3<Real> up(upx, upy, upz); 00204 up.normalize(); 00205 return lookAt(v3<Real>(eyex, eyey, eyez), dir, up); 00206 } 00207 00208 template<typename Real> 00209 m4x4<Real>& m4x4<Real>::operator = (const m4x4<Real>& a){ 00210 this->m[0][0]=a(0,0); this->m[0][1]=a(0,1); this->m[0][2]=a(0,2); this->m[0][3]=a(0,3); 00211 this->m[1][0]=a(1,0); this->m[1][1]=a(1,1); this->m[1][2]=a(1,2); this->m[1][3]=a(1,3); 00212 this->m[2][0]=a(2,0); this->m[2][1]=a(2,1); this->m[2][2]=a(2,2); this->m[2][3]=a(2,3); 00213 this->m[3][0]=a(3,0); this->m[3][1]=a(3,1); this->m[3][2]=a(3,2); this->m[3][3]=a(3,3); 00214 return *this; 00215 } 00216 00217 template<typename Real> 00218 m4x4<Real>& m4x4<Real>::operator += (const m4x4<Real>& a){ 00219 this->m[0][0]+=a(0,0); this->m[0][1]+=a(0,1); this->m[0][2]+=a(0,2); this->m[0][3]+=a(0,3); 00220 this->m[1][0]+=a(1,0); this->m[1][1]+=a(1,1); this->m[1][2]+=a(1,2); this->m[1][3]+=a(1,3); 00221 this->m[2][0]+=a(2,0); this->m[2][1]+=a(2,1); this->m[2][2]+=a(2,2); this->m[2][3]+=a(2,3); 00222 this->m[3][0]+=a(3,0); this->m[3][1]+=a(3,1); this->m[3][2]+=a(3,2); this->m[3][3]+=a(3,3); 00223 return *this; 00224 } 00225 00226 template<typename Real> 00227 m4x4<Real>& m4x4<Real>::operator -= (const m4x4<Real>& a){ 00228 this->m[0][0]-=a(0,0); this->m[0][1]-=a(0,1); this->m[0][2]-=a(0,2); this->m[0][3]-=a(0,3); 00229 this->m[1][0]-=a(1,0); this->m[1][1]-=a(1,1); this->m[1][2]-=a(1,2); this->m[1][3]-=a(1,3); 00230 this->m[2][0]-=a(2,0); this->m[2][1]-=a(2,1); this->m[2][2]-=a(2,2); this->m[2][3]-=a(2,3); 00231 this->m[3][0]-=a(3,0); this->m[3][1]-=a(3,1); this->m[3][2]-=a(3,2); this->m[3][3]-=a(3,3); 00232 return *this; 00233 } 00234 00235 template<typename Real> 00236 m4x4<Real>& m4x4<Real>::operator *= (const m4x4<Real>& a){ 00237 return *this = mul(*this, a); 00238 } 00239 00240 template<typename Real> 00241 bool m4x4<Real>::operator == (const m4x4<Real>& a){ 00242 for(char i=0; i<16; i++) 00243 if(m16[i] != a(i)) 00244 return false; 00245 return true; 00246 } 00247 00248 template<typename Real> 00249 bool m4x4<Real>::operator != (const m4x4<Real>& a){ 00250 for(char i=0; i<16; i++) 00251 if(m16[i] == a(i)) 00252 return false; 00253 return true; 00254 } 00255 00256 template<typename Real> 00257 v4<Real>& m4x4<Real>::operator [] (const int i){ 00258 assert(i<4); 00259 return &this->v[i]; 00260 } 00261 00262 template<typename Real> 00263 const v4<Real>& m4x4<Real>::operator [] (const int i) const{ 00264 assert(i<4); 00265 return &this->v[i]; 00266 } 00267 00268 template<typename Real> 00269 Real& m4x4<Real>::operator () (const int i){ 00270 assert(i<16); 00271 return this->m16[i]; 00272 } 00273 00274 template<typename Real> 00275 const Real m4x4<Real>::operator () (const int i) const{ 00276 assert(i<16); 00277 return this->m16[i]; 00278 } 00279 00280 template<typename Real> 00281 Real& m4x4<Real>::operator () (const int i, const int j){ 00282 assert(i<4); 00283 assert(j<4); 00284 return this->m[i][j]; 00285 } 00286 00287 template<typename Real> 00288 const Real m4x4<Real>::operator () (const int i, const int j) const{ 00289 assert(i<4); 00290 assert(j<4); 00291 return this->m[i][j]; 00292 } 00293 00294 template<typename Real> 00295 m4x4<Real> add(const m4x4<Real>& a, const m4x4<Real>& b){ 00296 return m4x4<Real>( 00297 a(0,0)+b(0,0), a(0,1)+b(0,1), a(0,2)+b(0,2), a(0,3)+b(0,3), 00298 a(1,0)+b(1,0), a(1,1)+b(1,1), a(1,2)+b(1,2), a(1,3)+b(1,3), 00299 a(2,0)+b(2,0), a(2,1)+b(2,1), a(2,2)+b(2,2), a(2,3)+b(2,3), 00300 a(3,0)+b(3,0), a(3,1)+b(3,1), a(3,2)+b(3,2), a(3,3)+b(3,3)); 00301 } 00302 00303 template<typename Real> 00304 m4x4<Real> sub(const m4x4<Real>& a, const m4x4<Real>& b){ 00305 return m4x4<Real>( 00306 a(0,0)-b(0,0), a(0,1)-b(0,1), a(0,2)-b(0,2), a(0,3)-b(0,3), 00307 a(1,0)-b(1,0), a(1,1)-b(1,1), a(1,2)-b(1,2), a(1,3)-b(1,3), 00308 a(2,0)-b(2,0), a(2,1)-b(2,1), a(2,2)-b(2,2), a(2,3)-b(2,3), 00309 a(3,0)-b(3,0), a(3,1)-b(3,1), a(3,2)-b(3,2), a(3,3)-b(3,3)); 00310 } 00311 00312 template<typename Real> 00313 m4x4<Real> mul(const m4x4<Real>& a, const m4x4<Real>& b){ 00314 return m4x4<Real>( 00315 a(0,0)*b(0,0) + a(0,1)*b(1,0) + a(0,2)*b(2,0) + a(0,3)*b(3,0), 00316 a(0,0)*b(0,1) + a(0,1)*b(1,1) + a(0,2)*b(2,1) + a(0,3)*b(3,1), 00317 a(0,0)*b(0,2) + a(0,1)*b(1,2) + a(0,2)*b(2,2) + a(0,3)*b(3,2), 00318 a(0,0)*b(0,3) + a(0,1)*b(1,3) + a(0,2)*b(2,3) + a(0,3)*b(3,3), 00319 00320 a(1,0)*b(0,0) + a(1,1)*b(1,0) + a(1,2)*b(2,0) + a(1,3)*b(3,0), 00321 a(1,0)*b(0,1) + a(1,1)*b(1,1) + a(1,2)*b(2,1) + a(1,3)*b(3,1), 00322 a(1,0)*b(0,2) + a(1,1)*b(1,2) + a(1,2)*b(2,2) + a(1,3)*b(3,2), 00323 a(1,0)*b(0,3) + a(1,1)*b(1,3) + a(1,2)*b(2,3) + a(1,3)*b(3,3), 00324 00325 a(2,0)*b(0,0) + a(2,1)*b(1,0) + a(2,2)*b(2,0) + a(2,3)*b(3,0), 00326 a(2,0)*b(0,1) + a(2,1)*b(1,1) + a(2,2)*b(2,1) + a(2,3)*b(3,1), 00327 a(2,0)*b(0,2) + a(2,1)*b(1,2) + a(2,2)*b(2,2) + a(2,3)*b(3,2), 00328 a(2,0)*b(0,3) + a(2,1)*b(1,3) + a(2,2)*b(2,3) + a(2,3)*b(3,3), 00329 00330 a(3,0)*b(0,0) + a(3,1)*b(1,0) + a(3,2)*b(2,0) + a(3,3)*b(3,0), 00331 a(3,0)*b(0,1) + a(3,1)*b(1,1) + a(3,2)*b(2,1) + a(3,3)*b(3,1), 00332 a(3,0)*b(0,2) + a(3,1)*b(1,2) + a(3,2)*b(2,2) + a(3,3)*b(3,2), 00333 a(3,0)*b(0,3) + a(3,1)*b(1,3) + a(3,2)*b(2,3) + a(3,3)*b(3,3)); 00334 } 00335 00336 template<typename Real> 00337 void mul(m4x4<Real>& out, const m4x4<Real>& a, const m4x4<Real>& b){ 00338 out(0,0) = a(0,0)*b(0,0) + a(0,1)*b(1,0) + a(0,2)*b(2,0) + a(0,3)*b(3,0); 00339 out(0,1) = a(0,0)*b(0,1) + a(0,1)*b(1,1) + a(0,2)*b(2,1) + a(0,3)*b(3,1); 00340 out(0,2) = a(0,0)*b(0,2) + a(0,1)*b(1,2) + a(0,2)*b(2,2) + a(0,3)*b(3,2); 00341 out(0,3) = a(0,0)*b(0,3) + a(0,1)*b(1,3) + a(0,2)*b(2,3) + a(0,3)*b(3,3); 00342 00343 out(1,0) = a(1,0)*b(0,0) + a(1,1)*b(1,0) + a(1,2)*b(2,0) + a(1,3)*b(3,0); 00344 out(1,1) = a(1,0)*b(0,1) + a(1,1)*b(1,1) + a(1,2)*b(2,1) + a(1,3)*b(3,1); 00345 out(1,2) = a(1,0)*b(0,2) + a(1,1)*b(1,2) + a(1,2)*b(2,2) + a(1,3)*b(3,2); 00346 out(1,3) = a(1,0)*b(0,3) + a(1,1)*b(1,3) + a(1,2)*b(2,3) + a(1,3)*b(3,3); 00347 00348 out(2,0) = a(2,0)*b(0,0) + a(2,1)*b(1,0) + a(2,2)*b(2,0) + a(2,3)*b(3,0); 00349 out(2,1) = a(2,0)*b(0,1) + a(2,1)*b(1,1) + a(2,2)*b(2,1) + a(2,3)*b(3,1); 00350 out(2,2) = a(2,0)*b(0,2) + a(2,1)*b(1,2) + a(2,2)*b(2,2) + a(2,3)*b(3,2); 00351 out(2,3) = a(2,0)*b(0,3) + a(2,1)*b(1,3) + a(2,2)*b(2,3) + a(2,3)*b(3,3); 00352 00353 out(3,0) = a(3,0)*b(0,0) + a(3,1)*b(1,0) + a(3,2)*b(2,0) + a(3,3)*b(3,0); 00354 out(3,1) = a(3,0)*b(0,1) + a(3,1)*b(1,1) + a(3,2)*b(2,1) + a(3,3)*b(3,1); 00355 out(3,2) = a(3,0)*b(0,2) + a(3,1)*b(1,2) + a(3,2)*b(2,2) + a(3,3)*b(3,2); 00356 out(3,3) = a(3,0)*b(0,3) + a(3,1)*b(1,3) + a(3,2)*b(2,3) + a(3,3)*b(3,3); 00357 } 00358 00359 template<typename Real> 00360 v4<Real> mul(const v4<Real>& v, const m4x4<Real>& m){ 00361 v4<Real> r; 00362 return mul(r, v, m), r; 00363 } 00364 00365 template<typename Real> 00366 void mul(v4<Real>& out, const v4<Real>& v, const m4x4<Real>& m){ 00367 out.x = v.x*m(0,0) + v.y*m(1,0) + v.z*m(2,0) + v.w*m(3,0); 00368 out.y = v.x*m(0,1) + v.y*m(1,1) + v.z*m(2,1) + v.w*m(3,1); 00369 out.z = v.x*m(0,2) + v.y*m(1,2) + v.z*m(2,2) + v.w*m(3,2); 00370 out.w = v.x*m(0,3) + v.y*m(1,3) + v.z*m(2,3) + v.w*m(3,3); 00371 } 00372 00373 template<typename Real> 00374 v4<Real> mul(const m4x4<Real>& m, const v4<Real>& v){ 00375 v4<Real> r; 00376 return mul(r, m, v), r; 00377 } 00378 00379 template<typename Real> 00380 void mul(v4<Real>& out, const m4x4<Real>& m, const v4<Real>& v){ 00381 out.x = v.x*m(0,0) + v.y*m(0,1) + v.z*m(0,2) + v.w*m(0,3); 00382 out.y = v.x*m(1,0) + v.y*m(1,1) + v.z*m(1,2) + v.w*m(1,3); 00383 out.z = v.x*m(2,0) + v.y*m(2,1) + v.z*m(2,2) + v.w*m(2,3); 00384 out.w = v.x*m(3,0) + v.y*m(3,1) + v.z*m(3,2) + v.w*m(3,3); 00385 } 00386 00387 template<typename Real> 00388 v4<Real> operator * (const v4<Real>& a, const m4x4<Real>& b){ 00389 return mul(a,b); 00390 } 00391 00392 template<typename Real> 00393 v4<Real> operator * (const m4x4<Real>& a, const v4<Real>& b){ 00394 return mul(a,b); 00395 } 00396 00397 template<typename Real> 00398 m4x4<Real> operator * (const m4x4<Real>& a, const m4x4<Real>& b){ 00399 return mul(a,b); 00400 } 00401 00402 template<typename Real> 00403 m4x4<Real> operator + (const m4x4<Real>& a, const m4x4<Real>& b){ 00404 return add(a,b); 00405 } 00406 00407 template<typename Real> 00408 m4x4<Real> operator - (const m4x4<Real>& a, const m4x4<Real>& b){ 00409 return sub(a,b); 00410 } 00411 00412 template<typename Real> 00413 m4x4<Real> Identity4x4(){ 00414 const Real one(1.0); 00415 const Real zero(0.0); 00416 return m4x4<Real>( 00417 one,zero,zero,zero, 00418 zero,one,zero,zero, 00419 zero,zero,one,zero, 00420 zero,zero,zero,one); 00421 } 00422 00423 template<typename Real> 00424 m4x4<Real> Zero4x4(){ 00425 const Real zero(0.0); 00426 return m4x4<Real>( 00427 zero,zero,zero,zero, 00428 zero,zero,zero,zero, 00429 zero,zero,zero,zero, 00430 zero,zero,zero,zero); 00431 } 00432 00433 template<typename Real> 00434 m4x4<Real> Translation(const Real x, const Real y, const Real z){ 00435 m4x4<Real> m; 00436 return m.asTranslation(x,y,z); 00437 } 00438 00439 template<typename Real> 00440 m4x4<Real> Scale(const Real x, const Real y, const Real z){ 00441 m4x4<Real> m; 00442 return m.asScale(x,y,z); 00443 } 00444 00445 template<typename Real> 00446 m4x4<Real> RotationX(const Real angle){ 00447 m4x4<Real> m; 00448 return m.asRotationX(angle); 00449 } 00450 00451 template<typename Real> 00452 m4x4<Real> RotationY(const Real angle){ 00453 m4x4<Real> m; 00454 return m.asRotationY(angle); 00455 } 00456 00457 template<typename Real> 00458 m4x4<Real> RotationZ(const Real angle){ 00459 m4x4<Real> m; 00460 return m.asRotationZ(angle); 00461 } 00462 00463 template<typename Real> 00464 m4x4<Real> inverse(const m4x4<Real> m){ 00465 const Real fA0 = m( 0)*m( 5) - m( 1)*m( 4); 00466 const Real fA1 = m( 0)*m( 6) - m( 2)*m( 4); 00467 const Real fA2 = m( 0)*m( 7) - m( 3)*m( 4); 00468 const Real fA3 = m( 1)*m( 6) - m( 2)*m( 5); 00469 const Real fA4 = m( 1)*m( 7) - m( 3)*m( 5); 00470 const Real fA5 = m( 2)*m( 7) - m( 3)*m( 6); 00471 const Real fB0 = m( 8)*m(13) - m( 9)*m(12); 00472 const Real fB1 = m( 8)*m(14) - m(10)*m(12); 00473 const Real fB2 = m( 8)*m(15) - m(11)*m(12); 00474 const Real fB3 = m( 9)*m(14) - m(10)*m(13); 00475 const Real fB4 = m( 9)*m(15) - m(11)*m(13); 00476 const Real fB5 = m(10)*m(15) - m(11)*m(14); 00477 00478 const Real det = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0; 00479 if(-Real(0.00001) < det && det < Real(0.00001)) 00480 return Identity4x4<Real>(); 00481 const Real invDet = 1.f/det; 00482 00483 return m4x4<Real>( 00484 (+ m( 5)*fB5 - m( 6)*fB4 + m( 7)*fB3)*invDet, 00485 (- m( 1)*fB5 + m( 2)*fB4 - m( 3)*fB3)*invDet, 00486 (+ m(13)*fA5 - m(14)*fA4 + m(15)*fA3)*invDet, 00487 (- m( 9)*fA5 + m(10)*fA4 - m(11)*fA3)*invDet, 00488 (- m( 4)*fB5 + m( 6)*fB2 - m( 7)*fB1)*invDet, 00489 (+ m( 0)*fB5 - m( 2)*fB2 + m( 3)*fB1)*invDet, 00490 (- m(12)*fA5 + m(14)*fA2 - m(15)*fA1)*invDet, 00491 (+ m( 8)*fA5 - m(10)*fA2 + m(11)*fA1)*invDet, 00492 (+ m( 4)*fB4 - m( 5)*fB2 + m( 7)*fB0)*invDet, 00493 (- m( 0)*fB4 + m( 1)*fB2 - m( 3)*fB0)*invDet, 00494 (+ m(12)*fA4 - m(13)*fA2 + m(15)*fA0)*invDet, 00495 (- m( 8)*fA4 + m( 9)*fA2 - m(11)*fA0)*invDet, 00496 (- m( 4)*fB3 + m( 5)*fB1 - m( 6)*fB0)*invDet, 00497 (+ m( 0)*fB3 - m( 1)*fB1 + m( 2)*fB0)*invDet, 00498 (- m(12)*fA3 + m(13)*fA1 - m(14)*fA0)*invDet, 00499 (+ m( 8)*fA3 - m( 9)*fA1 + m(10)*fA0)*invDet); 00500 } 00501 00502 template<typename Real> 00503 m4x4<Real> transpose(const m4x4<Real> m){ 00504 return m4x4<Real>(m(0,0),m(1,0),m(2,0),m(3,0), 00505 m(0,1),m(1,1),m(2,1),m(3,1), 00506 m(0,2),m(1,2),m(2,2),m(3,2), 00507 m(0,3),m(1,3),m(2,3),m(3,3)); 00508 } 00509 00510 template<typename Real> 00511 m4x4<Real> Projection(const int width, const int height, 00512 const Real Near, const Real Far, const Real FOV) 00513 { 00514 const Real h = 1.0f * tanf(FOV * (PI / 360.0f)); 00515 const Real w = (h * height) / width; 00516 const Real dz = Near - Far; 00517 00518 return m4x4<Real>( 00519 w, 0.0f, 0.0f, 0.0f, 00520 0.0f, h, 0.0f, 0.0f, 00521 0.0f, 0.0f, (Far+Near)/dz, (2*Far*Near)/dz, 00522 0.0f, 0.0f, -1.0f, 0.0f); 00523 } 00524 00525 template<typename Real> 00526 m4x4<Real> LookAt(const Real eyex, const Real eyey, const Real eyez, 00527 const Real centerx, const Real centery, const Real centerz, 00528 const Real upx, const Real upy, const Real upz) 00529 { 00530 v3<Real> z(eyex-centerx, eyey-centery, eyez-centerz); 00531 z.normalize(); 00532 v3<Real> x(upy*z[2] - upz*z[1], upz*z[0] - upx*z[2], upx*z[1] - upy*z[0]); 00533 x.normalize(); 00534 const v3<Real> y = cross(z, x); 00535 return m4x4<Real>( 00536 x[0], x[1], x[2], -x[0]*eyex - x[1]*eyey - x[2]*eyez, 00537 y[0], y[1], y[2], -y[0]*eyex - y[1]*eyey - y[2]*eyez, 00538 z[0], z[1], z[2], -z[0]*eyex - z[1]*eyey - z[2]*eyez, 00539 0.0f, 0.0f, 0.0f, 1.0f); 00540 } 00541 00542 template<typename Real> 00543 m4x4<Real> ProjectionStereo( 00544 const int screenWidth, 00545 const int screenHeight, 00546 const Real zn, 00547 const Real zf, 00548 const Real camFOV, 00549 const Real distToEyeCenter, 00550 const Real focalLength) 00551 { 00552 const Real ratio = screenWidth/float(screenHeight); 00553 const Real radians = (PI/180.f)*camFOV*.5f; 00554 const Real wd2 = zn * tan(radians); 00555 const Real ndfl = zn / focalLength; 00556 const Real l = -ratio * wd2 - distToEyeCenter * ndfl; 00557 const Real r = ratio * wd2 - distToEyeCenter * ndfl; 00558 const Real t = wd2; 00559 const Real b = -wd2; 00560 return m4x4<Real>( 00561 2*zn/(r-l), 0, (l+r)/(r-l), 0, 00562 0, 2*zn/(t-b), (t+b)/(t-b), 0, 00563 0, 0, zf/(zn-zf), zn*zf/(zn-zf), 00564 0, 0, -1, 0); 00565 } 00566 00567 template<typename Real> 00568 m4x4<Real> LookAtStereo( 00569 const v3<Real> camPos, 00570 const v3<Real> camLookAt, 00571 const v3<Real> camUp, 00572 const Real distToEyeCenter, 00573 const Real focalLength) 00574 { 00575 const v3<Real> dir(normalize(camLookAt-camPos)); 00576 const v3<Real> up(camUp); 00577 const v3<Real> r(normalize(cross(dir, up))*distToEyeCenter); 00578 const v3<Real> eye(camPos + r); 00579 const v3<Real> center(camPos + r + dir); 00580 const v3<Real> zaxis(normalize(eye-center)); 00581 const v3<Real> xaxis(normalize(cross(up, zaxis))); 00582 const v3<Real> yaxis(cross(zaxis, xaxis)); 00583 00584 return m4x4<Real>( 00585 xaxis.x, xaxis.y, xaxis.z, -dot(xaxis, eye), 00586 yaxis.x, yaxis.y, yaxis.z, -dot(yaxis, eye), 00587 zaxis.x, zaxis.y, zaxis.z, -dot(zaxis, eye), 00588 0.f, 0.f, 0.f, 1.f); 00589 }