diff --git a/panda/src/physics/angularEulerIntegrator.cxx b/panda/src/physics/angularEulerIntegrator.cxx index 959f9b375b..3530a03247 100644 --- a/panda/src/physics/angularEulerIntegrator.cxx +++ b/panda/src/physics/angularEulerIntegrator.cxx @@ -82,13 +82,13 @@ child_integrate(Physical *physical, continue; } - LVector3f accum_vec(0, 0, 0); + LRotationf accum_quat(0, 0, 0, 0); // set up the traversal stuff. ForceNode *force_node; AngularForceVector::const_iterator f_cur; - LVector3f f; + LRotationf f; // global forces f_cur = forces.begin(); @@ -105,10 +105,10 @@ child_integrate(Physical *physical, // now we go from force space to our object's space. assert(index >= 0 && index < matrices.size()); - f = cur_force->get_vector(current_object) * matrices[index++]; + f = matrices[index++] * cur_force->get_quat(current_object); // tally it into the accum vector, applying the inertial tensor. - accum_vec += f; + accum_quat += f; } // local forces @@ -125,39 +125,30 @@ child_integrate(Physical *physical, // go from force space to object space assert(index >= 0 && index < matrices.size()); - f = cur_force->get_vector(current_object) * matrices[index++]; + f = matrices[index++] * cur_force->get_quat(current_object); // tally it into the accum vectors - accum_vec += f; + accum_quat += f; } assert(index == matrices.size()); // apply the accumulated torque vector to the object's inertial tensor. // this matrix represents how much force the object 'wants' applied to it // in any direction, among other things. - accum_vec = accum_vec * current_object->get_inertial_tensor(); + accum_quat = current_object->get_inertial_tensor() * accum_quat; // derive this into the angular velocity vector. - LVector3f rot_vec = current_object->get_rotation(); - rot_vec += accum_vec * dt; - - // here's the trick. we've been accumulating these forces as vectors - // and treating them as vectors, but now we're going to treat them as pure - // imaginary quaternions where r = 0. This vector now represents the - // imaginary vector formed by (i, j, k). - float len = rot_vec.length(); - if (len) { - LVector3f normalized_rot_vec = rot_vec; - normalized_rot_vec *= 1.0f / len; - LRotationf rot_quat = LRotationf(normalized_rot_vec, len); + LRotationf rot_quat = current_object->get_rotation(); + rot_quat += (LVecBase4f(accum_quat) * dt); + if (rot_quat.normalize()) { LOrientationf old_orientation = current_object->get_orientation(); LOrientationf new_orientation = old_orientation * rot_quat; new_orientation.normalize(); // and write the results back. current_object->set_orientation(new_orientation); - current_object->set_rotation(rot_vec); + current_object->set_rotation(rot_quat); } } } diff --git a/panda/src/physics/angularForce.cxx b/panda/src/physics/angularForce.cxx index a2be260422..18b7cf4996 100644 --- a/panda/src/physics/angularForce.cxx +++ b/panda/src/physics/angularForce.cxx @@ -50,13 +50,13 @@ AngularForce:: } //////////////////////////////////////////////////////////////////// -// Function : get_vector +// Function : get_quat // Access : public // Description : access query //////////////////////////////////////////////////////////////////// -LVector3f AngularForce:: -get_vector(const PhysicsObject *po) { - LVector3f v = get_child_vector(po); +LRotationf AngularForce:: +get_quat(const PhysicsObject *po) { + LRotationf v = get_child_quat(po); return v; } diff --git a/panda/src/physics/angularForce.h b/panda/src/physics/angularForce.h index 7ed415d45e..2c0948bc26 100644 --- a/panda/src/physics/angularForce.h +++ b/panda/src/physics/angularForce.h @@ -30,7 +30,7 @@ PUBLISHED: virtual ~AngularForce(); virtual AngularForce *make_copy() const = 0; - LVector3f get_vector(const PhysicsObject *po); + LRotationf get_quat(const PhysicsObject *po); virtual bool is_linear() const; virtual void output(ostream &out) const; @@ -41,7 +41,7 @@ protected: AngularForce(const AngularForce ©); private: - virtual LVector3f get_child_vector(const PhysicsObject *po) = 0; + virtual LRotationf get_child_quat(const PhysicsObject *po) = 0; public: static TypeHandle get_class_type() { diff --git a/panda/src/physics/angularVectorForce.I b/panda/src/physics/angularVectorForce.I index 17bb8079ad..a4118dd311 100644 --- a/panda/src/physics/angularVectorForce.I +++ b/panda/src/physics/angularVectorForce.I @@ -17,28 +17,28 @@ //////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////// -// Function : set_vector +// Function : set_quat // Access : public //////////////////////////////////////////////////////////////////// INLINE void AngularVectorForce:: -set_vector(const LVector3f &v) { +set_quat(const LRotationf &v) { _fvec = v; } //////////////////////////////////////////////////////////////////// -// Function : set_vector +// Function : set_hpr // Access : public //////////////////////////////////////////////////////////////////// INLINE void AngularVectorForce:: -set_vector(float x, float y, float z) { - _fvec.set(x, y, z); +set_hpr(float h, float p, float r) { + _fvec.set_hpr(LVecBase3f(h, p, r)); } //////////////////////////////////////////////////////////////////// -// Function : get_local_vector +// Function : get_local_quat // Access : public //////////////////////////////////////////////////////////////////// -INLINE LVector3f AngularVectorForce:: -get_local_vector() const { +INLINE LRotationf AngularVectorForce:: +get_local_quat() const { return _fvec; } diff --git a/panda/src/physics/angularVectorForce.cxx b/panda/src/physics/angularVectorForce.cxx index 3346dcd1fd..1db1e2ff3c 100644 --- a/panda/src/physics/angularVectorForce.cxx +++ b/panda/src/physics/angularVectorForce.cxx @@ -26,7 +26,7 @@ TypeHandle AngularVectorForce::_type_handle; // Description : constructor //////////////////////////////////////////////////////////////////// AngularVectorForce:: -AngularVectorForce(const LVector3f &vec) : +AngularVectorForce(const LRotationf &vec) : AngularForce(), _fvec(vec) { } @@ -36,9 +36,9 @@ AngularVectorForce(const LVector3f &vec) : // Description : constructor //////////////////////////////////////////////////////////////////// AngularVectorForce:: -AngularVectorForce(float x, float y, float z) : +AngularVectorForce(float h, float p, float r) : AngularForce() { - _fvec.set(x, y, z); + _fvec.set_hpr(LVecBase3f(h, p, r)); } //////////////////////////////////////////////////////////////////// @@ -72,12 +72,12 @@ make_copy() const { } //////////////////////////////////////////////////////////////////// -// Function : get_child_vector +// Function : get_child_quat // Access : private, virtual // Description : query //////////////////////////////////////////////////////////////////// -LVector3f AngularVectorForce:: -get_child_vector(const PhysicsObject *) { +LRotationf AngularVectorForce:: +get_child_quat(const PhysicsObject *) { return _fvec; } diff --git a/panda/src/physics/angularVectorForce.h b/panda/src/physics/angularVectorForce.h index 1db54a3846..cc681c2a49 100644 --- a/panda/src/physics/angularVectorForce.h +++ b/panda/src/physics/angularVectorForce.h @@ -28,23 +28,23 @@ //////////////////////////////////////////////////////////////////// class EXPCL_PANDAPHYSICS AngularVectorForce : public AngularForce { PUBLISHED: - AngularVectorForce(const LVector3f& vec); - AngularVectorForce(float x = 0.0f, float y = 0.0f, float z = 0.0f); + AngularVectorForce(const LRotationf& quat); + AngularVectorForce(float h, float p, float r); AngularVectorForce(const AngularVectorForce ©); virtual ~AngularVectorForce(); - INLINE void set_vector(const LVector3f& v); - INLINE void set_vector(float x, float y, float z); - INLINE LVector3f get_local_vector() const; + INLINE void set_quat(const LRotationf& quat); + INLINE void set_hpr(float h, float p, float r); + INLINE LRotationf get_local_quat() const; virtual void output(ostream &out) const; virtual void write(ostream &out, unsigned int indent=0) const; private: - LVector3f _fvec; + LRotationf _fvec; virtual AngularForce *make_copy() const; - virtual LVector3f get_child_vector(const PhysicsObject *po); + virtual LRotationf get_child_quat(const PhysicsObject *po); public: static TypeHandle get_class_type() { diff --git a/panda/src/physics/baseForce.h b/panda/src/physics/baseForce.h index 13a1db1d1a..8900630cd9 100644 --- a/panda/src/physics/baseForce.h +++ b/panda/src/physics/baseForce.h @@ -42,8 +42,6 @@ PUBLISHED: INLINE ForceNode *get_force_node() const; - virtual LVector3f get_vector(const PhysicsObject *po) = 0; - virtual void output(ostream &out) const; virtual void write(ostream &out, unsigned int indent=0) const; @@ -55,8 +53,6 @@ private: ForceNode *_force_node; bool _active; - virtual LVector3f get_child_vector(const PhysicsObject *po) = 0; - public: static TypeHandle get_class_type() { return _type_handle; diff --git a/panda/src/physics/linearForce.h b/panda/src/physics/linearForce.h index 6b64637882..cad6b86f17 100644 --- a/panda/src/physics/linearForce.h +++ b/panda/src/physics/linearForce.h @@ -38,7 +38,7 @@ PUBLISHED: INLINE void set_vector_masks(bool x, bool y, bool z); - virtual LVector3f get_vector(const PhysicsObject *po); + LVector3f get_vector(const PhysicsObject *po); virtual LinearForce *make_copy() = 0; diff --git a/panda/src/physics/physicsObject.I b/panda/src/physics/physicsObject.I index 70451c89fc..9087b122f2 100644 --- a/panda/src/physics/physicsObject.I +++ b/panda/src/physics/physicsObject.I @@ -73,7 +73,7 @@ INLINE void PhysicsObject:: reset_orientation(const LOrientationf &orientation) { nassertv(!orientation.is_nan()); _orientation = orientation; - _rotation.set(0.0f, 0.0f, 0.0f); + _rotation = LRotationf::ident_quat(); } //////////////////////////////////////////////////////////////////// @@ -226,10 +226,10 @@ set_orientation(const LOrientationf &orientation) { //////////////////////////////////////////////////////////////////// // Function : set_rotation // Access : Public -// Description : set rotation on each axis per second. +// Description : set rotation as a quaternion delta per second. //////////////////////////////////////////////////////////////////// INLINE void PhysicsObject:: -set_rotation(const LVector3f &rotation) { +set_rotation(const LRotationf &rotation) { nassertv(!rotation.is_nan()); _rotation = rotation; } @@ -249,7 +249,7 @@ get_orientation() const { // Access : Public // Description : get rotation per second. //////////////////////////////////////////////////////////////////// -INLINE LVector3f PhysicsObject:: +INLINE LRotationf PhysicsObject:: get_rotation() const { return _rotation; } diff --git a/panda/src/physics/physicsObject.cxx b/panda/src/physics/physicsObject.cxx index a81df36d20..0cfa67e1a3 100644 --- a/panda/src/physics/physicsObject.cxx +++ b/panda/src/physics/physicsObject.cxx @@ -39,7 +39,7 @@ PhysicsObject() : _last_position = _position; _velocity.set(0.0f, 0.0f, 0.0f); _orientation.set(1.0 ,0.0f, 0.0f, 0.0f); - _rotation.set(0.0f, 0.0f, 0.0f); + _rotation = LRotationf::ident_quat(); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/physics/physicsObject.h b/panda/src/physics/physicsObject.h index 671f51a922..c242a246ec 100644 --- a/panda/src/physics/physicsObject.h +++ b/panda/src/physics/physicsObject.h @@ -75,8 +75,8 @@ PUBLISHED: INLINE void reset_orientation(const LOrientationf &orientation); - INLINE void set_rotation(const LVector3f &rotation); - INLINE LVector3f get_rotation() const; + INLINE void set_rotation(const LRotationf &rotation); + INLINE LRotationf get_rotation() const; virtual LMatrix4f get_inertial_tensor() const; virtual LMatrix4f get_lcs() const; @@ -102,7 +102,7 @@ private: // angular LOrientationf _orientation; - LVector3f _rotation; + LRotationf _rotation; float _terminal_velocity; float _mass;