working on adding reach

panda/src/collide/collisionHandlerFloor.I

@@ -41,6 +41,28 @@ get_offset() const {
   return _offset;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionHandlerFloor::set_reach
+//       Access: Public
+//  Description: Sets the reach to add to (or subtract from)
+//               the highest collision point
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionHandlerFloor::
+set_reach(float reach) {
+  _reach = reach;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionHandlerFloor::get_reach
+//       Access: Public
+//  Description: Returns the reach to add to (or subtract from)
+//               the highest collision point
+////////////////////////////////////////////////////////////////////
+INLINE float CollisionHandlerFloor::
+get_reach() const {
+  return _reach;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionHandlerFloor::set_max_velocity
 //       Access: Public

panda/src/collide/collisionHandlerFloor.cxx

@@ -33,6 +33,7 @@ TypeHandle CollisionHandlerFloor::_type_handle;
 CollisionHandlerFloor::
 CollisionHandlerFloor() {
   _offset = 0.0f;
+  _reach = 1.0f;
   _max_velocity = 0.0f;
 }
 
@@ -45,6 +46,88 @@ CollisionHandlerFloor::
 ~CollisionHandlerFloor() {
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionHandlerGravity::set_highest_collision
+//       Access: Protected
+//  Description: 
+//               
+//               
+//
+//               
+//               
+//               
+////////////////////////////////////////////////////////////////////
+float CollisionHandlerFloor::
+set_highest_collision(const NodePath &target_node_path, const NodePath &from_node_path, const Entries &entries) {
+  // Get the maximum height for all collisions with this node.
+  // This is really the distance to-the-ground, so it will
+  // be negative when the avatar is above the ground.
+  // Larger values (less negative) are higher elevation (assuming
+  // the avatar is right-side-up (or the ray is plumb)).
+  bool got_max = false;
+  bool got_min = false;
+  float max_height = 0.0f;
+  float min_height = 0.0f;
+  CollisionEntry *highest = NULL;
+  CollisionEntry *lowest = NULL;
+
+  Entries::const_iterator ei;
+  for (ei = entries.begin(); ei != entries.end(); ++ei) {
+    CollisionEntry *entry = (*ei);
+    nassertr(entry != (CollisionEntry *)NULL, 0.0f);
+    nassertr(from_node_path == entry->get_from_node_path(), 0.0f);
+
+    if (entry->has_surface_point()) {
+      LPoint3f point = entry->get_surface_point(target_node_path);
+      if (collide_cat.is_debug()) {
+        collide_cat.debug()
+          << "Intersection point detected at " << point << "\n";
+      }
+
+      float height = point[2];
+      if (height < _offset + _reach &&
+         (!got_max || height > max_height)) {
+        got_max = true;
+        max_height = height;
+        highest = entry;
+      }
+      if (!got_min || height < min_height) {
+        got_min = true;
+        min_height = height;
+        lowest = entry;
+      }
+    }
+  }
+  if (!got_max && got_min) {
+    // We've fallen through the world, but we're also under some walkable
+    // geometry.
+    // Move us up to the lowest surface:
+    got_max = true;
+    max_height = min_height;
+    highest = lowest;
+  }
+  //#*#_has_contact = got_max;
+
+  #if 0
+    cout<<"\ncolliding with:\n";
+    for (Colliding::const_iterator i = _current_colliding.begin(); i != _current_colliding.end(); ++i) {
+      (**i).write(cout, 2);
+    }
+    cout<<"\nhighest:\n";
+    highest->write(cout, 2);
+    cout<<endl;
+  #endif
+  #if 1
+  // We only collide with things we are impacting with.
+  // Remove the collisions:
+  _current_colliding.clear();
+  // Add only the one that we're impacting with:
+  add_entry(highest);
+  #endif
+  
+  return max_height;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionHandlerFloor::handle_entries
 //       Access: Protected, Virtual
@@ -83,6 +166,7 @@ handle_entries() {
     } else {
       ColliderDef &def = (*ci).second;
       {
+        #if 0
         // Get the maximum height for all collisions with this node.
         bool got_max = false;
         float max_height = 0.0f;
@@ -116,6 +200,12 @@ handle_entries() {
 
         // Now set our height accordingly.
         float adjust = max_height + _offset;
+        #else
+        float max_height = set_highest_collision(def._target, from_node_path, entries);
+
+        // Now set our height accordingly.
+        float adjust = max_height + _offset;        
+        #endif
         if (!IS_THRESHOLD_ZERO(adjust, 0.001)) {
           if (collide_cat.is_debug()) {
             collide_cat.debug()

panda/src/collide/collisionHandlerFloor.h

@@ -40,15 +40,20 @@ PUBLISHED:
   INLINE void set_offset(float offset);
   INLINE float get_offset() const;
 
+  INLINE void set_reach(float reach);
+  INLINE float get_reach() const;
+
   INLINE void set_max_velocity(float max_vel);
   INLINE float get_max_velocity() const;
 
 protected:
+  float set_highest_collision(const NodePath &target_node_path, const NodePath &from_node_path, const Entries &entries);
   virtual bool handle_entries();
   virtual void apply_linear_force(ColliderDef &def, const LVector3f &force);
 
 private:
   float _offset;
+  float _reach;
   float _max_velocity;
 
 

panda/src/collide/collisionHandlerGravity.I

@@ -41,6 +41,28 @@ get_offset() const {
   return _offset;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionHandlerGravity::set_reach
+//       Access: Public
+//  Description: Sets the reach to add to (or subtract from)
+//               the highest collision point
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionHandlerGravity::
+set_reach(float reach) {
+  _reach = reach;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionHandlerGravity::get_reach
+//       Access: Public
+//  Description: Returns the reach to add to (or subtract from)
+//               the highest collision point
+////////////////////////////////////////////////////////////////////
+INLINE float CollisionHandlerGravity::
+get_reach() const {
+  return _reach;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionHandlerGravity::get_airborne_height
 //       Access: Public

panda/src/collide/collisionHandlerGravity.cxx

@@ -33,6 +33,7 @@ TypeHandle CollisionHandlerGravity::_type_handle;
 CollisionHandlerGravity::
 CollisionHandlerGravity() {
   _offset = 0.0f;
+  _reach = 1.0f;
   _airborne_height = 0.0f;
   _impact_velocity = 0.0f;
   _gravity = 32.174f;
@@ -60,6 +61,57 @@ CollisionHandlerGravity::
 //               
 //               
 ////////////////////////////////////////////////////////////////////
+#define OLD_COLLISION_HANDLER_GRAVITY 0
+#if OLD_COLLISION_HANDLER_GRAVITY
+float CollisionHandlerGravity::
+set_highest_collision(const NodePath &target_node_path, const NodePath &from_node_path, const Entries &entries) {
+  // Get the maximum height for all collisions with this node.
+  bool got_max = false;
+  float max_height = 0.0f;
+  CollisionEntry *highest = NULL;
+
+  Entries::const_iterator ei;
+  for (ei = entries.begin(); ei != entries.end(); ++ei) {
+    CollisionEntry *entry = (*ei);
+    nassertr(entry != (CollisionEntry *)NULL, 0.0f);
+    nassertr(from_node_path == entry->get_from_node_path(), 0.0f);
+
+    if (entry->has_surface_point()) {
+      LPoint3f point = entry->get_surface_point(target_node_path);
+      if (collide_cat.is_debug()) {
+        collide_cat.debug()
+          << "Intersection point detected at " << point << "\n";
+      }
+
+      float height = point[2];
+      if (!got_max || height > max_height) {
+        got_max = true;
+        max_height = height;
+        highest = entry;
+      }
+    }
+  }
+  //#*#_has_contact = got_max;
+
+  #if 0
+    cout<<"\ncolliding with:\n";
+    for (Colliding::const_iterator i = _current_colliding.begin(); i != _current_colliding.end(); ++i) {
+      (**i).write(cout, 2);
+    }
+    cout<<"\nhighest:\n";
+    highest->write(cout, 2);
+    cout<<endl;
+  #endif
+
+  // We only collide with things we are impacting with.
+  // Remove the collisions:
+  _current_colliding.clear();
+  // Add only the one that we're impacting with:
+  add_entry(highest);
+  
+  return max_height;
+}
+#else
 float CollisionHandlerGravity::
 set_highest_collision(const NodePath &target_node_path, const NodePath &from_node_path, const Entries &entries) {
   // Get the maximum height for all collisions with this node.
@@ -88,7 +140,7 @@ set_highest_collision(const NodePath &target_node_path, const NodePath &from_nod
       }
 
       float height = point[2];
-      if (height < _offset  &&
+      if (height < _offset + _reach &&
          (!got_max || height > max_height)) {
         got_max = true;
         max_height = height;
@@ -129,6 +181,7 @@ set_highest_collision(const NodePath &target_node_path, const NodePath &from_nod
   
   return max_height;
 }
+#endif
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionHandlerGravity::handle_entries
@@ -165,7 +218,11 @@ handle_entries() {
       float max_height = set_highest_collision(def._target, from_node_path, entries);
 
       // Now set our height accordingly.
-      float adjust = max_height; // #*# + _offset;
+      #if OLD_COLLISION_HANDLER_GRAVITY
+      float adjust = max_height + _offset;
+      #else
+      float adjust = max_height + _offset;
+      #endif
       if (_current_velocity > 0.0f || !IS_THRESHOLD_ZERO(adjust, 0.001)) {
         if (collide_cat.is_debug()) {
           collide_cat.debug()

panda/src/collide/collisionHandlerGravity.h

@@ -40,6 +40,9 @@ PUBLISHED:
   INLINE void set_offset(float offset);
   INLINE float get_offset() const;
 
+  INLINE void set_reach(float reach);
+  INLINE float get_reach() const;
+
   INLINE float get_airborne_height() const;
   INLINE bool is_on_ground() const;
   INLINE float get_impact_velocity() const;
@@ -61,6 +64,7 @@ protected:
 
 private:
   float _offset;
+  float _reach;
   float _airborne_height;
   float _impact_velocity;
   float _gravity;