panda/src/linmath/lmatrix3_src.cxx Source File

00001 // Filename: lmatrix3_src.cxx
00002 // Created by:  drose (29Jan99)
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(LMatrix3)::_type_handle;
00020 
00021 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_ident_mat =
00022   FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
00023                       0.0f, 1.0f, 0.0f,
00024                       0.0f, 0.0f, 1.0f);
00025 
00026 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_y_to_z_up_mat =
00027   FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
00028                       0.0f, 0.0f, 1.0f,
00029                       0.0f,-1.0f, 0.0f);
00030 
00031 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_z_to_y_up_mat =
00032   FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
00033                       0.0f, 0.0f,-1.0f,
00034                       0.0f, 1.0f, 0.0f);
00035 
00036 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_flip_y_mat =
00037   FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
00038                       0.0f,-1.0f, 0.0f,
00039                       0.0f, 0.0f, 1.0f);
00040 
00041 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_flip_z_mat =
00042   FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
00043                       0.0f, 1.0f, 0.0f,
00044                       0.0f, 0.0f,-1.0f);
00045 
00046 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_lz_to_ry_mat =
00047   FLOATNAME(LMatrix3)::_flip_y_mat * FLOATNAME(LMatrix3)::_z_to_y_up_mat;
00048 
00049 const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_ly_to_rz_mat =
00050   FLOATNAME(LMatrix3)::_flip_z_mat * FLOATNAME(LMatrix3)::_y_to_z_up_mat;
00051 
00052 ////////////////////////////////////////////////////////////////////
00053 //     Function: LMatrix::convert_mat
00054 //       Access: Public, Static
00055 //  Description: Returns a matrix that transforms from the indicated
00056 //               coordinate system to the indicated coordinate system.
00057 ////////////////////////////////////////////////////////////////////
00058 const FLOATNAME(LMatrix3) &FLOATNAME(LMatrix3)::
00059 convert_mat(CoordinateSystem from, CoordinateSystem to) {
00060   if (from == CS_default) {
00061     from = default_coordinate_system;
00062   }
00063   if (to == CS_default) {
00064     to = default_coordinate_system;
00065   }
00066   switch (from) {
00067   case CS_zup_left:
00068     switch (to) {
00069     case CS_zup_left: return _ident_mat;
00070     case CS_yup_left: return _z_to_y_up_mat;
00071     case CS_zup_right: return _flip_y_mat;
00072     case CS_yup_right: return _lz_to_ry_mat;
00073     default: break;
00074     }
00075     break;
00076 
00077   case CS_yup_left:
00078     switch (to) {
00079     case CS_zup_left: return _y_to_z_up_mat;
00080     case CS_yup_left: return _ident_mat;
00081     case CS_zup_right: return _ly_to_rz_mat;
00082     case CS_yup_right: return _flip_z_mat;
00083     default: break;
00084     }
00085     break;
00086 
00087   case CS_zup_right:
00088     switch (to) {
00089     case CS_zup_left: return _flip_y_mat;
00090     case CS_yup_left: return _lz_to_ry_mat;
00091     case CS_zup_right: return _ident_mat;
00092     case CS_yup_right: return _z_to_y_up_mat;
00093     default: break;
00094     }
00095     break;
00096 
00097   case CS_yup_right:
00098     switch (to) {
00099     case CS_zup_left: return _ly_to_rz_mat;
00100     case CS_yup_left: return _flip_z_mat;
00101     case CS_zup_right: return _y_to_z_up_mat;
00102     case CS_yup_right: return _ident_mat;
00103     default: break;
00104     }
00105     break;
00106 
00107   default:
00108     break;
00109   }
00110 
00111   linmath_cat.error()
00112     << "Invalid coordinate system value!\n";
00113   return _ident_mat;
00114 }
00115 
00116 ////////////////////////////////////////////////////////////////////
00117 //     Function: LMatrix3::fill
00118 //       Access: Public
00119 //  Description: Sets each element of the matrix to the indicated
00120 //               fill_value.  This is of questionable value, but is
00121 //               sometimes useful when initializing to zero.
00122 ////////////////////////////////////////////////////////////////////
00123 void FLOATNAME(LMatrix3)::
00124 fill(FLOATTYPE fill_value) {
00125   set(fill_value, fill_value, fill_value,
00126       fill_value, fill_value, fill_value,
00127       fill_value, fill_value, fill_value);
00128 }
00129 
00130 ////////////////////////////////////////////////////////////////////
00131 //     Function: LMatrix3::compare_to
00132 //       Access: Public
00133 //  Description: Sorts matrices lexicographically, componentwise.
00134 //               Returns a number less than 0 if this matrix sorts
00135 //               before the other one, greater than zero if it sorts
00136 //               after, 0 if they are equivalent (within the indicated
00137 //               tolerance).
00138 ////////////////////////////////////////////////////////////////////
00139 int FLOATNAME(LMatrix3)::
00140 compare_to(const FLOATNAME(LMatrix3) &other, FLOATTYPE threshold) const {
00141   for (int i = 0; i < 9; i++) {
00142     if (!IS_THRESHOLD_COMPEQ(_m.data[i], other._m.data[i], threshold)) {
00143       return (_m.data[i] < other._m.data[i]) ? -1 : 1;
00144     }
00145   }
00146   return 0;
00147 }
00148 
00149 ////////////////////////////////////////////////////////////////////
00150 //     Function: LMatrix3::almost_equal
00151 //       Access: Public
00152 //  Description: Returns true if two matrices are memberwise equal
00153 //               within a specified tolerance.
00154 ////////////////////////////////////////////////////////////////////
00155 bool FLOATNAME(LMatrix3)::
00156 almost_equal(const FLOATNAME(LMatrix3) &other, FLOATTYPE threshold) const {
00157   return (IS_THRESHOLD_EQUAL((*this)(0, 0), other(0, 0), threshold) &&
00158           IS_THRESHOLD_EQUAL((*this)(0, 1), other(0, 1), threshold) &&
00159           IS_THRESHOLD_EQUAL((*this)(0, 2), other(0, 2), threshold) &&
00160           IS_THRESHOLD_EQUAL((*this)(1, 0), other(1, 0), threshold) &&
00161           IS_THRESHOLD_EQUAL((*this)(1, 1), other(1, 1), threshold) &&
00162           IS_THRESHOLD_EQUAL((*this)(1, 2), other(1, 2), threshold) &&
00163           IS_THRESHOLD_EQUAL((*this)(2, 0), other(2, 0), threshold) &&
00164           IS_THRESHOLD_EQUAL((*this)(2, 1), other(2, 1), threshold) &&
00165           IS_THRESHOLD_EQUAL((*this)(2, 2), other(2, 2), threshold));
00166 }
00167 
00168 
00169 ////////////////////////////////////////////////////////////////////
00170 //     Function: LMatrix3::output
00171 //       Access: Public
00172 //  Description:
00173 ////////////////////////////////////////////////////////////////////
00174 void FLOATNAME(LMatrix3)::
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       << " ] [ "
00181       << MAYBE_ZERO(_m.m._10) << " "
00182       << MAYBE_ZERO(_m.m._11) << " "
00183       << MAYBE_ZERO(_m.m._12)
00184       << " ] [ "
00185       << MAYBE_ZERO(_m.m._20) << " "
00186       << MAYBE_ZERO(_m.m._21) << " "
00187       << MAYBE_ZERO(_m.m._22)
00188       << " ]";
00189 }
00190 
00191 ////////////////////////////////////////////////////////////////////
00192 //     Function: LMatrix3::write
00193 //       Access: Public
00194 //  Description:
00195 ////////////////////////////////////////////////////////////////////
00196 void FLOATNAME(LMatrix3)::
00197 write(ostream &out, int indent_level) const {
00198   indent(out, indent_level)
00199     << MAYBE_ZERO(_m.m._00) << " "
00200     << MAYBE_ZERO(_m.m._01) << " "
00201     << MAYBE_ZERO(_m.m._02)
00202     << "\n";
00203   indent(out, indent_level)
00204     << MAYBE_ZERO(_m.m._10) << " "
00205     << MAYBE_ZERO(_m.m._11) << " "
00206     << MAYBE_ZERO(_m.m._12)
00207     << "\n";
00208   indent(out, indent_level)
00209     << MAYBE_ZERO(_m.m._20) << " "
00210     << MAYBE_ZERO(_m.m._21) << " "
00211     << MAYBE_ZERO(_m.m._22)
00212     << "\n";
00213 }
00214 
00215 ////////////////////////////////////////////////////////////////////
00216 //     Function: LMatrix3::generate_hash
00217 //       Access: Public
00218 //  Description: Adds the vector to the indicated hash generator.
00219 ////////////////////////////////////////////////////////////////////
00220 void FLOATNAME(LMatrix3)::
00221 generate_hash(ChecksumHashGenerator &hashgen, FLOATTYPE threshold) const {
00222   for(int i = 0; i < 3; i++) {
00223     for(int j = 0; j < 3; j++) {
00224       hashgen.add_fp(get_cell(i,j), threshold);
00225     }
00226   }
00227 }
00228 
00229 ////////////////////////////////////////////////////////////////////
00230 //     Function: LMatrix3::write_datagram
00231 //  Description: Writes the matrix to the datagram
00232 ////////////////////////////////////////////////////////////////////
00233 void FLOATNAME(LMatrix3)::
00234 write_datagram(Datagram &destination) const {
00235   for(int i = 0; i < 3; i++) {
00236     for(int j = 0; j < 3; j++) {
00237       destination.add_float32(get_cell(i,j));
00238     }
00239   }
00240 }
00241 
00242 ////////////////////////////////////////////////////////////////////
00243 //     Function: LMatrix3::read_datagram
00244 //  Description: Reads itself out of the datagram
00245 ////////////////////////////////////////////////////////////////////
00246 void FLOATNAME(LMatrix3)::
00247 read_datagram(DatagramIterator &scan) {
00248   for(int i = 0; i < 3; i++) {
00249     for(int j = 0; j < 3; j++) {
00250       set_cell(i, j, scan.get_float32());
00251     }
00252   }
00253 }
00254 
00255 
00256 ////////////////////////////////////////////////////////////////////
00257 //     Function: LMatrix3::init_type
00258 //       Access: Public, Static
00259 //  Description:
00260 ////////////////////////////////////////////////////////////////////
00261 void FLOATNAME(LMatrix3)::
00262 init_type() {
00263   if (_type_handle == TypeHandle::none()) {
00264     // Format a string to describe the type.
00265     string name = "LMatrix3";
00266     name += FLOATTOKEN;
00267     register_type(_type_handle, name);
00268   }
00269 }