00001 #include "SFMatrix34f.h" 00002 00003 #include <iostream> 00004 00005 using namespace X3DTK; 00006 using namespace std; 00007 00008 SFMatrix34f::SFMatrix34f() {} 00009 00010 SFMatrix34f::SFMatrix34f(float _11, float _21, float _31, float _12, float _22, float _32, float _13, float _23, float _33, float _14, float _24, float _34) 00011 { 00012 this->_11 = _11; 00013 this->_21 = _21; 00014 this->_31 = _31; 00015 00016 this->_12 = _12; 00017 this->_22 = _22; 00018 this->_32 = _32; 00019 00020 this->_13 = _13; 00021 this->_23 = _23; 00022 this->_33 = _33; 00023 00024 this->_14 = _14; 00025 this->_24 = _24; 00026 this->_34 = _34; 00027 } 00028 00029 SFMatrix34f::SFMatrix34f(const float GLMatrix[16]) 00030 { 00031 _11 = GLMatrix[0]; 00032 _21 = GLMatrix[1]; 00033 _31 = GLMatrix[2]; 00034 00035 _12 = GLMatrix[4]; 00036 _22 = GLMatrix[5]; 00037 _32 = GLMatrix[6]; 00038 00039 _13 = GLMatrix[8]; 00040 _23 = GLMatrix[9]; 00041 _33 = GLMatrix[10]; 00042 00043 _14 = GLMatrix[12]; 00044 _24 = GLMatrix[13]; 00045 _34 = GLMatrix[14]; 00046 } 00047 00048 00049 SFMatrix34f::SFMatrix34f(const SFVec3f &vx, const SFVec3f &vy, const SFVec3f &vz, const SFPoint3f &p) 00050 { 00051 _11 = vx.x; 00052 _21 = vx.y; 00053 _31 = vx.z; 00054 00055 _12 = vy.x; 00056 _22 = vy.y; 00057 _32 = vy.z; 00058 00059 _13 = vz.x; 00060 _23 = vz.y; 00061 _33 = vz.z; 00062 00063 _14 = p.x; 00064 _24 = p.y; 00065 _34 = p.z; 00066 } 00067 00068 SFMatrix34f::SFMatrix34f(const SFMatrix34f &m) 00069 { 00070 _11 = m._11; 00071 _21 = m._21; 00072 _31 = m._31; 00073 00074 _12 = m._12; 00075 _22 = m._22; 00076 _32 = m._32; 00077 00078 _13 = m._13; 00079 _23 = m._23; 00080 _33 = m._33; 00081 00082 _14 = m._14; 00083 _24 = m._24; 00084 _34 = m._34; 00085 } 00086 00087 SFMatrix34f X3DTK::operator+ (const SFMatrix34f &m1, const SFMatrix34f &m2) 00088 { 00089 SFMatrix34f res; 00090 00091 res._11 = m1._11 + m2._11; 00092 res._21 = m1._21 + m2._21; 00093 res._31 = m1._31 + m2._31; 00094 00095 res._12 = m1._12 + m2._12; 00096 res._22 = m1._22 + m2._22; 00097 res._32 = m1._32 + m2._32; 00098 00099 res._13 = m1._13 + m2._13; 00100 res._23 = m1._23 + m2._23; 00101 res._33 = m1._33 + m2._33; 00102 00103 res._14 = m1._14 + m2._14; 00104 res._24 = m1._24 + m2._24; 00105 res._34 = m1._34 + m2._34; 00106 00107 return res; 00108 } 00109 00110 SFMatrix34f X3DTK::operator- (const SFMatrix34f &m1, const SFMatrix34f &m2) 00111 { 00112 SFMatrix34f res; 00113 00114 res._11 = m1._11 - m2._11; 00115 res._21 = m1._21 - m2._21; 00116 res._31 = m1._31 - m2._31; 00117 00118 res._12 = m1._12 - m2._12; 00119 res._22 = m1._22 - m2._22; 00120 res._32 = m1._32 - m2._32; 00121 00122 res._13 = m1._13 - m2._13; 00123 res._23 = m1._23 - m2._23; 00124 res._33 = m1._33 - m2._33; 00125 00126 res._14 = m1._14 - m2._14; 00127 res._24 = m1._24 - m2._24; 00128 res._34 = m1._34 - m2._34; 00129 00130 return res; 00131 } 00132 00133 SFMatrix34f X3DTK::operator* (const SFMatrix34f &m1, const SFMatrix34f &m2) 00134 { 00135 SFMatrix34f res; 00136 00137 res._11 = m1._11 * m2._11 + m1._12 * m2._21 + m1._13 * m2._31; 00138 res._21 = m1._21 * m2._11 + m1._22 * m2._21 + m1._23 * m2._31; 00139 res._31 = m1._31 * m2._11 + m1._32 * m2._21 + m1._33 * m2._31; 00140 00141 res._12 = m1._11 * m2._12 + m1._12 * m2._22 + m1._13 * m2._32; 00142 res._22 = m1._21 * m2._12 + m1._22 * m2._22 + m1._23 * m2._32; 00143 res._32 = m1._31 * m2._12 + m1._32 * m2._22 + m1._33 * m2._32; 00144 00145 res._13 = m1._11 * m2._13 + m1._12 * m2._23 + m1._13 * m2._33; 00146 res._23 = m1._21 * m2._13 + m1._22 * m2._23 + m1._23 * m2._33; 00147 res._33 = m1._31 * m2._13 + m1._32 * m2._23 + m1._33 * m2._33; 00148 00149 res._14 = m1._11 * m2._14 + m1._12 * m2._24 + m1._13 * m2._34 + m1._14; 00150 res._24 = m1._21 * m2._14 + m1._22 * m2._24 + m1._23 * m2._34 + m1._24; 00151 res._34 = m1._31 * m2._14 + m1._32 * m2._24 + m1._33 * m2._34 + m1._34; 00152 00153 return res; 00154 } 00155 00156 SFMatrix34f X3DTK::operator* (const float a, const SFMatrix34f &m) 00157 { 00158 SFMatrix34f res; 00159 00160 res._11 = a * m._11; 00161 res._21 = a * m._21; 00162 res._31 = a * m._31; 00163 00164 res._12 = a * m._12; 00165 res._22 = a * m._22; 00166 res._32 = a * m._32; 00167 00168 res._13 = a * m._13; 00169 res._23 = a * m._23; 00170 res._33 = a * m._33; 00171 00172 res._14 = a * m._14; 00173 res._24 = a * m._24; 00174 res._34 = a * m._34; 00175 00176 return res; 00177 } 00178 00179 SFVec3f X3DTK::operator* (const SFMatrix34f &m, const SFVec3f &v) 00180 { 00181 SFVec3f res; 00182 00183 res.x = m._11 * v.x + m._12 * v.y + m._13 * v.z; 00184 res.y = m._21 * v.x + m._22 * v.y + m._23 * v.z; 00185 res.z = m._31 * v.x + m._32 * v.y + m._33 * v.z; 00186 00187 return res; 00188 } 00189 00190 SFPoint3f X3DTK::operator* (const SFMatrix34f &m, const SFPoint3f &v) 00191 { 00192 SFPoint3f res; 00193 00194 res.x = m._11 * v.x + m._12 * v.y + m._13 * v.z + m._14; 00195 res.y = m._21 * v.x + m._22 * v.y + m._23 * v.z + m._24; 00196 res.z = m._31 * v.x + m._32 * v.y + m._33 * v.z + m._34; 00197 00198 return res; 00199 } 00200 00201 SFMatrix34f SFMatrix34f :: i() 00202 { 00203 SFMatrix34f res; 00204 float d; 00205 00206 d = _11 * (_22 * _33 - _32 * _23) - _21 * (_12 * _33 - _32 * _13) + _31 * (_12 * _23 - _22 * _13); 00207 00208 res._11 = (_22 * _33 - _32 * _23)/d; 00209 res._12 = -(_12 * _33 - _32 * _13)/d; 00210 res._13 = (_12 * _23 - _22 * _13)/d; 00211 00212 res._21 = -(_21 * _33 - _31 * _23)/d; 00213 res._22 = (_11 * _33 - _31 * _13)/d; 00214 res._23 = -(_11 * _23 - _21 * _13)/d; 00215 00216 res._31 = (_21 * _32 - _31 * _22)/d; 00217 res._32 = -(_11 * _32 - _31 * _12)/d; 00218 res._33 = (_11 * _22 - _21 * _12)/d; 00219 00220 res._14 = -res._11 * _14 - res._12 * _24 - res._13 * _34; 00221 res._24 = -res._21 * _14 - res._22 * _24 - res._23 * _34; 00222 res._34 = -res._31 * _14 - res._32 * _24 - res._33 * _34; 00223 00224 return res; 00225 } 00226 00227 SFMatrix34f SFMatrix34f::iu() 00228 { 00229 SFMatrix34f res; 00230 00231 res._11 = _11; 00232 res._12 = _21; 00233 res._13 = _31; 00234 00235 res._21 = _12; 00236 res._22 = _22; 00237 res._23 = _32; 00238 00239 res._31 = _13; 00240 res._32 = _23; 00241 res._33 = _33; 00242 00243 res._14 = -_11*_14 - _21*_24 - _31*_34; 00244 res._24 = -_12*_14 - _22*_24 - _32*_34; 00245 res._34 = -_13*_14 - _23*_24 - _33*_34; 00246 00247 return res; 00248 } 00249 00250 void SFMatrix34f::toFloat16(std::vector<float> &M) const 00251 { 00252 M.resize(16); 00253 M[0] = _11; 00254 M[1] = _21; 00255 M[2] = _31; 00256 M[3] = 0.0f; 00257 00258 M[4] = _12; 00259 M[5] = _22; 00260 M[6] = _32; 00261 M[7] = 0.0f; 00262 00263 M[8] = _13; 00264 M[9] = _23; 00265 M[10] = _33; 00266 M[11] = 0.0f; 00267 00268 M[12] = _14; 00269 M[13] = _24; 00270 M[14] = _34; 00271 M[15] = 1.0f; 00272 } 00273 00274 SFMatrix34f X3DTK::identity34() 00275 { 00276 SFMatrix34f res; 00277 00278 res._11 = 1.0; 00279 res._21 = 0.0; 00280 res._31 = 0.0; 00281 00282 res._12 = 0.0; 00283 res._22 = 1.0; 00284 res._32 = 0.0; 00285 00286 res._13 = 0.0; 00287 res._23 = 0.0; 00288 res._33 = 1.0; 00289 00290 res._14 = 0.0; 00291 res._24 = 0.0; 00292 res._34 = 0.0; 00293 00294 return res; 00295 } 00296 00297 SFMatrix34f X3DTK::scale34(float sx, float sy, float sz) 00298 { 00299 SFMatrix34f res; 00300 00301 res._11 = sx; 00302 res._21 = 0.0; 00303 res._31 = 0.0; 00304 00305 res._12 = 0.0; 00306 res._22 = sy; 00307 res._32 = 0.0; 00308 00309 res._13 = 0.0; 00310 res._23 = 0.0; 00311 res._33 = sz; 00312 00313 res._14 = 0.0; 00314 res._24 = 0.0; 00315 res._34 = 0.0; 00316 00317 return res; 00318 } 00319 00320 SFMatrix34f X3DTK::rotationX(float angle) 00321 { 00322 SFMatrix34f res; 00323 00324 res._11 = 1.0; 00325 res._21 = 0.0; 00326 res._31 = 0.0; 00327 00328 res._12 = 0.0; 00329 res._22 = cosf(angle); 00330 res._32 = sinf(angle); 00331 00332 res._13 = 0.0; 00333 res._23 = -sinf(angle); 00334 res._33 = cosf(angle); 00335 00336 res._14 = 0.0; 00337 res._24 = 0.0; 00338 res._34 = 0.0; 00339 00340 return res; 00341 } 00342 00343 SFMatrix34f X3DTK::rotationY(float angle) 00344 { 00345 SFMatrix34f res; 00346 00347 res._11 = cosf(angle); 00348 res._21 = 0.0; 00349 res._31 = -sinf(angle); 00350 00351 res._12 = 0.0; 00352 res._22 = 1.0; 00353 res._32 = 0.0; 00354 00355 res._13 = sinf(angle); 00356 res._23 = 0.0; 00357 res._33 = cosf(angle); 00358 00359 res._14 = 0.0; 00360 res._24 = 0.0; 00361 res._34 = 0.0; 00362 00363 return res; 00364 } 00365 00366 SFMatrix34f X3DTK::rotationZ(float angle) 00367 { 00368 SFMatrix34f res; 00369 00370 res._11 = cosf(angle); 00371 res._21 = -sinf(angle); 00372 res._31 = 0.0f; 00373 00374 res._12 = sinf(angle); 00375 res._22 = cosf(angle); 00376 res._32 = 0.0; 00377 00378 res._13 = 0.0f; 00379 res._23 = 0.0; 00380 res._33 = 1.0f; 00381 00382 res._14 = 0.0; 00383 res._24 = 0.0; 00384 res._34 = 0.0; 00385 00386 return res; 00387 } 00388 00389 SFMatrix34f X3DTK::translation(const SFVec3f &t) 00390 { 00391 return SFMatrix34f(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, t.x, t.y, t.z); 00392 } 00393 00394 SFMatrix34f X3DTK::rotation(float angle, const SFVec3f &v) 00395 { 00396 SFVec3f u(v.normalized()); 00397 SFMatrix34f S(0.0f, u.z, -u.y, -u.z, 0.0f, u.x, u.y, -u.x, 0.0f, 0.0f, 0.0f, 0.0f); 00398 SFMatrix34f uTu(u.x*u.x, u.y*u.x, u.z*u.x, u.x*u.y, u.y*u.y, u.z*u.y, u.x*u.z, u.y*u.z, u.z*u.z, 0.0f, 0.0f, 0.0f); 00399 00400 return (uTu + cosf(angle)*(identity34() - uTu) + sinf(angle)*S); 00401 } 00402 00403 std::ostream& X3DTK::operator<<(std::ostream& o, const SFMatrix34f &M) 00404 { 00405 return o << "| " << M._11 << " " << M._12 << " " << M._13 << " " << M._14 << " |" << endl 00406 << "| " << M._21 << " " << M._22 << " " << M._23 << " " << M._24 << " |" << endl 00407 << "| " << M._31 << " " << M._32 << " " << M._33 << " " << M._34 << " |" << endl; 00408 }