From 2b3aa5cf319e52b10191bb65e4cb68ff95a0d678 Mon Sep 17 00:00:00 2001
From: Dave Schuyler <nthparameter@gmail.com>
Date: Sat, 16 Oct 2004 00:38:56 +0000
Subject: [PATCH] minor changes

---
 panda/src/linmath/lquaternion_src.I          | 10 ++++------
 panda/src/physics/angularEulerIntegrator.cxx |  8 ++++----
 panda/src/physics/forceNode.cxx              | 13 ++++++++-----
 3 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/panda/src/linmath/lquaternion_src.I b/panda/src/linmath/lquaternion_src.I
index 8f3abc0a28..6852e96a56 100644
--- a/panda/src/linmath/lquaternion_src.I
+++ b/panda/src/linmath/lquaternion_src.I
@@ -324,15 +324,13 @@ set_k(FLOATTYPE k) {
 ////////////////////////////////////////////////////////////////////
 INLINE_LINMATH bool FLOATNAME(LQuaternion)::
 normalize() {
-  FLOATTYPE l2 = (*this).dot(*this);
-  if (l2 == (FLOATTYPE)0.0f) {
+  FLOATTYPE length_squared = (*this).dot(*this);
+  if (length_squared == (FLOATTYPE)0.0f) {
     set(0.0f, 0.0f, 0.0f, 0.0f);
     return false;
-
-  } else if (!IS_THRESHOLD_EQUAL(l2, 1.0f, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) {
-    (*this) /= csqrt(l2);
+  } else if (!IS_THRESHOLD_EQUAL(length_squared, 1.0f, NEARLY_ZERO(FLOATTYPE))) {
+    (*this) /= csqrt(length_squared);
   }
-
   return true;
 }
 
diff --git a/panda/src/physics/angularEulerIntegrator.cxx b/panda/src/physics/angularEulerIntegrator.cxx
index 34f231dfc7..ce2cffb872 100644
--- a/panda/src/physics/angularEulerIntegrator.cxx
+++ b/panda/src/physics/angularEulerIntegrator.cxx
@@ -109,6 +109,7 @@ child_integrate(Physical *physical,
 
       // tally it into the accum vector, applying the inertial tensor.
       accum_vec += f;
+      cerr<<"global accum_vec"<<accum_vec<<"\n";
     }
 
     // local forces
@@ -129,6 +130,7 @@ child_integrate(Physical *physical,
 
       // tally it into the accum vectors
       accum_vec += f;
+      cerr<<"local accum_vec"<<accum_vec<<"\n";
     }
     assert(index == matrices.size());
 
@@ -136,10 +138,12 @@ child_integrate(Physical *physical,
     // 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();
+    cerr<<"tensor applied accum_vec"<<accum_vec<<"\n";
 
     // derive this into the angular velocity vector.
     LVector3f rot_vec = current_object->get_rotation();
     rot_vec += accum_vec * dt;
+    cerr<<"accum_vec * dt"<<(accum_vec * dt)<<"\n";
 
     // 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
@@ -152,14 +156,10 @@ child_integrate(Physical *physical,
       LVector3f normalized_rot_vec = rot_vec;
       normalized_rot_vec *= 1.0f / len;
       LRotationf rot_quat = LRotationf(normalized_rot_vec, len);
-      assert(!(rot_quat[0]==0.0f && rot_quat[1]==0.0f && rot_quat[2]==0.0f && rot_quat[3]==0.0f));
 
       LOrientationf old_orientation = current_object->get_orientation();
-      assert(!(old_orientation[0]==0.0f && old_orientation[1]==0.0f && old_orientation[2]==0.0f && old_orientation[3]==0.0f));
       LOrientationf new_orientation = old_orientation * rot_quat;
-      assert(!(new_orientation[0]==0.0f && new_orientation[1]==0.0f && new_orientation[2]==0.0f && new_orientation[3]==0.0f));
       new_orientation.normalize();
-      assert(!(new_orientation[0]==0.0f && new_orientation[1]==0.0f && new_orientation[2]==0.0f && new_orientation[3]==0.0f));
 
       // and write the results back.
       current_object->set_orientation(new_orientation);
diff --git a/panda/src/physics/forceNode.cxx b/panda/src/physics/forceNode.cxx
index 24c22e9724..02015a3df9 100644
--- a/panda/src/physics/forceNode.cxx
+++ b/panda/src/physics/forceNode.cxx
@@ -131,11 +131,12 @@ output(ostream &out) const {
 void ForceNode::
 write_forces(ostream &out, unsigned int indent) const {
   #ifndef NDEBUG //[
-  out.width(indent); out<<""<<"_forces\n";
+  out.width(indent); out<<""<<"_forces ("<<_forces.size()<<" forces)"<<"\n";
   for (ForceVector::const_iterator i=_forces.begin();
        i != _forces.end();
        ++i) {
-    (*i)->write(out, indent+2);
+    out.width(indent+2); out<<""; out<<"(id "<<&(*i)<<" "<<(*i)->is_linear()<<")\n";
+    //#*#(*i)->write(out, indent+2);
   }
   #endif //] NDEBUG
 }
@@ -149,8 +150,10 @@ write_forces(ostream &out, unsigned int indent) const {
 void ForceNode::
 write(ostream &out, unsigned int indent) const {
   #ifndef NDEBUG //[
-  out.width(indent); out<<""; out<<"ForceNode\n";
-  write_forces(out, indent+2);
-  //PandaNode::write(out, indent+2);
+  out.width(indent); out<<""; out<<"ForceNode (id "<<this<<") ";
+  //#*#PandaNode::output(out);
+  out<<"\n";
+  //#*#write_forces(out, indent+2);
+  PandaNode::write(out, indent+4);
   #endif //] NDEBUG
 }