From bd3d94ccec88fe2dbe01e7034f8098e6bffbcb79 Mon Sep 17 00:00:00 2001 From: David Rose Date: Thu, 20 Jun 2002 00:12:14 +0000 Subject: [PATCH] add quaternion::invert --- panda/src/linmath/lquaternion_src.I | 131 ++++++++++++++++++-------- panda/src/linmath/lquaternion_src.cxx | 12 +-- panda/src/linmath/lquaternion_src.h | 5 + 3 files changed, 105 insertions(+), 43 deletions(-) diff --git a/panda/src/linmath/lquaternion_src.I b/panda/src/linmath/lquaternion_src.I index cdad0b3e3d..80768c5f31 100644 --- a/panda/src/linmath/lquaternion_src.I +++ b/panda/src/linmath/lquaternion_src.I @@ -17,7 +17,7 @@ //////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Default Constructor +// Function: LQuaternion::Default Constructor // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -26,7 +26,7 @@ FLOATNAME(LQuaternion)() { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Copy Constructor +// Function: LQuaternion::Copy Constructor // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -37,7 +37,7 @@ FLOATNAME(LQuaternion)(const FLOATNAME(LQuaternion) &c) : } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Constructor +// Function: LQuaternion::Constructor // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -47,7 +47,7 @@ FLOATNAME(LQuaternion)(FLOATTYPE r, FLOATTYPE i, FLOATTYPE j, FLOATTYPE k) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::multiply +// Function: LQuaternion::multiply // Access: protected // Description: actual multiply call (non virtual) //////////////////////////////////////////////////////////////////// @@ -62,7 +62,7 @@ multiply(const FLOATNAME(LQuaternion)& rhs) const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Multiply Operator +// Function: LQuaternion::Multiply Operator // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -72,7 +72,7 @@ operator *(const FLOATNAME(LQuaternion)& c) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Multiply Assignment Operator +// Function: LQuaternion::Multiply Assignment Operator // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -83,7 +83,7 @@ operator *=(const FLOATNAME(LQuaternion)& c) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Multiply Operator +// Function: LQuaternion::Multiply Operator // Access: public // Description: Quat * Matrix = matrix //////////////////////////////////////////////////////////////////// @@ -95,7 +95,7 @@ operator *(const FLOATNAME(LMatrix3) &m) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::Multiply Operator +// Function: LQuaternion::Multiply Operator // Access: public // Description: Quat * Matrix = matrix //////////////////////////////////////////////////////////////////// @@ -114,7 +114,7 @@ operator *(const FLOATNAME(LMatrix4) &m) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::almost_equal +// Function: LQuaternion::almost_equal // Access: public // Description: Returns true if two quaternions are memberwise equal // within a specified tolerance. @@ -128,7 +128,7 @@ almost_equal(const FLOATNAME(LQuaternion)& c, FLOATTYPE threshold) const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::almost_equal +// Function: LQuaternion::almost_equal // Access: public // Description: Returns true if two quaternions are memberwise equal // within a default tolerance based on the numeric type. @@ -139,7 +139,7 @@ almost_equal(const FLOATNAME(LQuaternion)& c) const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::output +// Function: LQuaternion::output // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -152,7 +152,17 @@ output(ostream& os) const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::get_r +// Function: LQuaternion::set_from_matrix +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH void FLOATNAME(LQuaternion):: +set_from_matrix(const FLOATNAME(LMatrix4) &m) { + set_from_matrix(m.get_upper_3()); +} + +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::get_r // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -162,7 +172,7 @@ get_r() const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::get_i +// Function: LQuaternion::get_i // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -172,7 +182,7 @@ get_i() const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::get_j +// Function: LQuaternion::get_j // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -182,7 +192,7 @@ get_j() const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::get_k +// Function: LQuaternion::get_k // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -192,7 +202,7 @@ get_k() const { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::set_r +// Function: LQuaternion::set_r // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -202,7 +212,7 @@ set_r(FLOATTYPE r) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::set_i +// Function: LQuaternion::set_i // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -212,7 +222,7 @@ set_i(FLOATTYPE i) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::set_j +// Function: LQuaternion::set_j // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -222,7 +232,7 @@ set_j(FLOATTYPE j) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::set_k +// Function: LQuaternion::set_k // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -232,7 +242,7 @@ set_k(FLOATTYPE k) { } //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::normalize +// Function: LQuaternion::normalize // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -251,13 +261,71 @@ normalize() { } //////////////////////////////////////////////////////////////////// -// Function: set_from_matrix -// Access: public -// Description: +// Function: LQuaternion::invert_from +// Access: Public +// Description: Computes the inverse of the other quat, and stores +// the result in this quat. This is a fully general +// operation and makes no assumptions about the type of +// transform represented by the quat. +// +// The other quat must be a different object than this +// quat. However, if you need to invert a quat in +// place, see invert_in_place. +// +// The return value is true if the quat was +// successfully inverted, false if there was a +// singularity. //////////////////////////////////////////////////////////////////// -INLINE_LINMATH void FLOATNAME(LQuaternion):: -set_from_matrix(const FLOATNAME(LMatrix4) &m) { - set_from_matrix(m.get_upper_3()); +INLINE_LINMATH bool FLOATNAME(LQuaternion):: +invert_from(const FLOATNAME(LQuaternion) &other) { + FLOATTYPE norm = 1.0f / (other.dot(other)); + set(other[0] * norm, + -other[1] * norm, + -other[2] * norm, + -other[3] * norm); + + return true; +} + +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::invert_in_place +// Access: Public +// Description: Inverts the current quat. Returns true if the +// inverse is successful, false if the quat was +// singular. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH bool FLOATNAME(LQuaternion):: +invert_in_place() { + FLOATTYPE norm = 1.0f / ((*this).dot(*this)); + set((*this)[0] * norm, + -(*this)[1] * norm, + -(*this)[2] * norm, + -(*this)[3] * norm); + + return true; +} + + +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::ident_quat +// Access: Public, Static +// Description: Returns an identity quaternion. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH const FLOATNAME(LQuaternion) &FLOATNAME(LQuaternion):: +ident_quat() { + return _ident_quat; +} + +//////////////////////////////////////////////////////////////////// +// Function: invert +// Description: Inverts the given quat and returns it. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH FLOATNAME(LQuaternion) +invert(const FLOATNAME(LQuaternion) &a) { + FLOATNAME(LQuaternion) result; + bool nonsingular = result.invert_from(a); + nassertr(nonsingular, FLOATNAME(LQuaternion)::ident_quat()); + return result; } //////////////////////////////////////////////////////////////////// @@ -293,14 +361,3 @@ INLINE_LINMATH FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m, return q_matrix; } - - -//////////////////////////////////////////////////////////////////// -// Function: LQuaternion::ident_quat -// Access: Public, Static -// Description: Returns an identity quaternion. -//////////////////////////////////////////////////////////////////// -INLINE_LINMATH const FLOATNAME(LQuaternion) &FLOATNAME(LQuaternion):: -ident_quat() { - return _ident_quat; -} diff --git a/panda/src/linmath/lquaternion_src.cxx b/panda/src/linmath/lquaternion_src.cxx index b0aa454312..a52cf15595 100644 --- a/panda/src/linmath/lquaternion_src.cxx +++ b/panda/src/linmath/lquaternion_src.cxx @@ -22,7 +22,7 @@ const FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::_ident_quat = FLOATNAME(LQuaternion)(1.0f, 0.0f, 0.0f, 0.0f); //////////////////////////////////////////////////////////////////// -// Function: FLOATNAME(LQuaternion)::pure_imaginary_quat +// Function: LQuaternion::pure_imaginary_quat // Access: public // Description: //////////////////////////////////////////////////////////////////// @@ -32,7 +32,7 @@ pure_imaginary(const FLOATNAME(LVector3) &v) { } //////////////////////////////////////////////////////////////////// -// Function: extract_to_matrix (LMatrix3) +// Function: LQuaternion::extract_to_matrix (LMatrix3) // Access: Public // Description: Based on the quat lib from VRPN. //////////////////////////////////////////////////////////////////// @@ -53,7 +53,7 @@ extract_to_matrix(FLOATNAME(LMatrix3) &m) const { } //////////////////////////////////////////////////////////////////// -// Function: extract_to_matrix (LMatrix4) +// Function: LQuaternion::extract_to_matrix (LMatrix4) // Access: Public // Description: Based on the quat lib from VRPN. //////////////////////////////////////////////////////////////////// @@ -75,7 +75,7 @@ extract_to_matrix(FLOATNAME(LMatrix4) &m) const { } //////////////////////////////////////////////////////////////////// -// Function: set_hpr +// Function: LQuaternion::set_hpr // Access: public // Description: Sets the quaternion as the unit quaternion that // is equivalent to these Euler angles. @@ -105,7 +105,7 @@ set_hpr(const FLOATNAME(LVecBase3) &hpr) { } //////////////////////////////////////////////////////////////////// -// Function: get_hpr +// Function: LQuaternion::get_hpr // Access: public // Description: Extracts the equivalent Euler angles from the unit // quaternion. @@ -156,7 +156,7 @@ get_hpr() const { } //////////////////////////////////////////////////////////////////// -// Function: set_from_matrix +// Function: LQuaternion::set_from_matrix // Access: public // Description: Sets the quaternion according to the rotation // represented by the matrix. Originally we tried an diff --git a/panda/src/linmath/lquaternion_src.h b/panda/src/linmath/lquaternion_src.h index 28b481ff2c..29d2efef67 100644 --- a/panda/src/linmath/lquaternion_src.h +++ b/panda/src/linmath/lquaternion_src.h @@ -63,6 +63,9 @@ PUBLISHED: INLINE_LINMATH bool normalize(); + INLINE_LINMATH bool invert_from(const FLOATNAME(LQuaternion) &other); + INLINE_LINMATH bool invert_in_place(); + INLINE_LINMATH static const FLOATNAME(LQuaternion) &ident_quat(); private: @@ -83,6 +86,8 @@ INLINE_LINMATH ostream& operator<<(ostream& os, const FLOATNAME(LQuaternion)& q) return os; } +INLINE_LINMATH FLOATNAME(LQuaternion) invert(const FLOATNAME(LQuaternion) &a); + BEGIN_PUBLISH INLINE_LINMATH FLOATNAME(LMatrix3) operator * (const FLOATNAME(LMatrix3) &m, const FLOATNAME(LQuaternion) &q);