much better slerp algorithm

direct/src/interval/cLerpNodePathInterval.I

@@ -83,7 +83,7 @@ set_end_pos(const LVecBase3f &pos) {
 INLINE void CLerpNodePathInterval::
 set_start_hpr(const LVecBase3f &hpr) {
   _start_hpr = hpr;
-  _flags |= F_start_hpr;
+  _flags = (_flags & ~(F_slerp_setup | F_start_quat)) | F_start_hpr;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -135,7 +135,7 @@ set_end_hpr(const LQuaternionf &quat) {
 INLINE void CLerpNodePathInterval::
 set_start_quat(const LQuaternionf &quat) {
   _start_quat = quat;
-  _flags |= F_start_quat;
+  _flags = (_flags & ~(F_slerp_setup | F_start_hpr)) | F_start_quat;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -158,7 +158,7 @@ set_start_quat(const LQuaternionf &quat) {
 INLINE void CLerpNodePathInterval::
 set_end_quat(const LVecBase3f &hpr) {
   _end_quat.set_hpr(hpr);
-  _flags = (_flags & ~F_end_hpr) | F_end_quat;
+  _flags = (_flags & ~(F_slerp_setup | F_end_hpr)) | F_end_quat;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -175,7 +175,7 @@ set_end_quat(const LVecBase3f &hpr) {
 INLINE void CLerpNodePathInterval::
 set_end_quat(const LQuaternionf &quat) {
   _end_quat = quat;
-  _flags = (_flags & ~F_end_hpr) | F_end_quat;
+  _flags = (_flags & ~(F_slerp_setup | F_end_hpr)) | F_end_quat;
 }
 
 ////////////////////////////////////////////////////////////////////

direct/src/interval/cLerpNodePathInterval.cxx

@@ -66,7 +66,8 @@ CLerpNodePathInterval(const string &name, double duration,
   CLerpInterval(name, duration, blend_type),
   _node(node),
   _other(other),
-  _flags(0)
+  _flags(0),
+  _slerp(NULL)
 {
   if (bake_in_start) {
     _flags |= F_bake_in_start;
@@ -181,23 +182,35 @@ priv_step(double t) {
       }
     }
     if ((_flags & F_end_quat) != 0) {
-      if ((_flags & F_start_quat) != 0) {
-        lerp_value(quat, d, _start_quat, _end_quat);
+      if ((_flags & F_slerp_setup) == 0) {
+        if ((_flags & F_start_quat) != 0) {
+          setup_slerp();
 
-      } else if ((_flags & F_start_hpr) != 0) {
-        _start_quat.set_hpr(_start_hpr);
-        _flags |= F_start_quat;
-        lerp_value(quat, d, _start_quat, _end_quat);
+        } else if ((_flags & F_start_hpr) != 0) {
+          _start_quat.set_hpr(_start_hpr);
+          _flags |= F_start_quat;
+          setup_slerp();
 
-      } else if ((_flags & F_bake_in_start) != 0) {
-        set_start_quat(transform->get_quat());
-        lerp_value(quat, d, _start_quat, _end_quat);
+        } else if ((_flags & F_bake_in_start) != 0) {
+          set_start_quat(transform->get_quat());
+          setup_slerp();
 
-      } else {
-        quat = transform->get_quat();
-        lerp_value_from_prev(quat, d, _prev_d, quat, _end_quat);
+        } else {
+          if (_prev_d == 1.0) {
+            _start_quat = _end_quat;
+          } else {
+            LQuaternionf prev_value = transform->get_quat();
+            _start_quat = (prev_value - _prev_d * _end_quat) / (1.0 - _prev_d);
+          }
+          setup_slerp();
+
+          // In this case, clear the slerp_setup flag because we need
+          // to re-setup the slerp each time.
+          _flags &= ~F_slerp_setup;
+        }
       }
-      quat.normalize();
+      nassertv(_slerp != NULL);
+      (this->*_slerp)(quat, d);
     }
     if ((_flags & F_end_scale) != 0) {
       if ((_flags & F_start_scale) != 0) {
@@ -558,3 +571,118 @@ output(ostream &out) const {
 
   out << " dur " << get_duration();
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: CLerpNodePathInterval::setup_slerp
+//       Access: Private
+//  Description: Sets up a spherical lerp from _start_quat to
+//               _end_quat.  This precomputes some important values
+//               (like the angle between the quaternions) and sets up
+//               the _slerp method pointer.
+////////////////////////////////////////////////////////////////////
+void CLerpNodePathInterval::
+setup_slerp() {
+  if (_start_quat.dot(_end_quat) < 0.0f) {
+    // Make sure both quaternions are on the same side.
+    _start_quat = -_start_quat;
+  }
+
+  _slerp_angle = _start_quat.angle_rad(_end_quat);
+
+  if (_slerp_angle < 0.1f) {
+    // If the angle is small, use sin(angle)/angle as the denominator,
+    // to provide better behavior with small divisors.  This is Don
+    // Hatch's suggestion from http://www.hadron.org/~hatch/rightway.php .
+    _slerp_denom = csin_over_x(_slerp_angle);
+    _slerp = &CLerpNodePathInterval::slerp_angle_0;
+
+  } else if (_slerp_angle > 3.14) {
+    // If the angle is close to 180 degrees, the lerp is ambiguous.
+    // which plane should we lerp through?  Better pick an
+    // intermediate point to resolve the ambiguity up front.
+
+    // We pick it by choosing a linear point between the quats and
+    // normalizing it out; this will give an arbitrary point when the
+    // angle is exactly 180, but will behave sanely as the angle
+    // approaches 180.
+    _slerp_c = (_start_quat + _end_quat);
+    _slerp_c.normalize();
+    _slerp_angle = _end_quat.angle_rad(_slerp_c);
+    _slerp_denom = csin(_slerp_angle);
+
+    _slerp = &CLerpNodePathInterval::slerp_angle_180;
+    
+  } else {
+    // Otherwise, use the original Shoemake equation for spherical
+    // lerp.
+    _slerp_denom = csin(_slerp_angle);
+    _slerp = &CLerpNodePathInterval::slerp_basic;
+  }
+
+  _flags |= F_slerp_setup;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CLerpNodePathInterval::slerp_basic
+//       Access: Private
+//  Description: Implements Ken Shoemake's spherical lerp equation.
+//               This is appropriate when the angle between the
+//               quaternions is not near one extreme or the other.
+////////////////////////////////////////////////////////////////////
+void CLerpNodePathInterval::
+slerp_basic(LQuaternionf &result, float t) const {
+  float ti = 1.0f - t;
+  float ta = t * _slerp_angle;
+  float tia = ti * _slerp_angle;
+
+  result = (csin(tia) * _start_quat + csin(ta) * _end_quat) / _slerp_denom;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CLerpNodePathInterval::slerp_angle_0
+//       Access: Private
+//  Description: Implements Don Hatch's modified spherical lerp
+//               equation, appropriate for when the angle between the
+//               quaternions approaches zero.
+////////////////////////////////////////////////////////////////////
+void CLerpNodePathInterval::
+slerp_angle_0(LQuaternionf &result, float t) const {
+  float ti = 1.0f - t;
+  float ta = t * _slerp_angle;
+  float tia = ti * _slerp_angle;
+
+  result = (csin_over_x(tia) * ti * _start_quat + csin_over_x(ta) * t * _end_quat) / _slerp_denom;
+}
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CLerpNodePathInterval::slerp_angle_180
+//       Access: Private
+//  Description: Implements a two-part slerp, to an intermediate point
+//               and out again, appropriate for when the angle between
+//               the quaternions approaches 180 degrees.
+////////////////////////////////////////////////////////////////////
+void CLerpNodePathInterval::
+slerp_angle_180(LQuaternionf &result, float t) const {
+  if (t < 0.5) {
+    // The first half of the lerp: _start_quat to _slerp_c.
+
+    t *= 2.0f;
+
+    float ti = 1.0f - t;
+    float ta = t * _slerp_angle;
+    float tia = ti * _slerp_angle;
+    
+    result = (csin(tia) * _start_quat + csin(ta) * _slerp_c) / _slerp_denom;
+
+  } else {
+    // The second half of the lerp: _slerp_c to _end_quat.
+    t = t * 2.0f - 1.0f;
+
+    float ti = 1.0f - t;
+    float ta = t * _slerp_angle;
+    float tia = ti * _slerp_angle;
+    
+    result = (csin(tia) * _slerp_c + csin(ta) * _end_quat) / _slerp_denom;
+  }
+}

direct/src/interval/cLerpNodePathInterval.h

@@ -66,28 +66,32 @@ PUBLISHED:
   virtual void output(ostream &out) const;
 
 private:
+  void setup_slerp();
+
   NodePath _node;
   NodePath _other;
 
   enum Flags {
-    F_end_pos            = 0x0001,
-    F_end_hpr            = 0x0002,
-    F_end_quat           = 0x0004,
-    F_end_scale          = 0x0008,
-    F_end_color          = 0x0010,
-    F_end_color_scale    = 0x0020,
-    F_end_shear          = 0x0040,
-
-    F_start_pos          = 0x0080,
-    F_start_hpr          = 0x0100,
-    F_start_quat         = 0x0200,
-    F_start_scale        = 0x0400,
-    F_start_color        = 0x0800,
-    F_start_color_scale  = 0x1000,
-    F_start_shear        = 0x2000,
-
-    F_fluid              = 0x4000,
-    F_bake_in_start      = 0x8000,
+    F_end_pos            = 0x000001,
+    F_end_hpr            = 0x000002,
+    F_end_quat           = 0x000004,
+    F_end_scale          = 0x000008,
+    F_end_color          = 0x000010,
+    F_end_color_scale    = 0x000020,
+    F_end_shear          = 0x000040,
+
+    F_start_pos          = 0x000080,
+    F_start_hpr          = 0x000100,
+    F_start_quat         = 0x000200,
+    F_start_scale        = 0x000400,
+    F_start_color        = 0x000800,
+    F_start_color_scale  = 0x001000,
+    F_start_shear        = 0x002000,
+
+    F_fluid              = 0x004000,
+    F_bake_in_start      = 0x008000,
+    
+    F_slerp_setup        = 0x010000,
   };
   
   unsigned int _flags;
@@ -100,6 +104,16 @@ private:
   LVecBase4f _start_color_scale, _end_color_scale;
 
   double _prev_d;
+  float _slerp_angle;
+  float _slerp_denom;
+  LQuaternionf _slerp_c;
+
+  void slerp_basic(LQuaternionf &result, float t) const;
+  void slerp_angle_0(LQuaternionf &result, float t) const;
+  void slerp_angle_180(LQuaternionf &result, float t) const;
+
+  // Define a pointer to one of the above three methods.
+  void (CLerpNodePathInterval::*_slerp)(LQuaternionf &result, float t) const;
   
 public:
   static TypeHandle get_class_type() {

direct/src/showbase/ShowBase.py

@@ -749,7 +749,6 @@ class ShowBase(DirectObject.DirectObject):
         lens.setAspectRatio(aspectRatio)
             
         camNode.setLens(lens)
-        camNode.setScene(scene)
 
         # self.camera is the parent node of all cameras: a node that
         # we can move around to move all cameras as a group.
@@ -791,7 +790,6 @@ class ShowBase(DirectObject.DirectObject):
         lens.setFilmOffset((right + left) * 0.5, (top + bottom) * 0.5)
         lens.setNearFar(-1000, 1000)
         cam2dNode.setLens(lens)
-        cam2dNode.setScene(self.render2d)
 
         # self.camera2d is the analog of self.camera, although it's
         # not as clear how useful it is.
@@ -825,7 +823,6 @@ class ShowBase(DirectObject.DirectObject):
         lens.setFilmOffset((right + left) * 0.5, (top + bottom) * 0.5)
         lens.setNearFar(-1000, 1000)
         cam2dNode.setLens(lens)
-        cam2dNode.setScene(self.render2dp)
 
         # self.camera2d is the analog of self.camera, although it's
         # not as clear how useful it is.