panda/src/linmath/lmatrix4_src.cxx Source File

00001 // Filename: lmatrix4_src.cxx
00002 // Created by:  drose (15Jan99)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
00008 //
00009 // All use of this software is subject to the terms of the Panda 3d
00010 // Software license.  You should have received a copy of this license
00011 // along with this source code; you will also find a current copy of
00012 // the license at http://www.panda3d.org/license.txt .
00013 //
00014 // To contact the maintainers of this program write to
00015 // panda3d@yahoogroups.com .
00016 //
00017 ////////////////////////////////////////////////////////////////////
00018 
00019 TypeHandle FLOATNAME(LMatrix4)::_type_handle;
00020 
00021 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_ident_mat =
00022   FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
00023                       0.0f, 1.0f, 0.0f, 0.0f,
00024                       0.0f, 0.0f, 1.0f, 0.0f,
00025                       0.0f, 0.0f, 0.0f, 1.0f);
00026 
00027 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_y_to_z_up_mat =
00028   FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
00029                       0.0f, 0.0f, 1.0f, 0.0f,
00030                       0.0f,-1.0f, 0.0f, 0.0f,
00031                       0.0f, 0.0f, 0.0f, 1.0f);
00032 
00033 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_z_to_y_up_mat =
00034   FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
00035                       0.0f, 0.0f,-1.0f, 0.0f,
00036                       0.0f, 1.0f, 0.0f, 0.0f,
00037                       0.0f, 0.0f, 0.0f, 1.0f);
00038 
00039 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_flip_y_mat =
00040   FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
00041                       0.0f,-1.0f, 0.0f, 0.0f,
00042                       0.0f, 0.0f, 1.0f, 0.0f,
00043                       0.0f, 0.0f, 0.0f, 1.0f);
00044 
00045 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_flip_z_mat =
00046   FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
00047                       0.0f, 1.0f, 0.0f, 0.0f,
00048                       0.0f, 0.0f,-1.0f, 0.0f,
00049                       0.0f, 0.0f, 0.0f, 1.0f);
00050 
00051 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_lz_to_ry_mat =
00052   FLOATNAME(LMatrix4)::_flip_y_mat * FLOATNAME(LMatrix4)::_z_to_y_up_mat;
00053 
00054 const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_ly_to_rz_mat =
00055   FLOATNAME(LMatrix4)::_flip_z_mat * FLOATNAME(LMatrix4)::_y_to_z_up_mat;
00056 
00057 ////////////////////////////////////////////////////////////////////
00058 //     Function: LMatrix::convert_mat
00059 //       Access: Public, Static
00060 //  Description: Returns a matrix that transforms from the indicated
00061 //               coordinate system to the indicated coordinate system.
00062 ////////////////////////////////////////////////////////////////////
00063 const FLOATNAME(LMatrix4) &FLOATNAME(LMatrix4)::
00064 convert_mat(CoordinateSystem from, CoordinateSystem to) {
00065   if (from == CS_default) {
00066     from = default_coordinate_system;
00067   }
00068   if (to == CS_default) {
00069     to = default_coordinate_system;
00070   }
00071   switch (from) {
00072   case CS_zup_left:
00073     switch (to) {
00074     case CS_zup_left: return _ident_mat;
00075     case CS_yup_left: return _z_to_y_up_mat;
00076     case CS_zup_right: return _flip_y_mat;
00077     case CS_yup_right: return _lz_to_ry_mat;
00078     default: break;
00079     }
00080     break;
00081 
00082   case CS_yup_left:
00083     switch (to) {
00084     case CS_zup_left: return _y_to_z_up_mat;
00085     case CS_yup_left: return _ident_mat;
00086     case CS_zup_right: return _ly_to_rz_mat;
00087     case CS_yup_right: return _flip_z_mat;
00088     default: break;
00089     }
00090     break;
00091 
00092   case CS_zup_right:
00093     switch (to) {
00094     case CS_zup_left: return _flip_y_mat;
00095     case CS_yup_left: return _lz_to_ry_mat;
00096     case CS_zup_right: return _ident_mat;
00097     case CS_yup_right: return _z_to_y_up_mat;
00098     default: break;
00099     }
00100     break;
00101 
00102   case CS_yup_right:
00103     switch (to) {
00104     case CS_zup_left: return _ly_to_rz_mat;
00105     case CS_yup_left: return _flip_z_mat;
00106     case CS_zup_right: return _y_to_z_up_mat;
00107     case CS_yup_right: return _ident_mat;
00108     default: break;
00109     }
00110     break;
00111 
00112   default:
00113     break;
00114   }
00115 
00116   linmath_cat.error()
00117     << "Invalid coordinate system value!\n";
00118   return _ident_mat;
00119 }
00120 
00121 ////////////////////////////////////////////////////////////////////
00122 //     Function: LMatrix4::compare_to
00123 //       Access: Public
00124 //  Description: Sorts matrices lexicographically, componentwise.
00125 //               Returns a number less than 0 if this matrix sorts
00126 //               before the other one, greater than zero if it sorts
00127 //               after, 0 if they are equivalent (within the indicated
00128 //               tolerance).
00129 ////////////////////////////////////////////////////////////////////
00130 int FLOATNAME(LMatrix4)::
00131 compare_to(const FLOATNAME(LMatrix4) &other, FLOATTYPE threshold) const {
00132   // We compare values in reverse order, since the last row of the
00133   // matrix is most likely to be different between different matrices.
00134   for (int i = 15; i >= 0; i--) {
00135     if (!IS_THRESHOLD_COMPEQ(_m.data[i], other._m.data[i], threshold)) {
00136       return (_m.data[i] < other._m.data[i]) ? -1 : 1;
00137     }
00138   }
00139   return 0;
00140 }
00141 
00142 ////////////////////////////////////////////////////////////////////
00143 //     Function: LMatrix4::almost_equal
00144 //       Access: Public
00145 //  Description: Returns true if two matrices are memberwise equal
00146 //               within a specified tolerance.
00147 ////////////////////////////////////////////////////////////////////
00148 bool FLOATNAME(LMatrix4)::
00149 almost_equal(const FLOATNAME(LMatrix4) &other, FLOATTYPE threshold) const {
00150   return (IS_THRESHOLD_EQUAL((*this)(0, 0), other(0, 0), threshold) &&
00151           IS_THRESHOLD_EQUAL((*this)(0, 1), other(0, 1), threshold) &&
00152           IS_THRESHOLD_EQUAL((*this)(0, 2), other(0, 2), threshold) &&
00153           IS_THRESHOLD_EQUAL((*this)(0, 3), other(0, 3), threshold) &&
00154           IS_THRESHOLD_EQUAL((*this)(1, 0), other(1, 0), threshold) &&
00155           IS_THRESHOLD_EQUAL((*this)(1, 1), other(1, 1), threshold) &&
00156           IS_THRESHOLD_EQUAL((*this)(1, 2), other(1, 2), threshold) &&
00157           IS_THRESHOLD_EQUAL((*this)(1, 3), other(1, 3), threshold) &&
00158           IS_THRESHOLD_EQUAL((*this)(2, 0), other(2, 0), threshold) &&
00159           IS_THRESHOLD_EQUAL((*this)(2, 1), other(2, 1), threshold) &&
00160           IS_THRESHOLD_EQUAL((*this)(2, 2), other(2, 2), threshold) &&
00161           IS_THRESHOLD_EQUAL((*this)(2, 3), other(2, 3), threshold) &&
00162           IS_THRESHOLD_EQUAL((*this)(3, 0), other(3, 0), threshold) &&
00163           IS_THRESHOLD_EQUAL((*this)(3, 1), other(3, 1), threshold) &&
00164           IS_THRESHOLD_EQUAL((*this)(3, 2), other(3, 2), threshold) &&
00165           IS_THRESHOLD_EQUAL((*this)(3, 3), other(3, 3), threshold));
00166 }
00167 
00168 
00169 ////////////////////////////////////////////////////////////////////
00170 //     Function: LMatrix4::output
00171 //       Access: Public
00172 //  Description:
00173 ////////////////////////////////////////////////////////////////////
00174 void FLOATNAME(LMatrix4)::
00175 output(ostream &out) const {
00176   out << "[ "
00177       << MAYBE_ZERO(_m.m._00) << " "
00178       << MAYBE_ZERO(_m.m._01) << " "
00179       << MAYBE_ZERO(_m.m._02) << " "
00180       << MAYBE_ZERO(_m.m._03)
00181       << " ] [ "
00182       << MAYBE_ZERO(_m.m._10) << " "
00183       << MAYBE_ZERO(_m.m._11) << " "
00184       << MAYBE_ZERO(_m.m._12) << " "
00185       << MAYBE_ZERO(_m.m._13)
00186       << " ] [ "
00187       << MAYBE_ZERO(_m.m._20) << " "
00188       << MAYBE_ZERO(_m.m._21) << " "
00189       << MAYBE_ZERO(_m.m._22) << " "
00190       << MAYBE_ZERO(_m.m._23)
00191       << " ] [ "
00192       << MAYBE_ZERO(_m.m._30) << " "
00193       << MAYBE_ZERO(_m.m._31) << " "
00194       << MAYBE_ZERO(_m.m._32) << " "
00195       << MAYBE_ZERO(_m.m._33)
00196       << " ]";
00197 }
00198 
00199 ////////////////////////////////////////////////////////////////////
00200 //     Function: LMatrix4::write
00201 //       Access: Public
00202 //  Description:
00203 ////////////////////////////////////////////////////////////////////
00204 void FLOATNAME(LMatrix4)::
00205 write(ostream &out, int indent_level) const {
00206   indent(out, indent_level)
00207     << MAYBE_ZERO(_m.m._00) << " "
00208     << MAYBE_ZERO(_m.m._01) << " "
00209     << MAYBE_ZERO(_m.m._02) << " "
00210     << MAYBE_ZERO(_m.m._03)
00211     << "\n";
00212   indent(out, indent_level)
00213     << MAYBE_ZERO(_m.m._10) << " "
00214     << MAYBE_ZERO(_m.m._11) << " "
00215     << MAYBE_ZERO(_m.m._12) << " "
00216     << MAYBE_ZERO(_m.m._13)
00217     << "\n";
00218   indent(out, indent_level)
00219     << MAYBE_ZERO(_m.m._20) << " "
00220     << MAYBE_ZERO(_m.m._21) << " "
00221     << MAYBE_ZERO(_m.m._22) << " "
00222     << MAYBE_ZERO(_m.m._23)
00223     << "\n";
00224   indent(out, indent_level)
00225     << MAYBE_ZERO(_m.m._30) << " "
00226     << MAYBE_ZERO(_m.m._31) << " "
00227     << MAYBE_ZERO(_m.m._32) << " "
00228     << MAYBE_ZERO(_m.m._33)
00229     << "\n";
00230 }
00231 
00232 
00233 ////////////////////////////////////////////////////////////////////
00234 //     Function: LMatrix4::generate_hash
00235 //       Access: Public
00236 //  Description: Adds the vector to the indicated hash generator.
00237 ////////////////////////////////////////////////////////////////////
00238 void FLOATNAME(LMatrix4)::
00239 generate_hash(ChecksumHashGenerator &hashgen, FLOATTYPE threshold) const {
00240   for(int i = 0; i < 4; i++) {
00241     for(int j = 0; j < 4; j++) {
00242       hashgen.add_fp(get_cell(i,j), threshold);
00243     }
00244   }
00245 }
00246 
00247 ////////////////////////////////////////////////////////////////////
00248 //     Function: LMatrix4::decompose_mat
00249 //       Access: Private
00250 //  Description:
00251 ////////////////////////////////////////////////////////////////////
00252 bool FLOATNAME(LMatrix4)::
00253 decompose_mat(int index[4]) {
00254   int i, j, k;
00255   FLOATTYPE vv[4];
00256   for (i = 0; i < 4; i++) {
00257     FLOATTYPE big = 0.0f;
00258     for (j = 0; j < 4; j++) {
00259       FLOATTYPE temp = fabs((*this)(i,j));
00260       if (temp > big) {
00261         big = temp;
00262       }
00263     }
00264 
00265     // We throw the value out only if it's smaller than our "small"
00266     // threshold squared.  This helps reduce overly-sensitive
00267     // rejections.
00268     if (IS_THRESHOLD_ZERO(big, (NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)))) {
00269       //    if (IS_NEARLY_ZERO(big)) {
00270       return false;
00271     }
00272     vv[i] = 1.0f / big;
00273   }
00274 
00275   for (j = 0; j < 4; j++) {
00276     for (i = 0; i < j; i++) {
00277       FLOATTYPE sum = (*this)(i,j);
00278       for (k = 0; k < i; k++) {
00279         sum -= (*this)(i,k) * (*this)(k,j);
00280       }
00281       (*this)(i,j) = sum;
00282     }
00283 
00284     FLOATTYPE big = 0.0f;
00285     int imax = -1;
00286     for (i = j; i < 4; i++) {
00287       FLOATTYPE sum = (*this)(i,j);
00288       for (k = 0; k < j; k++) {
00289         sum -= (*this)(i,k) * (*this)(k,j);
00290       }
00291       (*this)(i,j) = sum;
00292 
00293       FLOATTYPE dum = vv[i] * fabs(sum);
00294       if (dum >= big) {
00295         big = dum;
00296         imax = i;
00297       }
00298     }
00299     nassertr(imax >= 0, false);
00300     if (j != imax) {
00301       for (k = 0; k < 4; k++) {
00302         FLOATTYPE dum = (*this)(imax,k);
00303         (*this)(imax,k) = (*this)(j,k);
00304         (*this)(j,k) = dum;
00305       }
00306       vv[imax] = vv[j];
00307     }
00308     index[j] = imax;
00309 
00310     if ((*this)(j,j) == 0.0f) {
00311       (*this)(j,j) = NEARLY_ZERO(FLOATTYPE);
00312     }
00313 
00314     if (j != 4 - 1) {
00315       FLOATTYPE dum = 1.0f / (*this)(j,j);
00316       for (i = j + 1; i < 4; i++) {
00317         (*this)(i,j) *= dum;
00318       }
00319     }
00320   }
00321   return true;
00322 }
00323 
00324 ////////////////////////////////////////////////////////////////////
00325 //     Function: LMatrix4::back_sub_mat
00326 //       Access: Private
00327 //  Description:
00328 ////////////////////////////////////////////////////////////////////
00329 bool FLOATNAME(LMatrix4)::
00330 back_sub_mat(int index[4], FLOATNAME(LMatrix4) &inv, int row) const {
00331   int ii = -1;
00332   int i, j;
00333   for (i = 0; i < 4; i++) {
00334     int ip = index[i];
00335     FLOATTYPE sum = inv(row, ip);
00336     inv(row, ip) = inv(row, i);
00337     if (ii >= 0) {
00338       for (j = ii; j <= i - 1; j++) {
00339         sum -= (*this)(i,j) * inv(row, j);
00340       }
00341     } else if (sum) {
00342       ii = i;
00343     }
00344 
00345     inv(row, i) = sum;
00346   }
00347 
00348   for (i = 4 - 1; i >= 0; i--) {
00349     FLOATTYPE sum = inv(row, i);
00350     for (j = i + 1; j < 4; j++) {
00351       sum -= (*this)(i,j) * inv(row, j);
00352     }
00353     inv(row, i) = sum / (*this)(i,i);
00354   }
00355 
00356   return true;
00357 }
00358 
00359 ////////////////////////////////////////////////////////////////////
00360 //     Function: LMatrix4::write_datagram
00361 //  Description: Writes the matrix to the datagram
00362 ////////////////////////////////////////////////////////////////////
00363 void FLOATNAME(LMatrix4)::
00364 write_datagram(Datagram &destination) const {
00365   for(int i = 0; i < 4; i++) {
00366     for(int j = 0; j < 4; j++) {
00367       destination.add_float32(get_cell(i,j));
00368     }
00369   }
00370 }
00371 
00372 
00373 ////////////////////////////////////////////////////////////////////
00374 //     Function: LMatrix4::read_datagram
00375 //  Description: Reads itself out of the datagram
00376 ////////////////////////////////////////////////////////////////////
00377 void FLOATNAME(LMatrix4)::
00378 read_datagram(DatagramIterator &scan) {
00379   for(int i = 0; i < 4; i++) {
00380     for(int j = 0; j < 4; j++) {
00381       set_cell(i, j, scan.get_float32());
00382     }
00383   }
00384 }
00385 
00386 ////////////////////////////////////////////////////////////////////
00387 //     Function: LMatrix4::init_type
00388 //       Access: Public, Static
00389 //  Description:
00390 ////////////////////////////////////////////////////////////////////
00391 void FLOATNAME(LMatrix4)::
00392 init_type() {
00393   if (_type_handle == TypeHandle::none()) {
00394     // Format a string to describe the type.
00395     string name = "LMatrix4";
00396     name += FLOATTOKEN;
00397     register_type(_type_handle, name);
00398   }
00399 }