panda/src/linmath/lquaternion_src.cxx Source File

00001 // Filename: lquaternion_src.cxx
00002 // Created by:  
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 #include "config_linmath.h"
00020 #include "lmatrix.h"
00021 #include "luse.h"
00022 
00023 TypeHandle FLOATNAME(LQuaternion)::_type_handle;
00024 
00025 const FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::_ident_quat =
00026   FLOATNAME(LQuaternion)(1.0f, 0.0f, 0.0f, 0.0f);
00027 
00028 ////////////////////////////////////////////////////////////////////
00029 //     Function: LQuaternion::pure_imaginary_quat
00030 //       Access: public
00031 //  Description:
00032 ////////////////////////////////////////////////////////////////////
00033 FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
00034 pure_imaginary(const FLOATNAME(LVector3) &v) {
00035   return FLOATNAME(LQuaternion)(0, v[0], v[1], v[2]);
00036 }
00037 
00038 ////////////////////////////////////////////////////////////////////
00039 //     Function: LQuaternion::extract_to_matrix (LMatrix3)
00040 //       Access: Public
00041 //  Description: Based on the quat lib from VRPN.
00042 ////////////////////////////////////////////////////////////////////
00043 void FLOATNAME(LQuaternion)::
00044 extract_to_matrix(FLOATNAME(LMatrix3) &m) const {
00045   FLOATTYPE N = this->dot(*this);
00046   FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
00047   FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00048 
00049   xs = _v.data[1] * s;   ys = _v.data[2] * s;   zs = _v.data[3] * s;
00050   wx = _v.data[0] * xs;  wy = _v.data[0] * ys;  wz = _v.data[0] * zs;
00051   xx = _v.data[1] * xs;  xy = _v.data[1] * ys;  xz = _v.data[1] * zs;
00052   yy = _v.data[2] * ys;  yz = _v.data[2] * zs;  zz = _v.data[3] * zs;
00053 
00054   m.set((1.0f - (yy + zz)), (xy + wz), (xz - wy),
00055         (xy - wz), (1.0f - (xx + zz)), (yz + wx),
00056         (xz + wy), (yz - wx), (1.0f - (xx + yy)));
00057 }
00058 
00059 ////////////////////////////////////////////////////////////////////
00060 //     Function: LQuaternion::extract_to_matrix (LMatrix4)
00061 //       Access: Public
00062 //  Description: Based on the quat lib from VRPN.
00063 ////////////////////////////////////////////////////////////////////
00064 void FLOATNAME(LQuaternion)::
00065 extract_to_matrix(FLOATNAME(LMatrix4) &m) const {
00066   FLOATTYPE N = this->dot(*this);
00067   FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
00068   FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00069 
00070   xs = _v.data[1] * s;   ys = _v.data[2] * s;   zs = _v.data[3] * s;
00071   wx = _v.data[0] * xs;  wy = _v.data[0] * ys;  wz = _v.data[0] * zs;
00072   xx = _v.data[1] * xs;  xy = _v.data[1] * ys;  xz = _v.data[1] * zs;
00073   yy = _v.data[2] * ys;  yz = _v.data[2] * zs;  zz = _v.data[3] * zs;
00074 
00075   m.set((1.0f - (yy + zz)), (xy + wz), (xz - wy), 0.0f,
00076         (xy - wz), (1.0f - (xx + zz)), (yz + wx), 0.0f,
00077         (xz + wy), (yz - wx), (1.0f - (xx + yy)), 0.0f,
00078         0.0f, 0.0f, 0.0f, 1.0f);
00079 }
00080 
00081 ////////////////////////////////////////////////////////////////////
00082 //     Function: LQuaternion::set_hpr
00083 //       Access: public
00084 //  Description: Sets the quaternion as the unit quaternion that
00085 //               is equivalent to these Euler angles.
00086 //               (from Real-time Rendering, p.49)
00087 ////////////////////////////////////////////////////////////////////
00088 void FLOATNAME(LQuaternion)::
00089 set_hpr(const FLOATNAME(LVecBase3) &hpr) {
00090   FLOATNAME(LQuaternion) quat_h, quat_p, quat_r;
00091 
00092   FLOATNAME(LVector3) v;
00093   FLOATTYPE a, s, c;
00094 
00095   v = FLOATNAME(LVector3)::up();
00096   a = deg_2_rad(hpr[0] * 0.5f);
00097   csincos(a, &s, &c);
00098   quat_h.set(c, v[0] * s, v[1] * s, v[2] * s);
00099   v = FLOATNAME(LVector3)::right();
00100   a = deg_2_rad(hpr[1] * 0.5f);
00101   csincos(a, &s, &c);
00102   s = csin(a);
00103   quat_p.set(c, v[0] * s, v[1] * s, v[2] * s);
00104   v = FLOATNAME(LVector3)::forward();
00105   a = deg_2_rad(hpr[2] * 0.5f);
00106   csincos(a, &s, &c);
00107   quat_r.set(c, v[0] * s, v[1] * s, v[2] * s);
00108 
00109   (*this) = quat_r * quat_p * quat_h;
00110 
00111   if (!temp_hpr_fix) {
00112     // Compute the old, broken hpr.
00113     (*this) = quat_p * quat_h * invert(quat_r);
00114   }
00115 
00116 #ifndef NDEBUG
00117   if (paranoid_hpr_quat) {
00118     FLOATNAME(LMatrix3) mat;
00119     compose_matrix(mat, FLOATNAME(LVecBase3)(1.0f, 1.0f, 1.0f), hpr);
00120     FLOATNAME(LQuaternion) compare;
00121     compare.set_from_matrix(mat);
00122     if (!compare.almost_equal(*this) && !compare.almost_equal(-(*this))) {
00123       linmath_cat.warning()
00124         << "hpr-to-quat of " << hpr << " computed " << *this
00125         << " instead of " << compare << "\n";
00126       (*this) = compare;
00127     }
00128   }
00129 #endif  // NDEBUG
00130 }
00131 
00132 ////////////////////////////////////////////////////////////////////
00133 //     Function: LQuaternion::get_hpr
00134 //       Access: public
00135 //  Description: Extracts the equivalent Euler angles from the unit
00136 //               quaternion.
00137 ////////////////////////////////////////////////////////////////////
00138 FLOATNAME(LVecBase3) FLOATNAME(LQuaternion)::
00139 get_hpr() const {
00140   if (!temp_hpr_fix) {
00141     // With the old, broken hpr code in place, just go through the
00142     // existing matrix decomposition code.  Not particularly speedy,
00143     // but I don't want to bother with working out how to do it
00144     // directly for code that hopefully won't need to last much
00145     // longer.
00146     FLOATNAME(LMatrix3) mat;
00147     extract_to_matrix(mat);
00148     FLOATNAME(LVecBase3) scale, hpr;
00149     decompose_matrix(mat, scale, hpr);
00150     return hpr;
00151   }
00152 
00153   FLOATNAME(LVecBase3) hpr;
00154   FLOATTYPE N = (_v.data[0] * _v.data[0]) + (_v.data[1] * _v.data[1]) + (_v.data[2] * _v.data[2]) + (_v.data[3] * _v.data[3]);
00155   FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
00156   FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4;
00157   FLOATTYPE cr, sr, cp, sp, ch, sh;
00158 
00159   xs = _v.data[1] * s;   ys = _v.data[2] * s;   zs = _v.data[3] * s;
00160   wx = _v.data[0] * xs;  wy = _v.data[0] * ys;  wz = _v.data[0] * zs;
00161   xx = _v.data[1] * xs;  xy = _v.data[1] * ys;  xz = _v.data[1] * zs;
00162   yy = _v.data[2] * ys;  yz = _v.data[2] * zs;  zz = _v.data[3] * zs;
00163   c1 = xz - wy;
00164   c2 = 1.0f - (xx + yy);
00165   c3 = 1.0f - (yy + zz);
00166   c4 = xy + wz;
00167 
00168   if (c1 == 0.0f) {  // (roll = 0 or 180) or (pitch = +/- 90)
00169     if (c2 >= 0.0f) {
00170       hpr[2] = 0.0f;
00171       ch = c3;
00172       sh = c4;
00173       cp = c2;
00174     } else {
00175       hpr[2] = 180.0f;
00176       ch = -c3;
00177       sh = -c4;
00178       cp = -c2;
00179     }
00180   } else {
00181     // this should work all the time, but the above saves some trig operations
00182     FLOATTYPE roll = catan2(-c1, c2);
00183     csincos(roll, &sr, &cr);
00184     hpr[2] = rad_2_deg(roll);
00185     ch = (cr * c3) + (sr * (xz + wy));
00186     sh = (cr * c4) + (sr * (yz - wx));
00187     cp = (cr * c2) - (sr * c1);
00188   }
00189   sp = yz + wx;
00190   hpr[0] = rad_2_deg(catan2(sh, ch));
00191   hpr[1] = rad_2_deg(catan2(sp, cp));
00192 
00193 #ifndef NDEBUG
00194   if (paranoid_hpr_quat) {
00195     FLOATNAME(LMatrix3) mat;
00196     extract_to_matrix(mat);
00197     FLOATNAME(LVecBase3) scale, compare_hpr;
00198     decompose_matrix(mat, scale, compare_hpr);
00199     if (!compare_hpr.almost_equal(hpr)) {
00200       linmath_cat.warning()
00201         << "quat-to-hpr of " << *this << " computed " << hpr << " instead of "
00202         << compare_hpr << "\n";
00203       hpr = compare_hpr;
00204     }
00205   }
00206 #endif  // NDEBUG
00207 
00208   return hpr;
00209 }
00210 
00211 ////////////////////////////////////////////////////////////////////
00212 //     Function: LQuaternion::set_from_matrix
00213 //       Access: public
00214 //  Description: Sets the quaternion according to the rotation
00215 //               represented by the matrix.  Originally we tried an
00216 //               algorithm presented by Do-While Jones, but that
00217 //               turned out to be broken.  This is based on the quat
00218 //               lib from UNC.
00219 ////////////////////////////////////////////////////////////////////
00220 void FLOATNAME(LQuaternion)::
00221 set_from_matrix(const FLOATNAME(LMatrix3) &m) {
00222   FLOATTYPE m00, m01, m02, m10, m11, m12, m20, m21, m22;
00223 
00224   m00 = m(0, 0);
00225   m10 = m(1, 0);
00226   m20 = m(2, 0);
00227   m01 = m(0, 1);
00228   m11 = m(1, 1);
00229   m21 = m(2, 1);
00230   m02 = m(0, 2);
00231   m12 = m(1, 2);
00232   m22 = m(2, 2);
00233 
00234   FLOATTYPE trace = m00 + m11 + m22;
00235 
00236   if (trace > 0.0f) {
00237     // The easy case.
00238     FLOATTYPE S = csqrt(trace + 1.0f);
00239     _v.data[0] = S * 0.5f;
00240     S = 0.5f / S;
00241     _v.data[1] = (m12 - m21) * S;
00242     _v.data[2] = (m20 - m02) * S;
00243     _v.data[3] = (m01 - m10) * S;
00244 
00245   } else {
00246     // The harder case.  First, figure out which column to take as
00247     // root.  This will be the column with the largest value.
00248 
00249     // It is tempting to try to compare the absolute values of the
00250     // diagonal values in the code below, instead of their normal,
00251     // signed values.  Don't do it.  We are actually maximizing the
00252     // value of S, which must always be positive, and is therefore
00253     // based on the diagonal whose actual value--not absolute
00254     // value--is greater than those of the other two.
00255 
00256     // We already know that m00 + m11 + m22 <= 0 (because we are here
00257     // in the harder case).
00258 
00259     if (m00 > m11 && m00 > m22) {
00260       // m00 is larger than m11 and m22.
00261       FLOATTYPE S = 1.0f + m00 - (m11 + m22);
00262       nassertv(S > 0.0f);
00263       S = csqrt(S);
00264       _v.data[1] = S * 0.5f;
00265       S = 0.5f / S;
00266       _v.data[2] = (m01 + m10) * S;
00267       _v.data[3] = (m02 + m20) * S;
00268       _v.data[0] = (m12 - m21) * S;
00269 
00270     } else if (m11 > m22) {
00271       // m11 is larger than m00 and m22.
00272       FLOATTYPE S = 1.0f + m11 - (m22 + m00);
00273       nassertv(S > 0.0f);
00274       S = csqrt(S);
00275       _v.data[2] = S * 0.5f;
00276       S = 0.5f / S;
00277       _v.data[3] = (m12 + m21) * S;
00278       _v.data[1] = (m10 + m01) * S;
00279       _v.data[0] = (m20 - m02) * S;
00280 
00281     } else {
00282       // m22 is larger than m00 and m11.
00283       FLOATTYPE S = 1.0f + m22 - (m00 + m11);
00284       nassertv(S > 0.0f);
00285       S = csqrt(S);
00286       _v.data[3] = S * 0.5f;
00287       S = 0.5f / S;
00288       _v.data[1] = (m20 + m02) * S;
00289       _v.data[2] = (m21 + m12) * S;
00290       _v.data[0] = (m01 - m10) * S;
00291     }
00292   }
00293 }
00294 
00295 ////////////////////////////////////////////////////////////////////
00296 //     Function: LQuaternion::init_type
00297 //       Access: public
00298 //  Description:
00299 ////////////////////////////////////////////////////////////////////
00300 void FLOATNAME(LQuaternion)::
00301 init_type() {
00302   if (_type_handle == TypeHandle::none()) {
00303     FLOATNAME(LVecBase4)::init_type();
00304     string name = "LQuaternion";
00305     name += FLOATTOKEN;
00306     register_type(_type_handle, name,
00307                   FLOATNAME(LVecBase4)::get_class_type());
00308   }
00309 }