From 1343e40e31b79905ac41679a2b5df345013380c6 Mon Sep 17 00:00:00 2001 From: David Rose Date: Tue, 5 Sep 2006 06:39:27 +0000 Subject: [PATCH] fix problem with non-normalized quat --- panda/src/pgraph/transformState.I | 42 +++++++++++++++++++++++++++-- panda/src/pgraph/transformState.cxx | 28 +++++++++++++------ panda/src/pgraph/transformState.h | 6 ++++- 3 files changed, 65 insertions(+), 11 deletions(-) diff --git a/panda/src/pgraph/transformState.I b/panda/src/pgraph/transformState.I index 9799c6cc42..8b69b41f9e 100644 --- a/panda/src/pgraph/transformState.I +++ b/panda/src/pgraph/transformState.I @@ -504,8 +504,15 @@ get_hpr() const { // Function: TransformState::get_quat // Access: Published // Description: Returns the rotation component of the transform as a -// quaternion. It is an error to call this if -// has_components() returned false. +// quaternion. The return value will be normalized if a +// normalized quaternion was given to the constructor +// (or if the quaternion was computed implicitly); it +// will be non-normalized if a non-normalized quaternion +// was given to the constructor. See also +// get_norm_quat(). +// +// It is an error to call this if has_components() +// returned false. //////////////////////////////////////////////////////////////////// INLINE const LQuaternionf &TransformState:: get_quat() const { @@ -514,6 +521,22 @@ get_quat() const { return _quat; } +//////////////////////////////////////////////////////////////////// +// Function: TransformState::get_norm_quat +// Access: Published +// Description: Returns the rotation component of the transform as a +// quaternion. Unlike the result of get_quat(), the +// return value of this method is guaranteed to be +// normalized. It is an error to call this if +// has_components() returned false. +//////////////////////////////////////////////////////////////////// +INLINE const LQuaternionf &TransformState:: +get_norm_quat() const { + check_norm_quat(); + nassertr(!is_invalid(), _norm_quat); + return _norm_quat; +} + //////////////////////////////////////////////////////////////////// // Function: TransformState::get_scale // Access: Published @@ -835,6 +858,21 @@ check_quat() const { } } +//////////////////////////////////////////////////////////////////// +// Function: TransformState::check_norm_quat +// Access: Private +// Description: Ensures that we know the normalized quat of the transform +// (or that we know they cannot be derived). +//////////////////////////////////////////////////////////////////// +INLINE void TransformState:: +check_norm_quat() const { + // This pretends to be a const function, even though it's not, + // because it only updates a transparent cache value. + if ((_flags & F_norm_quat_known) == 0) { + ((TransformState *)this)->calc_norm_quat(); + } +} + //////////////////////////////////////////////////////////////////// // Function: TransformState::check_mat // Access: Private diff --git a/panda/src/pgraph/transformState.cxx b/panda/src/pgraph/transformState.cxx index d720923601..3d3014f390 100644 --- a/panda/src/pgraph/transformState.cxx +++ b/panda/src/pgraph/transformState.cxx @@ -1327,7 +1327,7 @@ do_compose(const TransformState *other) const { // Do a 2-d compose. LVecBase2f pos = get_pos2d(); float rotate = get_rotate2d(); - LQuaternionf quat = get_quat(); + LQuaternionf quat = get_norm_quat(); float scale = get_uniform_scale(); LPoint3f op = quat.xform(other->get_pos()); @@ -1341,12 +1341,11 @@ do_compose(const TransformState *other) const { } else { // A normal 3-d compose. LVecBase3f pos = get_pos(); - LQuaternionf quat = get_quat(); + LQuaternionf quat = get_norm_quat(); float scale = get_uniform_scale(); pos += quat.xform(other->get_pos()) * scale; - quat = other->get_quat() * quat; - quat.normalize(); + quat = other->get_norm_quat() * quat; LVecBase3f new_scale = other->get_scale() * scale; result = make_pos_quat_scale(pos, quat, new_scale); @@ -1406,7 +1405,7 @@ do_invert_compose(const TransformState *other) const { // Do a 2-d invert compose. LVecBase2f pos = get_pos2d(); float rotate = get_rotate2d(); - LQuaternionf quat = get_quat(); + LQuaternionf quat = get_norm_quat(); float scale = get_uniform_scale(); // First, invert our own transform. @@ -1435,7 +1434,7 @@ do_invert_compose(const TransformState *other) const { } else { // Do a normal, 3-d invert compose. LVecBase3f pos = get_pos(); - LQuaternionf quat = get_quat(); + LQuaternionf quat = get_norm_quat(); float scale = get_uniform_scale(); // First, invert our own transform. @@ -1451,8 +1450,7 @@ do_invert_compose(const TransformState *other) const { // Now compose the inverted transform with the other transform. if (!other->is_identity()) { pos += quat.xform(other->get_pos()) * scale; - quat = other->get_quat() * quat; - quat.normalize(); + quat = other->get_norm_quat() * quat; new_scale = other->get_scale() * scale; } @@ -1873,6 +1871,20 @@ calc_quat() { } } +//////////////////////////////////////////////////////////////////// +// Function: TransformState::calc_norm_quat +// Access: Private +// Description: Derives the normalized quat from the quat. +//////////////////////////////////////////////////////////////////// +void TransformState:: +calc_norm_quat() { + LQuaternionf quat = get_quat(); + MutexHolder holder(_lock); + _norm_quat = quat; + _norm_quat.normalize(); + _flags |= F_norm_quat_known; +} + //////////////////////////////////////////////////////////////////// // Function: TransformState::do_calc_mat // Access: Private diff --git a/panda/src/pgraph/transformState.h b/panda/src/pgraph/transformState.h index d96d89d392..8032cec370 100644 --- a/panda/src/pgraph/transformState.h +++ b/panda/src/pgraph/transformState.h @@ -142,6 +142,7 @@ PUBLISHED: INLINE const LPoint3f &get_pos() const; INLINE const LVecBase3f &get_hpr() const; INLINE const LQuaternionf &get_quat() const; + INLINE const LQuaternionf &get_norm_quat() const; INLINE const LVecBase3f &get_scale() const; INLINE float get_uniform_scale() const; INLINE const LVecBase3f &get_shear() const; @@ -271,6 +272,7 @@ private: INLINE void check_components() const; INLINE void check_hpr() const; INLINE void check_quat() const; + INLINE void check_norm_quat() const; INLINE void check_mat() const; INLINE void calc_hash(); void do_calc_hash(); @@ -280,6 +282,7 @@ private: INLINE void calc_hpr(); void do_calc_hpr(); void calc_quat(); + void calc_norm_quat(); INLINE void calc_mat(); void do_calc_mat(); @@ -311,10 +314,11 @@ private: F_is_destructing = 0x00008000, F_is_2d = 0x00010000, F_hash_known = 0x00020000, + F_norm_quat_known = 0x00040000, }; LPoint3f _pos; LVecBase3f _hpr, _scale, _shear; - LQuaternionf _quat; + LQuaternionf _quat, _norm_quat; LMatrix4f _mat; LMatrix4f *_inv_mat; size_t _hash;