changes from drose for rotation by hpr

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);
     }
   }
 }

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;
 }
 

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 &copy);
 
 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() {

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;
 }

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;
 }
 

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 &copy);
   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() {

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;

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;
 

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;
 }

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();
 }
 
 ////////////////////////////////////////////////////////////////////

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;