diff --git a/panda/src/linmath/lquaternion.I b/panda/src/linmath/lquaternion.I index 18a52e872e..82f7ff22ab 100644 --- a/panda/src/linmath/lquaternion.I +++ b/panda/src/linmath/lquaternion.I @@ -40,11 +40,11 @@ PUBLISHED: void set(const FLOATNAME(LMatrix3) &m); INLINE void set(const FLOATNAME(LMatrix4) &m); - INLINE void extract_to_matrix(FLOATNAME(LMatrix3) &m) const; - INLINE void extract_to_matrix(FLOATNAME(LMatrix4) &m) const; + void extract_to_matrix(FLOATNAME(LMatrix3) &m) const; + void extract_to_matrix(FLOATNAME(LMatrix4) &m) const; - INLINE void set_hpr(const FLOATNAME(LVecBase3) &hpr); - INLINE FLOATNAME(LVecBase3) get_hpr() const; + void set_hpr(const FLOATNAME(LVecBase3) &hpr); + FLOATNAME(LVecBase3) get_hpr() const; INLINE FLOATTYPE1 get_r(void) const; INLINE FLOATTYPE1 get_i(void) const; @@ -407,209 +407,13 @@ set(const FLOATNAME(LMatrix4) &m) { set(m.get_upper_3()); } -//////////////////////////////////////////////////////////////////// -// Function: extract (LMatrix3) -// Access: public -// Description: Do-While Jones paper from cary. -//////////////////////////////////////////////////////////////////// - -INLINE void FLOATNAME(LQuaternionBase):: -extract_to_matrix(FLOATNAME(LMatrix3) &m) const { - FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k); - FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N); - FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; - - xs = _i * s; ys = _j * s; zs = _k * s; - wx = _r * xs; wy = _r * ys; wz = _r * zs; - xx = _i * xs; xy = _i * ys; xz = _i * zs; - yy = _j * ys; yz = _j * zs; zz = _k * zs; - - m = FLOATNAME(LMatrix3)((1. - (yy + zz)), (xy - wz), (xz + wy), - (xy + wz), (1. - (xx + zz)), (yz - wx), - (xz - wy), (yz + wx), (1. - (xx + yy))); -} - -//////////////////////////////////////////////////////////////////// -// Function: extract (LMatrix4) -// Access: public -// Description: -//////////////////////////////////////////////////////////////////// - -INLINE void FLOATNAME(LQuaternionBase):: -extract_to_matrix(FLOATNAME(LMatrix4) &m) const { - FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k); - FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N); - FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; - - xs = _i * s; ys = _j * s; zs = _k * s; - wx = _r * xs; wy = _r * ys; wz = _r * zs; - xx = _i * xs; xy = _i * ys; xz = _i * zs; - yy = _j * ys; yz = _j * zs; zz = _k * zs; - - m = FLOATNAME(LMatrix4)((1. - (yy + zz)), (xy - wz), (xz + wy), 0., - (xy + wz), (1. - (xx + zz)), (yz - wx), 0., - (xz - wy), (yz + wx), (1. - (xx + yy)), 0., - 0., 0., 0., 1.); -} - -//////////////////////////////////////////////////////////////////// -// Function: set_hpr -// Access: public -// Description: Sets the quaternion as the unit quaternion that -// is equivalent to these Euler angles. -// (from Real-time Rendering, p.49) -//////////////////////////////////////////////////////////////////// - -INLINE void FLOATNAME(LQuaternionBase):: -set_hpr(const FLOATNAME(LVecBase3) &hpr) { - FLOATNAME(LQuaternionBase) quat_h, quat_p, quat_r; - - FLOATNAME(LVector3) v = FLOATNAME(LVector3)::up(); - FLOATTYPE1 a = deg_2_rad(hpr[0] * 0.5); - FLOATTYPE1 s,c; - - csincos(a,&s,&c); - quat_h.set(c, v[0] * s, v[1] * s, v[2] * s); - v = FLOATNAME(LVector3)::right(); - a = deg_2_rad(hpr[1] * 0.5); - csincos(a,&s,&c); - s = csin(a); - quat_p.set(c, v[0] * s, v[1] * s, v[2] * s); - v = FLOATNAME(LVector3)::forward(); - a = deg_2_rad(hpr[2] * 0.5); - csincos(a,&s,&c); - quat_r.set(c, v[0] * s, v[1] * s, v[2] * s); - - (*this) = quat_h * quat_p * quat_r; -} - -//////////////////////////////////////////////////////////////////// -// Function: get_hpr -// Access: public -// Description: Extracts the equivalent Euler angles from the unit -// quaternion. -//////////////////////////////////////////////////////////////////// - -INLINE FLOATNAME(LVecBase3) FLOATNAME(LQuaternionBase):: -get_hpr() const { - FLOATTYPE1 heading, pitch, roll; - FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k); - FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N); - FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4; - FLOATTYPE1 cr, sr, cp, sp, ch, sh; - - xs = _i * s; ys = _j * s; zs = _k * s; - wx = _r * xs; wy = _r * ys; wz = _r * zs; - xx = _i * xs; xy = _i * ys; xz = _i * zs; - yy = _j * ys; yz = _j * zs; zz = _k * zs; - c1 = xz - wy; - c2 = 1. - (xx + yy); - c3 = 1. - (yy + zz); - c4 = xy + wz; - - if (c1 == 0.) { // (roll = 0 or 180) or (pitch = +/- 90 - if (c2 >= 0.) { - roll = 0.; - ch = c3; - sh = c4; - cp = c2; - } else { - roll = 180.; - ch = -c3; - sh = -c4; - cp = -c2; - } - } else { - // this should work all the time, but the above saves some trig operations - roll = catan2(-c1, c2); - csincos(roll,&sr,&cr); - roll = rad_2_deg(roll); - ch = (cr * c3) + (sr * (xz + wy)); - sh = (cr * c4) + (sr * (yz - wx)); - cp = (cr * c2) - (sr * c1); - } - sp = yz + wx; - heading = rad_2_deg(catan2(sh, ch)); - pitch = rad_2_deg(catan2(sp, cp)); - - return FLOATNAME(LVecBase3)(heading, pitch, roll); -} - -//////////////////////////////////////////////////////////////////// -// Function: set -// Access: public -// Description: Do-While Jones. -//////////////////////////////////////////////////////////////////// - -INLINE void FLOATNAME(LQuaternionBase):: -set(const FLOATNAME(LMatrix3) &m) { - FLOATTYPE1 m00 = m.get_cell(0, 0); - FLOATTYPE1 m01 = m.get_cell(0, 1); - FLOATTYPE1 m02 = m.get_cell(0, 2); - FLOATTYPE1 m10 = m.get_cell(1, 0); - FLOATTYPE1 m11 = m.get_cell(1, 1); - FLOATTYPE1 m12 = m.get_cell(1, 2); - FLOATTYPE1 m20 = m.get_cell(2, 0); - FLOATTYPE1 m21 = m.get_cell(2, 1); - FLOATTYPE1 m22 = m.get_cell(2, 2); - - FLOATTYPE1 T = m00 + m11 + m22 + 1.; - - if (T > 0.) { - // the easy case - FLOATTYPE1 S = 0.5 / csqrt(T); - _r = 0.25 / S; - _i = (m21 - m12) * S; - _j = (m02 - m20) * S; - _k = (m10 - m01) * S; - } else { - // figure out which column to take as root - int c = 0; - if (cabs(m00) > cabs(m11)) { - if (cabs(m00) > cabs(m22)) - c = 0; - else - c = 2; - } else if (cabs(m11) > cabs(m22)) - c = 1; - else - c = 2; - - FLOATTYPE1 S; - - switch (c) { - case 0: - S = csqrt(1. + m00 - m11 - m22) * 2.; - _r = (m12 + m21) / S; - _i = 0.5 / S; - _j = (m01 + m10) / S; - _k = (m02 + m20) / S; - break; - case 1: - S = csqrt(1. + m11 - m00 - m22) * 2.; - _r = (m02 + m20) / S; - _i = (m01 + m10) / S; - _j = 0.5 / S; - _k = (m12 + m21) / S; - break; - case 2: - S = csqrt(1. + m22 - m00 - m11) * 2.; - _r = (m01 + m10) / S; - _i = (m02 + m20) / S; - _j = (m12 + m21) / S; - _k = 0.5 / S; - break; - } - } -} - //////////////////////////////////////////////////////////////////// // Function: operator *(Matrix3, Quat) // Access: public // Description: //////////////////////////////////////////////////////////////////// -FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m, +INLINE FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m, const FLOATNAME(LQuaternionBase) &q) { FLOATNAME(LMatrix3) q_matrix; q.extract_to_matrix(q_matrix); @@ -623,7 +427,7 @@ FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m, // Description: //////////////////////////////////////////////////////////////////// -FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m, +INLINE FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m, const FLOATNAME(LQuaternionBase) &q) { FLOATNAME(LMatrix4) q_matrix; q.extract_to_matrix(q_matrix); diff --git a/panda/src/linmath/lquaternion_src.I b/panda/src/linmath/lquaternion_src.I index 564a65721b..d2439b0891 100644 --- a/panda/src/linmath/lquaternion_src.I +++ b/panda/src/linmath/lquaternion_src.I @@ -22,6 +22,202 @@ pure_imaginary(const FLOATNAME(LVector3) &v) { return FLOATNAME(LQuaternionBase)(0, v[0], v[1], v[2]); } +//////////////////////////////////////////////////////////////////// +// Function: extract (LMatrix3) +// Access: public +// Description: Do-While Jones paper from cary. +//////////////////////////////////////////////////////////////////// + +void FLOATNAME(LQuaternionBase):: +extract_to_matrix(FLOATNAME(LMatrix3) &m) const { + FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k); + FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N); + FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; + + xs = _i * s; ys = _j * s; zs = _k * s; + wx = _r * xs; wy = _r * ys; wz = _r * zs; + xx = _i * xs; xy = _i * ys; xz = _i * zs; + yy = _j * ys; yz = _j * zs; zz = _k * zs; + + m = FLOATNAME(LMatrix3)((1. - (yy + zz)), (xy - wz), (xz + wy), + (xy + wz), (1. - (xx + zz)), (yz - wx), + (xz - wy), (yz + wx), (1. - (xx + yy))); +} + +//////////////////////////////////////////////////////////////////// +// Function: extract (LMatrix4) +// Access: public +// Description: +//////////////////////////////////////////////////////////////////// + +void FLOATNAME(LQuaternionBase):: +extract_to_matrix(FLOATNAME(LMatrix4) &m) const { + FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k); + FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N); + FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; + + xs = _i * s; ys = _j * s; zs = _k * s; + wx = _r * xs; wy = _r * ys; wz = _r * zs; + xx = _i * xs; xy = _i * ys; xz = _i * zs; + yy = _j * ys; yz = _j * zs; zz = _k * zs; + + m = FLOATNAME(LMatrix4)((1. - (yy + zz)), (xy - wz), (xz + wy), 0., + (xy + wz), (1. - (xx + zz)), (yz - wx), 0., + (xz - wy), (yz + wx), (1. - (xx + yy)), 0., + 0., 0., 0., 1.); +} + +//////////////////////////////////////////////////////////////////// +// Function: set_hpr +// Access: public +// Description: Sets the quaternion as the unit quaternion that +// is equivalent to these Euler angles. +// (from Real-time Rendering, p.49) +//////////////////////////////////////////////////////////////////// + +void FLOATNAME(LQuaternionBase):: +set_hpr(const FLOATNAME(LVecBase3) &hpr) { + FLOATNAME(LQuaternionBase) quat_h, quat_p, quat_r; + + FLOATNAME(LVector3) v = FLOATNAME(LVector3)::up(); + FLOATTYPE1 a = deg_2_rad(hpr[0] * 0.5); + FLOATTYPE1 s,c; + + csincos(a,&s,&c); + quat_h.set(c, v[0] * s, v[1] * s, v[2] * s); + v = FLOATNAME(LVector3)::right(); + a = deg_2_rad(hpr[1] * 0.5); + csincos(a,&s,&c); + s = csin(a); + quat_p.set(c, v[0] * s, v[1] * s, v[2] * s); + v = FLOATNAME(LVector3)::forward(); + a = deg_2_rad(hpr[2] * 0.5); + csincos(a,&s,&c); + quat_r.set(c, v[0] * s, v[1] * s, v[2] * s); + + (*this) = quat_h * quat_p * quat_r; +} + +//////////////////////////////////////////////////////////////////// +// Function: get_hpr +// Access: public +// Description: Extracts the equivalent Euler angles from the unit +// quaternion. +//////////////////////////////////////////////////////////////////// + +FLOATNAME(LVecBase3) FLOATNAME(LQuaternionBase):: +get_hpr() const { + FLOATTYPE1 heading, pitch, roll; + FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k); + FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N); + FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4; + FLOATTYPE1 cr, sr, cp, sp, ch, sh; + + xs = _i * s; ys = _j * s; zs = _k * s; + wx = _r * xs; wy = _r * ys; wz = _r * zs; + xx = _i * xs; xy = _i * ys; xz = _i * zs; + yy = _j * ys; yz = _j * zs; zz = _k * zs; + c1 = xz - wy; + c2 = 1. - (xx + yy); + c3 = 1. - (yy + zz); + c4 = xy + wz; + + if (c1 == 0.) { // (roll = 0 or 180) or (pitch = +/- 90 + if (c2 >= 0.) { + roll = 0.; + ch = c3; + sh = c4; + cp = c2; + } else { + roll = 180.; + ch = -c3; + sh = -c4; + cp = -c2; + } + } else { + // this should work all the time, but the above saves some trig operations + roll = catan2(-c1, c2); + csincos(roll,&sr,&cr); + roll = rad_2_deg(roll); + ch = (cr * c3) + (sr * (xz + wy)); + sh = (cr * c4) + (sr * (yz - wx)); + cp = (cr * c2) - (sr * c1); + } + sp = yz + wx; + heading = rad_2_deg(catan2(sh, ch)); + pitch = rad_2_deg(catan2(sp, cp)); + + return FLOATNAME(LVecBase3)(heading, pitch, roll); +} + +//////////////////////////////////////////////////////////////////// +// Function: set +// Access: public +// Description: Do-While Jones. +//////////////////////////////////////////////////////////////////// + +void FLOATNAME(LQuaternionBase):: +set(const FLOATNAME(LMatrix3) &m) { + FLOATTYPE1 m00 = m.get_cell(0, 0); + FLOATTYPE1 m01 = m.get_cell(0, 1); + FLOATTYPE1 m02 = m.get_cell(0, 2); + FLOATTYPE1 m10 = m.get_cell(1, 0); + FLOATTYPE1 m11 = m.get_cell(1, 1); + FLOATTYPE1 m12 = m.get_cell(1, 2); + FLOATTYPE1 m20 = m.get_cell(2, 0); + FLOATTYPE1 m21 = m.get_cell(2, 1); + FLOATTYPE1 m22 = m.get_cell(2, 2); + + FLOATTYPE1 T = m00 + m11 + m22 + 1.; + + if (T > 0.) { + // the easy case + FLOATTYPE1 S = 0.5 / csqrt(T); + _r = 0.25 / S; + _i = (m21 - m12) * S; + _j = (m02 - m20) * S; + _k = (m10 - m01) * S; + } else { + // figure out which column to take as root + int c = 0; + if (cabs(m00) > cabs(m11)) { + if (cabs(m00) > cabs(m22)) + c = 0; + else + c = 2; + } else if (cabs(m11) > cabs(m22)) + c = 1; + else + c = 2; + + FLOATTYPE1 S; + + switch (c) { + case 0: + S = csqrt(1. + m00 - m11 - m22) * 2.; + _r = (m12 + m21) / S; + _i = 0.5 / S; + _j = (m01 + m10) / S; + _k = (m02 + m20) / S; + break; + case 1: + S = csqrt(1. + m11 - m00 - m22) * 2.; + _r = (m02 + m20) / S; + _i = (m01 + m10) / S; + _j = 0.5 / S; + _k = (m12 + m21) / S; + break; + case 2: + S = csqrt(1. + m22 - m00 - m11) * 2.; + _r = (m01 + m10) / S; + _i = (m02 + m20) / S; + _j = (m12 + m21) / S; + _k = 0.5 / S; + break; + } + } +} + //////////////////////////////////////////////////////////////////// // Function: FLOATNAME(LQuaternionBase)::ident_quat // Access: public