txform.cc

Go to the documentation of this file.
00001 
00003 
00004 //
00005 // Copyright (c) 2002-2004 California Institute of Technology
00006 // Copyright (c) 2004-2007 University of Southern California
00007 // Rob Peters <rjpeters at usc dot edu>
00008 //
00009 // created: Fri Jun 21 14:00:54 2002
00010 // commit: $Id: txform.cc 10065 2007-04-12 05:54:56Z rjpeters $
00011 // $HeadURL: file:///lab/rjpeters/svnrepo/code/trunk/groovx/src/geom/txform.cc $
00012 //
00013 // --------------------------------------------------------------------
00014 //
00015 // This file is part of GroovX.
00016 //   [http://ilab.usc.edu/rjpeters/groovx/]
00017 //
00018 // GroovX is free software; you can redistribute it and/or modify it
00019 // under the terms of the GNU General Public License as published by
00020 // the Free Software Foundation; either version 2 of the License, or
00021 // (at your option) any later version.
00022 //
00023 // GroovX is distributed in the hope that it will be useful, but
00024 // WITHOUT ANY WARRANTY; without even the implied warranty of
00025 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00026 // General Public License for more details.
00027 //
00028 // You should have received a copy of the GNU General Public License
00029 // along with GroovX; if not, write to the Free Software Foundation,
00030 // Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
00031 //
00033 
00034 #ifndef GROOVX_GEOM_TXFORM_CC_UTC20050626084023_DEFINED
00035 #define GROOVX_GEOM_TXFORM_CC_UTC20050626084023_DEFINED
00036 
00037 #include "txform.h"
00038 
00039 #include "geom/rect.h"
00040 #include "geom/vec3.h"
00041 
00042 #include "rutz/rand.h"
00043 
00044 #include <cmath>
00045 #include <cstring>
00046 
00047 #define GVX_NO_PROF
00048 #include "rutz/trace.h"
00049 #include "rutz/debug.h"
00050 GVX_DBG_REGISTER
00051 
00052 using geom::vec2d;
00053 using geom::vec3d;
00054 
00055 namespace
00056 {
00057   void mul_mtx_4x4(const double* m1, const double* m2, double* result)
00058   {
00059     // Note we're using column-major order here
00060     for (int result_col = 0; result_col < 4; ++result_col)
00061       for (int result_row = 0; result_row < 4; ++result_row)
00062         {
00063           result[result_col * 4 + result_row] =
00064               m1[0 * 4 + result_row] * m2[result_col * 4 + 0]
00065             + m1[1 * 4 + result_row] * m2[result_col * 4 + 1]
00066             + m1[2 * 4 + result_row] * m2[result_col * 4 + 2]
00067             + m1[3 * 4 + result_row] * m2[result_col * 4 + 3];
00068         }
00069   }
00070 }
00071 
00072 geom::txform::txform(const vec3d& translation,
00073                      const vec3d& scaling,
00074                      const vec3d& rotation_axis,
00075                      double rotation_angle)
00076 {
00077 GVX_TRACE("geom::txform::txform(tx, scl, rot)");
00078 
00079   // The matrices look transposed because OpenGL expects column-major
00080 
00081   const double sx = scaling.x();
00082   const double sy = scaling.y();
00083   const double sz = scaling.z();
00084 
00085   const double tx = translation.x();
00086   const double ty = translation.y();
00087   const double tz = translation.z();
00088 
00089   // The result of (1) translation, then (2) scaling
00090   /*
00091     const double ts[16] =
00092     {
00093     sx,   0.0, 0.0, 0.0,
00094     0.0,  sy,  0.0, 0.0,
00095     0.0,  0.0, sz,  0.0,
00096     tx,   ty,  tz,  1.0
00097     };
00098   */
00099 
00100   const double rl = rotation_axis.length();
00101 
00102   const double rx = rotation_axis.x() / rl;
00103   const double ry = rotation_axis.y() / rl;
00104   const double rz = rotation_axis.z() / rl;
00105 
00106   const double c = cos((rotation_angle) * M_PI / 180.0);
00107   const double s = sin((rotation_angle) * M_PI / 180.0);
00108 
00109   // The result of rotation
00110   /*
00111     const double r[16] =
00112     {
00113     rx*rx*(1-c)+c,    ry*rx*(1-c)+rz*s, rx*rz*(1-c)-ry*s, 0.0,
00114     rx*ry*(1-c)-rz*s, ry*ry*(1-c)+c,    ry*rz*(1-c)+rx*s, 0.0,
00115     rx*rz*(1-c)+ry*s, ry*rz*(1-c)-rx*s, rz*rz*(1-c)+c,    0.0,
00116     0.0,              0.0,              0.0,              1.0
00117     };
00118   */
00119 
00120   // Compute result = t * s * r = ts * r;
00121   /* mul_mtx_4x4(ts, r, result); */
00122 
00123   m_mtx[0]=sx*(rx*rx*(1-c)+c);    m_mtx[4]=sx*(rx*ry*(1-c)-rz*s); m_mtx[8]=sx*(rx*rz*(1-c)+ry*s); m_mtx[12]=tx;
00124   m_mtx[1]=sy*(ry*rx*(1-c)+rz*s); m_mtx[5]=sy*(ry*ry*(1-c)+c);    m_mtx[9]=sy*(ry*rz*(1-c)-rx*s); m_mtx[13]=ty;
00125   m_mtx[2]=sz*(rz*rx*(1-c)-ry*s); m_mtx[6]=sz*(rz*ry*(1-c)+rx*s); m_mtx[10]=sz*(rz*rz*(1-c)+c);   m_mtx[14]=tz;
00126   m_mtx[3]=0.0;                   m_mtx[7]=0.0;                   m_mtx[11]=0.0;                  m_mtx[15]=1.0;
00127 }
00128 
00129 geom::txform geom::txform::identity()
00130 {
00131 GVX_TRACE("geom::txform::identity");
00132   txform r(true);
00133   r[0] = 1.0; r[4] = 0.0; r[8] = 0.0; r[12] = 0.0;
00134   r[1] = 0.0; r[5] = 1.0; r[9] = 0.0; r[13] = 0.0;
00135   r[2] = 0.0; r[6] = 0.0; r[10] = 1.0; r[14] = 0.0;
00136   r[3] = 0.0; r[7] = 0.0; r[11] = 0.0; r[15] = 1.0;
00137   return r;
00138 }
00139 
00140 geom::txform geom::txform::random()
00141 {
00142 GVX_TRACE("geom::txform::random");
00143 
00144   static rutz::urand generator(rutz::default_rand_seed);
00145 
00146   txform result(true);
00147   for (int i = 0; i < 16; ++i)
00148     result.m_mtx[i] = generator.fdraw_range(0.0, 1.0);
00149   return result;
00150 }
00151 
00152 geom::txform geom::txform::orthographic(const geom::rect<double>& b,
00153                                         double zNear, double zFar)
00154 {
00155 GVX_TRACE("geom::txform::orthographic");
00156 
00157   txform result = txform::no_init();
00158 
00159   result[0] = 2.0/(b.width());
00160   result[1] = 0;
00161   result[2] = 0;
00162   result[3] = 0;
00163 
00164   result[4] = 0;
00165   result[5] = 2.0/(b.height());
00166   result[6] = 0;
00167   result[7] = 0;
00168 
00169   result[8] = 0;
00170   result[9] = 0;
00171   result[10] = -2.0/(zFar - zNear);
00172   result[11] = 0;
00173 
00174   result[12] = -(b.right()+b.left())/(b.right()-b.left());
00175   result[13] = -(b.top()+b.bottom())/(b.top()-b.bottom());
00176   result[14] = -(zFar+zNear)/(zFar-zNear);
00177   result[15] = 1.0;
00178 
00179   return result;
00180 }
00181 
00182 geom::txform geom::txform::inverted() const
00183 {
00184 GVX_TRACE("geom::txform::inverted");
00185 
00186   // ALGORITHM (Cramer's Rule):
00187 
00188   // 1. Compute the transpose, M' = transpose(M)
00189   // 2. Compute the cofactor matrix, cof(M')
00190   // 3. Compute the determinant of the cofactor matrix, det(cof(M'))
00191   // 4. Compute the inverse, inv(M) = cof(M')/det(cof(M'))
00192 
00193   /*
00194      |  0  4  8  C  |
00195      |  1  5  9  D  |
00196      |  2  6  A  E  |
00197      |  3  7  B  F  |
00198   */
00199 
00200 #define A 10
00201 #define B 11
00202 #define C 12
00203 #define D 13
00204 #define E 14
00205 #define F 15
00206 
00207   //
00208   // 1. Compute the transpose, M' = transpose(M)
00209   //
00210 
00211 #define s0 (m_mtx[0])
00212 #define s1 (m_mtx[4])
00213 #define s2 (m_mtx[8])
00214 #define s3 (m_mtx[C])
00215 #define s4 (m_mtx[1])
00216 #define s5 (m_mtx[5])
00217 #define s6 (m_mtx[9])
00218 #define s7 (m_mtx[D])
00219 #define s8 (m_mtx[2])
00220 #define s9 (m_mtx[6])
00221 #define sA (m_mtx[A])
00222 #define sB (m_mtx[E])
00223 #define sC (m_mtx[3])
00224 #define sD (m_mtx[7])
00225 #define sE (m_mtx[B])
00226 #define sF (m_mtx[F])
00227 
00228   //
00229   // 2. Compute the cofactor matrix, cof(M')
00230   //
00231 
00232   double cof[16]; // cofactor matrix
00233 
00234   /*
00235      |  0  4  8  C  |
00236      |  1  5  9  D  |
00237      |  2  6  A  E  |
00238      |  3  7  B  F  |
00239   */
00240 
00241   {
00242     // We compute each of the 2x2 determinants in the following form:
00243     // (upperleft*lowerright - lowerleft*upperright)
00244 
00245     // This makes it easier to verify that we have all the correct +/-
00246     // signs in the 3x3 determinants (i.e. cofactors) below.
00247 
00248     const double t8D_9C = (s8 * sD) - (s9 * sC);
00249     const double t8E_AC = (s8 * sE) - (sA * sC);
00250     const double t8F_BC = (s8 * sF) - (sB * sC);
00251     const double t9E_AD = (s9 * sE) - (sA * sD);
00252     const double t9F_BD = (s9 * sF) - (sB * sD);
00253     const double tAF_BE = (sA * sF) - (sB * sE);
00254 
00255     cof[0]=    + tAF_BE*s5 - t9F_BD*s6 + t9E_AD*s7;
00256     cof[1]=    - tAF_BE*s4 + t8F_BC*s6 - t8E_AC*s7;
00257     cof[2]=    + t9F_BD*s4 - t8F_BC*s5 + t8D_9C*s7;
00258     cof[3]=    - t9E_AD*s4 + t8E_AC*s5 - t8D_9C*s6;
00259 
00260     cof[4]=    - tAF_BE*s1 + t9F_BD*s2 - t9E_AD*s3;
00261     cof[5]=    + tAF_BE*s0 - t8F_BC*s2 + t8E_AC*s3;
00262     cof[6]=    - t9F_BD*s0 + t8F_BC*s1 - t8D_9C*s3;
00263     cof[7]=    + t9E_AD*s0 - t8E_AC*s1 + t8D_9C*s2;
00264   }
00265 
00266   /*
00267      |  0  4  8  C  |
00268      |  1  5  9  D  |
00269      |  2  6  A  E  |
00270      |  3  7  B  F  |
00271   */
00272 
00273   {
00274     const double t05_14 = (s0 * s5) - (s1 * s4);
00275     const double t06_24 = (s0 * s6) - (s2 * s4);
00276     const double t07_34 = (s0 * s7) - (s3 * s4);
00277     const double t16_25 = (s1 * s6) - (s2 * s5);
00278     const double t17_35 = (s1 * s7) - (s3 * s5);
00279     const double t27_36 = (s2 * s7) - (s3 * s6);
00280 
00281     cof[8]=    + t27_36*sD - t17_35*sE + t16_25*sF;
00282     cof[9]=    - t27_36*sC + t07_34*sE - t06_24*sF;
00283     cof[A]=    + t17_35*sC - t07_34*sD + t05_14*sF;
00284     cof[B]=    - t16_25*sC + t06_24*sD - t05_14*sE;
00285 
00286     cof[C]=    - t27_36*s9 + t17_35*sA - t16_25*sB;
00287     cof[D]=    + t27_36*s8 - t07_34*sA + t06_24*sB;
00288     cof[E]=    - t17_35*s8 + t07_34*s9 - t05_14*sB;
00289     cof[F]=    + t16_25*s8 - t06_24*s9 + t05_14*sA;
00290   }
00291 
00292   //
00293   // 3. Compute the determinant of the cofactor matrix, det(cof(M'))
00294   //
00295 
00296   const double det_reciprocal =
00297     1.0 / (s0*cof[0] +
00298            s1*cof[1] +
00299            s2*cof[2] +
00300            s3*cof[3]);
00301 
00302 #undef A
00303 #undef B
00304 #undef C
00305 #undef D
00306 #undef E
00307 #undef F
00308 
00309 #undef s0
00310 #undef s1
00311 #undef s2
00312 #undef s3
00313 #undef s4
00314 #undef s5
00315 #undef s6
00316 #undef s7
00317 #undef s8
00318 #undef s9
00319 #undef sA
00320 #undef sB
00321 #undef sC
00322 #undef sD
00323 #undef sE
00324 #undef sF
00325 
00326   //
00327   // 4. Compute the inverse, inv(M) = cof(M')/det(cof(M'))
00328   //
00329 
00330   geom::txform inverse(true);
00331 
00332   for (int i = 0; i < 16; ++i)
00333     {
00334       inverse.m_mtx[i] = cof[i] * det_reciprocal;
00335     }
00336 
00337   return inverse;
00338 }
00339 
00340 void geom::txform::translate(const vec3d& t)
00341 {
00342 GVX_TRACE("geom::txform::translate");
00343 
00344 #if 0
00345   /*                     brute force
00346 
00347               | m0 m4 m8  m12 |   |  1  0  0 tx |
00348      result = | m1 m5 m9  m13 | * |  0  1  0 ty |
00349               | m2 m6 m10 m14 |   |  0  0  1 tz |
00350               | m3 m7 m11 m15 |   |  0  0  0  1 |
00351   */
00352   txform old_mtx(*this);
00353 
00354   double tm[16];
00355   tm[0] = 1.0; tm[4] = 0.0;  tm[8] = 0.0; tm[12] = t.x();
00356   tm[1] = 0.0; tm[5] = 1.0;  tm[9] = 0.0; tm[13] = t.y();
00357   tm[2] = 0.0; tm[6] = 0.0; tm[10] = 1.0; tm[14] = t.z();
00358   tm[3] = 0.0; tm[7] = 0.0; tm[11] = 0.0; tm[15] = 1.0;
00359 
00360   mul_mtx_4x4(old_mtx.m_mtx, tm, this->m_mtx);
00361 #else
00362   /*                 optimized
00363 
00364               | m0 m4 m8  m0*tx+m4*ty+m8*tz+m12  |
00365      result = | m1 m5 m9  m1*tx+m5*ty+m9*tz+m13  |
00366               | m2 m6 m10 m2*tx+m6*ty+m10*tz+m14 |
00367               | m3 m7 m11 m3*tx+m7*ty+m11*tz+m15 |
00368   */
00369   m_mtx[12] += t.x()*m_mtx[0] + t.y()*m_mtx[4] + t.z()*m_mtx[8];
00370   m_mtx[13] += t.x()*m_mtx[1] + t.y()*m_mtx[5] + t.z()*m_mtx[9];
00371   m_mtx[14] += t.x()*m_mtx[2] + t.y()*m_mtx[6] + t.z()*m_mtx[10];
00372   m_mtx[15] += t.x()*m_mtx[3] + t.y()*m_mtx[7] + t.z()*m_mtx[11];
00373 #endif
00374 }
00375 
00376 void geom::txform::scale(const vec3d& s)
00377 {
00378 GVX_TRACE("geom::txform::scale");
00379 
00380 #if 0
00381   /*                brute force
00382 
00383               | m0 m4 m8  m12 |   | sx  0  0  0 |
00384      result = | m1 m5 m9  m13 | * |  0 sy  0  0 |
00385               | m2 m6 m10 m14 |   |  0  0 sz  0 |
00386               | m3 m7 m11 m15 |   |  0  0  0  1 |
00387   */
00388   txform old_mtx(*this);
00389 
00390   double sm[16];
00391   sm[0] = s.x(); sm[4] = 0.0;    sm[8] = 0.0;    sm[12] = 0.0;
00392   sm[1] = 0.0;   sm[5] = s.y();  sm[9] = 0.0;    sm[13] = 0.0;
00393   sm[2] = 0.0;   sm[6] = 0.0;    sm[10] = s.z(); sm[14] = 0.0;
00394   sm[3] = 0.0;   sm[7] = 0.0;    sm[11] = 0.0;   sm[15] = 1.0;
00395 
00396   mul_mtx_4x4(old_mtx.m_mtx, sm, this->m_mtx);
00397 #else
00398   /*                 optimized
00399 
00400               | sx*m0 sy*m4 sz*m8  m12 |
00401      result = | sx*m1 sy*m5 sz*m9  m13 |
00402               | sx*m2 sy*m6 sz*m10 m14 |
00403               | sx*m3 sy*m7 sz*m11 m15 |
00404   */
00405   m_mtx[0] *= s.x();
00406   m_mtx[1] *= s.x();
00407   m_mtx[2] *= s.x();
00408   m_mtx[3] *= s.x();
00409   m_mtx[4] *= s.y();
00410   m_mtx[5] *= s.y();
00411   m_mtx[6] *= s.y();
00412   m_mtx[7] *= s.y();
00413   m_mtx[8] *= s.z();
00414   m_mtx[9] *= s.z();
00415   m_mtx[10] *= s.z();
00416   m_mtx[11] *= s.z();
00417 #endif
00418 }
00419 
00420 void geom::txform::rotate(const vec3d& rotation_axis,
00421                           double rotation_angle)
00422 {
00423 GVX_TRACE("geom::txform::rotate");
00424 
00425   txform rotation(vec3d(0, 0, 0),
00426                   vec3d(1, 1, 1),
00427                   rotation_axis, rotation_angle);
00428 
00429   this->transform(rotation);
00430 }
00431 
00432 geom::txform geom::txform::mtx_mul(const txform& other) const
00433 {
00434 GVX_TRACE("geom::txform::mtx_mul");
00435 
00436   txform result(true);
00437 
00438   mul_mtx_4x4(this->m_mtx, other.m_mtx, result.m_mtx);
00439 
00440   return result;
00441 }
00442 
00443 void geom::txform::transform(const geom::txform& other)
00444 {
00445 GVX_TRACE("geom::txform::transform");
00446 
00447   txform old_mtx(*this);
00448 
00449   mul_mtx_4x4(old_mtx.m_mtx, other.m_mtx, this->m_mtx);
00450 }
00451 
00452 vec2d geom::txform::apply_to(const vec2d& input) const
00453 {
00454 GVX_TRACE("geom::txform::apply_to(vec2d)");
00455   /*
00456         | m0 m4 m8  m12 |   | input.x |
00457         | m1 m5 m9  m13 | * | input.y |
00458         | m2 m6 m10 m14 |   |    0    |
00459         | m3 m7 m11 m15 |   |    1    |
00460    */
00461 
00462   const double output_x =
00463     m_mtx[0] * input.x()
00464     + m_mtx[4] * input.y()
00465     // + m_mtx[8] * 0.0
00466     + m_mtx[12] * 1.0;
00467 
00468   const double output_y =
00469     m_mtx[1] * input.x()
00470     + m_mtx[5] * input.y()
00471     // + m_mtx[9] * 0.0
00472     + m_mtx[13] * 1.0;
00473 
00474   // output_z is forced to zero since we're returning a 2-D result
00475 
00476   const double output_w =
00477     m_mtx[3] * input.x()
00478     + m_mtx[7] * input.y()
00479     // + m_mtx[11] * 0.0
00480     + m_mtx[15] * 1.0;
00481 
00482   return (output_w != 0.0)
00483     ? vec2d(output_x / output_w, output_y / output_w)
00484     : vec2d(0.0, 0.0);
00485 }
00486 
00487 vec3d geom::txform::apply_to(const vec3d& input) const
00488 {
00489 GVX_TRACE("geom::txform::apply_to(vec3d)");
00490   /*
00491         | m0 m4 m8  m12 |   | input.x |
00492         | m1 m5 m9  m13 | * | input.y |
00493         | m2 m6 m10 m14 |   | input.z |
00494         | m3 m7 m11 m15 |   |    1    |
00495    */
00496 
00497   const double output_x =
00498     m_mtx[0] * input.x()
00499     + m_mtx[4] * input.y()
00500     + m_mtx[8] * input.z()
00501     + m_mtx[12] * 1.0;
00502 
00503   const double output_y =
00504     m_mtx[1] * input.x()
00505     + m_mtx[5] * input.y()
00506     + m_mtx[9] * input.z()
00507     + m_mtx[13] * 1.0;
00508 
00509   const double output_z =
00510     m_mtx[2] * input.x()
00511     + m_mtx[6] * input.y()
00512     + m_mtx[10] * input.z()
00513     + m_mtx[14] * 1.0;
00514 
00515   const double output_w =
00516     m_mtx[3] * input.x()
00517     + m_mtx[7] * input.y()
00518     + m_mtx[11] * input.z()
00519     + m_mtx[15] * 1.0;
00520 
00521   return (output_w != 0.0)
00522     ? vec3d(output_x / output_w,
00523             output_y / output_w,
00524             output_z / output_w)
00525     : vec3d(0.0, 0.0, 0.0);
00526 }
00527 
00528 void geom::txform::set_col_major_data(const double* data)
00529 {
00530 GVX_TRACE("geom::txform::set_col_major_data");
00531   for (int i = 0; i < 16; ++i)
00532     m_mtx[i] = data[i];
00533 }
00534 
00535 void geom::txform::debug_dump() const
00536 {
00537 GVX_TRACE("geom::txform::debug_dump");
00538   dbg_nl(0);
00539   dbg_print(0, m_mtx[0]); dbg_print(0, m_mtx[4]); dbg_print(0, m_mtx[8]); dbg_print_nl(0, m_mtx[12]);
00540   dbg_print(0, m_mtx[1]); dbg_print(0, m_mtx[5]); dbg_print(0, m_mtx[9]); dbg_print_nl(0, m_mtx[13]);
00541   dbg_print(0, m_mtx[2]); dbg_print(0, m_mtx[6]); dbg_print(0, m_mtx[10]); dbg_print_nl(0, m_mtx[14]);
00542   dbg_print(0, m_mtx[3]); dbg_print(0, m_mtx[7]); dbg_print(0, m_mtx[11]); dbg_print_nl(0, m_mtx[15]);
00543 }
00544 
00545 double geom::txform::debug_sse(const txform& ref) const
00546 {
00547 GVX_TRACE("geom::txform::debug_sse");
00548   double result = 0.0;
00549   for (int i = 0; i < 16; ++i)
00550     {
00551       const double diff = this->m_mtx[i] - ref.m_mtx[i];
00552       result += (diff*diff);
00553     }
00554   return result;
00555 }
00556 
00557 static const char __attribute__((used)) vcid_groovx_geom_txform_cc_utc20050626084023[] = "$Id: txform.cc 10065 2007-04-12 05:54:56Z rjpeters $ $HeadURL: file:
00558 #endif // !GROOVX_GEOM_TXFORM_CC_UTC20050626084023_DEFINED

The software described here is Copyright (c) 1998-2005, Rob Peters.
This page was generated Wed Dec 3 06:49:38 2008 by Doxygen version 1.5.5.