contact_pos for collision polys, spheres and tubes, improvements to fluidpusher

panda/src/collide/collisionEntry.I

@@ -275,25 +275,25 @@ has_interior_point() const {
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: CollisionEntry::set_contact_point
+//     Function: CollisionEntry::set_contact_pos
 //       Access: Published
-//  Description: Stores the point, on the surface of the "into"
-//               object, at which the collision is first detected.
+//  Description: Stores the position of the "from" object at the
+//               instant at which the collision is first detected.
 //
-//               This point is specified in the coordinate space of
+//               This position is specified in the coordinate space of
 //               the "into" object.
 ////////////////////////////////////////////////////////////////////
 INLINE void CollisionEntry::
-set_contact_point(const LPoint3f &point) {
-  _contact_point = point;
-  _flags |= F_has_contact_point;
+set_contact_pos(const LPoint3f &pos) {
+  _contact_pos = pos;
+  _flags |= F_has_contact_pos;
 }
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionEntry::set_contact_normal
 //       Access: Published
 //  Description: Stores the surface normal of the "into" object at the
-//               contact point.
+//               contact pos.
 //
 //               This normal is specified in the coordinate space of
 //               the "into" object.
@@ -305,16 +305,16 @@ set_contact_normal(const LVector3f &normal) {
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: CollisionEntry::has_contact_point
+//     Function: CollisionEntry::has_contact_pos
 //       Access: Published
-//  Description: Returns true if the contact point has been specified,
-//               false otherwise.  See get_contact_point().  Some
+//  Description: Returns true if the contact position has been specified,
+//               false otherwise.  See get_contact_pos().  Some
 //               types of collisions may not compute the contact
-//               point.
+//               pos.
 ////////////////////////////////////////////////////////////////////
 INLINE bool CollisionEntry::
-has_contact_point() const {
-  return (_flags & F_has_contact_point) != 0;
+has_contact_pos() const {
+  return (_flags & F_has_contact_pos) != 0;
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/collide/collisionEntry.cxx

@@ -41,7 +41,7 @@ CollisionEntry(const CollisionEntry &copy) :
   _surface_point(copy._surface_point),
   _surface_normal(copy._surface_normal),
   _interior_point(copy._interior_point),
-  _contact_point(copy._contact_point),
+  _contact_pos(copy._contact_pos),
   _contact_normal(copy._contact_normal)
 {
 }
@@ -65,7 +65,7 @@ operator = (const CollisionEntry &copy) {
   _surface_point = copy._surface_point;
   _surface_normal = copy._surface_normal;
   _interior_point = copy._interior_point;
-  _contact_point = copy._contact_point;
+  _contact_pos = copy._contact_pos;
   _contact_normal = copy._contact_normal;
 }
 
@@ -169,33 +169,26 @@ get_all(const NodePath &space, LPoint3f &surface_point,
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: CollisionEntry::get_contact_point
+//     Function: CollisionEntry::get_contact_pos
 //       Access: Published
-//  Description: Returns the point, on the surface of the "into"
-//               object, at which a collision is detected.  This can
-//               be thought of as the first point of intersection.
-//               The surface point is not always the initial point of
-//               intersection. We preserve the original implementation
-//               of surface_point detection so that the existing
-//               collision response code will still work, and provide
-//               the contact_point for collision response code that
-//               needs precise collision information.
+//  Description: Returns the position of the "from" object at the instant
+//               that a collision is first detected.
 //
-//               The point will be converted into whichever coordinate
+//               The position will be converted into whichever coordinate
 //               space the caller specifies.
 ////////////////////////////////////////////////////////////////////
 LPoint3f CollisionEntry::
-get_contact_point(const NodePath &space) const {
-  nassertr(has_contact_point(), LPoint3f::zero());
+get_contact_pos(const NodePath &space) const {
+  nassertr(has_contact_pos(), LPoint3f::zero());
   CPT(TransformState) transform = _into_node_path.get_transform(space);
-  return _contact_point * transform->get_mat();
+  return _contact_pos * transform->get_mat();
 }
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionEntry::get_contact_normal
 //       Access: Published
 //  Description: Returns the surface normal of the "into" object at
-//               the contact point.
+//               the contact position.
 //
 //               The normal will be converted into whichever coordinate
 //               space the caller specifies.
@@ -210,25 +203,25 @@ get_contact_normal(const NodePath &space) const {
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionEntry::get_all_contact_info
 //       Access: Published
-//  Description: Simultaneously transforms the surface point, surface
-//               normal, and interior point of the collision into the
+//  Description: Simultaneously transforms the contact position and
+//               contact normal of the collision into the
 //               indicated coordinate space.
 //
 //               Returns true if all three properties are available,
 //               or false if any one of them is not.
 ////////////////////////////////////////////////////////////////////
 bool CollisionEntry::
-get_all_contact_info(const NodePath &space, LPoint3f &contact_point,
+get_all_contact_info(const NodePath &space, LPoint3f &contact_pos,
                      LVector3f &contact_normal) const {
   CPT(TransformState) transform = _into_node_path.get_transform(space);
   const LMatrix4f &mat = transform->get_mat();
   bool all_ok = true;
 
-  if (!has_contact_point()) {
-    contact_point = LPoint3f::zero();
+  if (!has_contact_pos()) {
+    contact_pos = LPoint3f::zero();
     all_ok = false;
   } else {
-    contact_point = _contact_point * mat;
+    contact_pos = _contact_pos * mat;
   }
 
   if (!has_contact_normal()) {

panda/src/collide/collisionEntry.h

@@ -78,10 +78,10 @@ PUBLISHED:
   INLINE bool has_surface_normal() const;
   INLINE bool has_interior_point() const;
 
-  INLINE void set_contact_point(const LPoint3f &point);
+  INLINE void set_contact_pos(const LPoint3f &pos);
   INLINE void set_contact_normal(const LVector3f &normal);
 
-  INLINE bool has_contact_point() const;
+  INLINE bool has_contact_pos() const;
   INLINE bool has_contact_normal() const;
 
   LPoint3f get_surface_point(const NodePath &space) const;
@@ -92,10 +92,10 @@ PUBLISHED:
                LVector3f &surface_normal,
                LPoint3f &interior_point) const;
 
-  LPoint3f get_contact_point(const NodePath &space) const;
+  LPoint3f get_contact_pos(const NodePath &space) const;
   LVector3f get_contact_normal(const NodePath &space) const;
   bool get_all_contact_info(const NodePath &space,
-                            LPoint3f &contact_point,
+                            LPoint3f &contact_pos,
                             LVector3f &contact_normal) const;
 
   void output(ostream &out) const;
@@ -133,7 +133,7 @@ private:
     F_has_interior_point      = 0x0004,
     F_respect_prev_transform  = 0x0008,
     F_checked_clip_planes     = 0x0010,
-    F_has_contact_point       = 0x0020,
+    F_has_contact_pos         = 0x0020,
     F_has_contact_normal      = 0x0040,
   };
 
@@ -143,7 +143,7 @@ private:
   LVector3f _surface_normal;
   LPoint3f _interior_point;
 
-  LPoint3f _contact_point;
+  LPoint3f _contact_pos;
   LVector3f _contact_normal;
   
 public:

panda/src/collide/collisionHandlerFluidPusher.cxx

@@ -142,12 +142,12 @@ handle_entries() {
       Entries entries(*orig_entries);
       
       // this is the original position delta for the entire frame, before collision response
-      LPoint3f M(from_node_path.get_pos_delta(wrt_node));
+      LVector3f M(from_node_path.get_pos_delta(wrt_node));
       if (_horizontal) {
         M[2] = 0.0f;
       }
       // this is used to track position deltas every time we collide against a solid
-      LPoint3f N(M);
+      LVector3f N(M);
       collide_cat.info() << "N: " << N << endl;
       
       const LPoint3f orig_pos(from_node_path.get_pos(wrt_node));
@@ -198,7 +198,7 @@ handle_entries() {
       
       // iterate until the mover runs out of movement or gets stuck
       while (true) {
-        cout << "while (true)" << endl;
+        collide_cat.info() << "while (true)" << endl;
         const CollisionEntry *C = 0;
         // find the first (earliest) collision
         Entries::const_iterator cei;
@@ -219,22 +219,22 @@ handle_entries() {
         
         collide_cat.info() << "t: " << C->get_t() << endl;
         
-        // move back to initial contact point
-        LPoint3f contact_point;
+        // move back to initial contact position
+        LPoint3f contact_pos;
         LVector3f contact_normal;
 
-        if (!C->get_all_contact_info(wrt_node, contact_point, contact_normal)) {
+        if (!C->get_all_contact_info(wrt_node, contact_pos, contact_normal)) {
           collide_cat.warning()
             << "Cannot shove on " << from_node_path << " for collision into "
-            << C->get_into_node_path() << "; no contact point/normal information.\n";
+            << C->get_into_node_path() << "; no contact pos/normal information.\n";
           break;
         }
         // calculate the position of the target node at the point of contact
-        contact_point -= sphere_offset;
-        collide_cat.info() << "contact_point: " << contact_point << endl;
+        contact_pos -= sphere_offset;
+        collide_cat.info() << "contact_pos: " << contact_pos << endl;
         
         uncollided_pos = candidate_final_pos;
-        candidate_final_pos = contact_point;
+        candidate_final_pos = contact_pos;
         
         LVector3f proj_surface_normal(contact_normal);
         if (_horizontal) {
@@ -284,16 +284,24 @@ handle_entries() {
           }
         }
         
-        LVector3f blocked_movement(uncollided_pos - contact_point);
+        LVector3f blocked_movement(uncollided_pos - contact_pos);
         if (_horizontal) {
           blocked_movement[2] = 0.0f;
         }
         collide_cat.info() << "blocked movement: " << blocked_movement << endl;
         
+        float push_magnitude(-blocked_movement.dot(proj_surface_normal));
+        LVector3f push;
+        if (push_magnitude < 0.0f) {
+          // don't ever push into plane, always push out along plane normal
+          push = LVector3f(0,0,0);
+        } else {
+          push = norm_proj_surface_normal * push_magnitude;
+        }
+        
         // calculate new position given that you collided with this thing
         // project the final position onto the plane of the obstruction
-        candidate_final_pos = uncollided_pos + (norm_proj_surface_normal *
-                                                -blocked_movement.dot(proj_surface_normal));
+        candidate_final_pos = uncollided_pos + push;
         
         collide_cat.info() << "candidate_final_pos: " << candidate_final_pos << endl;
         
@@ -303,8 +311,8 @@ handle_entries() {
         from_node_path.set_pos(wrt_node, candidate_final_pos);
         CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
         collide_cat.info() << "prev_trans->get_pos: " << prev_trans->get_pos() << endl;
-        prev_trans = prev_trans->set_pos(contact_point);
-        collide_cat.info() << "contact_point: " << contact_point << endl;
+        prev_trans = prev_trans->set_pos(contact_pos);
+        collide_cat.info() << "contact_pos: " << contact_pos << endl;
         collide_cat.info() << "prev_trans->get_pos: " << prev_trans->get_pos() << endl;
         from_node_path.set_prev_transform(wrt_node, prev_trans);
         

panda/src/collide/collisionInvSphere.cxx

@@ -175,7 +175,7 @@ test_intersection_from_line(const CollisionEntry &entry) const {
   LVector3f from_direction = line->get_direction() * wrt_mat;
 
   double t1, t2;
-  if (!intersects_line(t1, t2, from_origin, from_direction)) {
+  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
     // The line is in the middle of space, and therefore intersects
     // the sphere.
     t1 = t2 = 0.0;
@@ -218,7 +218,7 @@ test_intersection_from_ray(const CollisionEntry &entry) const {
   LVector3f from_direction = ray->get_direction() * wrt_mat;
 
   double t1, t2;
-  if (!intersects_line(t1, t2, from_origin, from_direction)) {
+  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
     // The ray is in the middle of space, and therefore intersects
     // the sphere.
     t1 = t2 = 0.0;
@@ -265,7 +265,7 @@ test_intersection_from_segment(const CollisionEntry &entry) const {
   LVector3f from_direction = from_b - from_a;
 
   double t1, t2;
-  if (!intersects_line(t1, t2, from_a, from_direction)) {
+  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
     // The segment is in the middle of space, and therefore intersects
     // the sphere.
     t1 = t2 = 0.0;

panda/src/collide/collisionPolygon.cxx

@@ -571,7 +571,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
   new_entry->set_surface_normal(normal);
   new_entry->set_surface_point(from_center - normal * dist);
   new_entry->set_interior_point(from_center - normal * (dist + into_depth));
-  new_entry->set_contact_point(contact_point);
+  new_entry->set_contact_pos(contact_point);
   new_entry->set_contact_normal(get_normal());
   new_entry->set_t(actual_t);
 

panda/src/collide/collisionSphere.cxx

@@ -315,21 +315,57 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
   const CollisionSphere *sphere;
   DCAST_INTO_R(sphere, entry.get_from(), 0);
 
-  const LMatrix4f &wrt_mat = entry.get_wrt_mat();
+  CPT(TransformState) wrt_space = entry.get_wrt_space();
+  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
+
+  const LMatrix4f &wrt_mat = wrt_space->get_mat();
+
+  LPoint3f from_a = sphere->get_center() * wrt_prev_space->get_mat();
+  LPoint3f from_b = sphere->get_center() * wrt_mat;
+
+  LPoint3f into_center(get_center());
+  float into_radius(get_radius());
 
-  LPoint3f from_center = sphere->get_center() * wrt_mat;
   LVector3f from_radius_v =
     LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
   float from_radius = length(from_radius_v);
 
-  LPoint3f into_center = get_center();
-  float into_radius = get_radius();
+  LPoint3f into_intersection_point;
+  double t1, t2;
 
-  LVector3f vec = from_center - into_center;
-  float dist2 = dot(vec, vec);
-  if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) {
-    // No intersection.
-    return NULL;
+  if (from_a != from_b) {
+    LVector3f from_direction = from_b - from_a;
+    if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
+      // No intersection.
+      return NULL;
+    }
+
+    if (t2 < 0.0 || t1 > 1.0) {
+      // Both intersection points are before the start of the segment or
+      // after the end of the segment.
+      return NULL;
+    }
+
+    if (t1 < 0.0) {
+      // Point a is within the sphere.  The first intersection point is
+      // point a itself.
+      into_intersection_point = from_a;
+    } else {
+      // Point a is outside the sphere, and point b is either inside the
+      // sphere or beyond it.  The first intersection point is at t1.
+      into_intersection_point = from_a + t1 * from_direction;
+    }
+
+  } else {
+    LVector3f vec = from_a - into_center;
+    float dist2 = dot(vec, vec);
+    if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) {
+      // No intersection.
+      return NULL;
+    }
+
+    into_intersection_point = from_a;
+    t1 = 0.0f;
   }
 
   if (collide_cat.is_debug()) {
@@ -339,22 +375,28 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
   }
   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
 
+  LPoint3f from_center = sphere->get_center() * wrt_mat;
+
   LVector3f surface_normal;
-  float vec_length = vec.length();
+  LVector3f v(into_intersection_point - into_center);
+  float vec_length = v.length();
   if (IS_NEARLY_ZERO(vec_length)) {
     // If we don't have a collision normal (e.g. the centers are
     // exactly coincident), then make up an arbitrary normal--any one
     // is as good as any other.
     surface_normal.set(1.0, 0.0, 0.0);
   } else {
-    surface_normal = vec / vec_length;
+    surface_normal = v / vec_length;
   }
 
-  LVector3f normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
+  LVector3f eff_normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
 
-  new_entry->set_surface_normal(normal);
+  new_entry->set_surface_normal(eff_normal);
   new_entry->set_surface_point(into_center + surface_normal * into_radius);
   new_entry->set_interior_point(from_center - surface_normal * from_radius);
+  new_entry->set_contact_pos(into_intersection_point);
+  new_entry->set_contact_normal(surface_normal);
+  new_entry->set_t(t1);
 
   return new_entry;
 }
@@ -375,7 +417,7 @@ test_intersection_from_line(const CollisionEntry &entry) const {
   LVector3f from_direction = line->get_direction() * wrt_mat;
 
   double t1, t2;
-  if (!intersects_line(t1, t2, from_origin, from_direction)) {
+  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
     // No intersection.
     return NULL;
   }
@@ -417,7 +459,7 @@ test_intersection_from_ray(const CollisionEntry &entry) const {
   LVector3f from_direction = ray->get_direction() * wrt_mat;
 
   double t1, t2;
-  if (!intersects_line(t1, t2, from_origin, from_direction)) {
+  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
     // No intersection.
     return NULL;
   }
@@ -467,7 +509,7 @@ test_intersection_from_segment(const CollisionEntry &entry) const {
   LVector3f from_direction = from_b - from_a;
 
   double t1, t2;
-  if (!intersects_line(t1, t2, from_a, from_direction)) {
+  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
     // No intersection.
     return NULL;
   }
@@ -560,7 +602,8 @@ fill_viz_geom() {
 ////////////////////////////////////////////////////////////////////
 bool CollisionSphere::
 intersects_line(double &t1, double &t2,
-                const LPoint3f &from, const LVector3f &delta) const {
+                const LPoint3f &from, const LVector3f &delta,
+                float inflate_radius) const {
   // Solve the equation for the intersection of a line with a sphere
   // using the quadratic equation.
 
@@ -598,7 +641,8 @@ intersects_line(double &t1, double &t2,
   LVector3f fc = from - get_center();
   double B = 2.0f* dot(delta, fc);
   double fc_d2 = dot(fc, fc);
-  double C = fc_d2 - get_radius() * get_radius();
+  double radius = get_radius() + inflate_radius;
+  double C = fc_d2 - radius * radius;
 
   double radical = B*B - 4.0*A*C;
 

panda/src/collide/collisionSphere.h

@@ -79,7 +79,8 @@ protected:
 
 protected:
   bool intersects_line(double &t1, double &t2,
-                       const LPoint3f &from, const LVector3f &delta) const;
+                       const LPoint3f &from, const LVector3f &delta,
+                       float inflate_radius) const;
   Vertexf compute_point(float latitude, float longitude) const;
 
 private:

panda/src/collide/collisionTube.cxx

@@ -804,10 +804,7 @@ set_intersection_point(CollisionEntry *new_entry,
   point = point * _mat;
   normal = normal * _mat;
 
-  // Also adjust the original point into the tube by the amount of
-  // extra_radius, which should put it on the surface of the tube if
-  // our collision was tangential.
-  LPoint3f orig_point = into_intersection_point - normal * extra_radius;
+  LVector3f contact_normal(normal);
 
   if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) {
     normal = get_effective_normal();
@@ -815,7 +812,12 @@ set_intersection_point(CollisionEntry *new_entry,
 
   new_entry->set_surface_normal(normal);
   new_entry->set_surface_point(point);
-  new_entry->set_interior_point(orig_point);
+  // Also adjust the original point into the tube by the amount of
+  // extra_radius, which should put it on the surface of the tube if
+  // our collision was tangential.
+  new_entry->set_interior_point(into_intersection_point - normal * extra_radius);
+  new_entry->set_contact_pos(into_intersection_point);
+  new_entry->set_contact_normal(contact_normal);
 }
 
 ////////////////////////////////////////////////////////////////////