spheres and tubes working with fluid pusher

panda/src/collide/collisionSphere.cxx

@@ -330,8 +330,10 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
     LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
   float from_radius = length(from_radius_v);
 
-  LPoint3f into_intersection_point;
+  LPoint3f into_intersection_point(from_b);
   double t1, t2;
+  LPoint3f contact_point(into_intersection_point);
+  float actual_t = 0.0f;
 
   if (from_a != from_b) {
     LVector3f from_direction = from_b - from_a;
@@ -346,6 +348,9 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
       return NULL;
     }
 
+    actual_t = min(1.0f, max(0.0f, t1));
+    contact_point = from_a + actual_t * (from_b - from_a);
+
     if (t1 < 0.0) {
       // Point a is within the sphere.  The first intersection point is
       // point a itself.
@@ -391,12 +396,24 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
 
   LVector3f eff_normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
 
+  LVector3f contact_normal;
+  LVector3f v2 = contact_point - into_center;
+  float v2_len = v2.length();
+  if (IS_NEARLY_ZERO(v2_len)) {
+    // 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.
+    contact_normal.set(1.0, 0.0, 0.0);
+  } else {
+    contact_normal = v2 / v2_len;
+  }
+
   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);
+  new_entry->set_contact_pos(contact_point);
+  new_entry->set_contact_normal(contact_normal);
+  new_entry->set_t(actual_t);
 
   return new_entry;
 }

panda/src/collide/collisionTube.cxx

@@ -171,6 +171,9 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
   LPoint3f from_a = sphere->get_center() * wrt_mat;
   LPoint3f from_b = from_a;
 
+  LPoint3f contact_point;
+  float actual_t = 0.0f;
+
   if (wrt_prev_space != wrt_space) {
     // If the sphere is moving relative to the tube, it becomes a tube
     // itself.
@@ -195,6 +198,9 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
     return NULL;
   }
 
+  actual_t = min(1.0f, max(0.0f, t1));
+  contact_point = from_a + actual_t * (from_b - from_a);
+
   if (collide_cat.is_debug()) {
     collide_cat.debug()
       << "intersection detected from " << entry.get_from_node_path() << " into "
@@ -213,7 +219,16 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
     into_intersection_point = from_a + t1 * from_direction;
   }
   set_intersection_point(new_entry, into_intersection_point, from_radius);
-  new_entry->set_t(t1);
+
+  LPoint3f fake_contact_point;
+  LVector3f contact_normal;
+  calculate_surface_point_and_normal(contact_point,
+                                     from_radius,
+                                     fake_contact_point,
+                                     contact_normal);
+  new_entry->set_contact_pos(contact_point);
+  new_entry->set_contact_normal(contact_normal);
+  new_entry->set_t(actual_t);
 
   return new_entry;
 }
@@ -758,19 +773,20 @@ sphere_intersects_line(double &t1, double &t2, float center_y,
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: CollisionTube::set_intersection_point
+//     Function: CollisionTube::calculate_surface_point_and_normal
 //       Access: Private
-//  Description: After an intersection has been detected, record the
-//               computed intersection point in the CollisionEntry,
-//               and also compute the relevant normal based on that
-//               point.
+//  Description: Calculates a point that is exactly on the surface
+//               of the tube and its corresponding normal, given
+//               a point that is supposedly on the surface of the
+//               tube.
 ////////////////////////////////////////////////////////////////////
 void CollisionTube::
-set_intersection_point(CollisionEntry *new_entry, 
-                       const LPoint3f &into_intersection_point, 
-                       double extra_radius) const {
+calculate_surface_point_and_normal(const LPoint3f &surface_point,
+                                   double extra_radius,
+                                   LPoint3f &result_point,
+                                   LVector3f &result_normal) const {
   // Convert the point into our canonical space for analysis.
-  LPoint3f point = into_intersection_point * _inv_mat;
+  LPoint3f point = surface_point * _inv_mat;
   LVector3f normal;
 
   if (point[1] <= 0.0) {
@@ -801,10 +817,29 @@ set_intersection_point(CollisionEntry *new_entry,
   }
 
   // Now convert the point and normal back into real space.
-  point = point * _mat;
-  normal = normal * _mat;
+  result_point = point * _mat;
+  result_normal = normal * _mat;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::set_intersection_point
+//       Access: Private
+//  Description: After an intersection has been detected, record the
+//               computed intersection point in the CollisionEntry,
+//               and also compute the relevant normal based on that
+//               point.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+set_intersection_point(CollisionEntry *new_entry, 
+                       const LPoint3f &into_intersection_point, 
+                       double extra_radius) const {
+  LPoint3f point;
+  LVector3f normal;
 
-  LVector3f contact_normal(normal);
+  calculate_surface_point_and_normal(into_intersection_point,
+                                     extra_radius,
+                                     point,
+                                     normal);
 
   if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) {
     normal = get_effective_normal();
@@ -816,8 +851,6 @@ set_intersection_point(CollisionEntry *new_entry,
   // 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);
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/collide/collisionTube.h

@@ -97,6 +97,10 @@ private:
   bool sphere_intersects_line(double &t1, double &t2, float center_y,
                               const LPoint3f &from, const LVector3f &delta,
                               float inflate_radius) const;
+  void calculate_surface_point_and_normal(const LPoint3f &surface_point,
+                                          double extra_radius,
+                                          LPoint3f &result_point,
+                                          LVector3f &result_normal) const;
   void set_intersection_point(CollisionEntry *new_entry, 
                               const LPoint3f &into_intersection_point, 
                               double extra_radius) const;