robustify tube

panda/src/collide/collisionTube.cxx

@@ -171,7 +171,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
     // tube or beyond it.  The first intersection point is at t1.
     into_intersection_point = from_a + t1 * from_direction;
   }
-  new_entry->set_into_intersection_point(into_intersection_point);
+  set_intersection_point(new_entry, into_intersection_point, from_radius);
 
   return new_entry;
 }
@@ -217,7 +217,7 @@ test_intersection_from_ray(const CollisionEntry &entry) const {
     // at t1.
     into_intersection_point = from_origin + t1 * from_direction;
   }
-  new_entry->set_into_intersection_point(into_intersection_point);
+  set_intersection_point(new_entry, into_intersection_point, 0.0);
 
   return new_entry;
 }
@@ -265,7 +265,7 @@ test_intersection_from_segment(const CollisionEntry &entry) const {
     // tube or beyond it.  The first intersection point is at t1.
     into_intersection_point = from_a + t1 * from_direction;
   }
-  new_entry->set_into_intersection_point(into_intersection_point);
+  set_intersection_point(new_entry, into_intersection_point, 0.0);
 
   return new_entry;
 }
@@ -336,7 +336,7 @@ fill_viz_geom() {
   tube->set_lengths(lengths);
 
   _viz_geom->add_geom(tube, get_solid_viz_state());
-  _viz_geom->add_geom(tube, get_wireframe_viz_state());
+  //_viz_geom->add_geom(tube, get_wireframe_viz_state());
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -351,10 +351,9 @@ recalc_internals() {
   LVector3f direction = (_b - _a);
   _length = direction.length();
 
-  LMatrix4f mat;
+  look_at(_mat, direction, LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right);
-  look_at(mat, direction, LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right);
+  _mat.set_row(3, _a);
-  mat.set_row(3, _a);
+  _inv_mat.invert_from(_mat);
-  _inv_mat.invert_from(mat);
 
   mark_viz_stale();
   mark_bound_stale();
@@ -629,6 +628,70 @@ sphere_intersects_line(double &t1, double &t2, float center_y,
   return true;
 }
 
+////////////////////////////////////////////////////////////////////
+//     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 {
+  // Convert the point into our canonical space for analysis.
+  LPoint3f point = into_intersection_point * _inv_mat;
+  LVector3f normal;
+
+  if (point[1] <= 0.0) {
+    // The point is on the first endcap.
+    normal = point;
+    if (!normal.normalize()) {
+      normal.set(0.0, -1.0, 0.0);
+    }
+    point = normal * _radius;
+
+  } else if (point[1] >= _length) {
+    // The point is on the second endcap.
+    normal.set(point[0], point[1] - _length, point[2]);
+    if (!normal.normalize()) {
+      normal.set(0.0, 1.0, 0.0);
+    }
+    point = normal * _radius;
+    point[1] += _length;
+
+  } else {
+    // The point is within the cylinder part.
+    LVector2d normal2d(point[0], point[2]);
+    if (!normal2d.normalize()) {
+      normal2d.set(0.0, 1.0);
+    }
+    normal.set(normal2d[0], 0.0, normal2d[1]);
+    point.set(normal[0] * _radius, point[1], normal[2] * _radius);
+  }
+
+  // Now convert the point and normal back into real space.
+  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;
+
+  // And the difference between point and orig_point is the depth to
+  // which the object has intersected the tube.
+  float depth = (point - orig_point).length();
+
+  new_entry->set_into_intersection_point(point);
+  new_entry->set_into_surface_normal(normal);
+  new_entry->set_into_depth(depth);
+
+  LVector3f from_depth_vec = (normal * depth) * new_entry->get_inv_wrt_space();
+  new_entry->set_from_depth(from_depth_vec.length());
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionTube::register_with_read_factory
 //       Access: Public, Static

panda/src/collide/collisionTube.h

@@ -89,12 +89,16 @@ private:
   bool sphere_intersects_line(double &t1, double &t2, float center_y,
                               const LPoint3f &from, const LVector3f &delta,
                               float inflate_radius) const;
+  void set_intersection_point(CollisionEntry *new_entry, 
+                              const LPoint3f &into_intersection_point, 
+                              double extra_radius) const;
 
 private:
   LPoint3f _a, _b;
   float _radius;
 
   // These are derived from the above.
+  LMatrix4f _mat;
   LMatrix4f _inv_mat;
   float _length;
 

panda/src/collide/collisionVisualizer.cxx

@@ -197,6 +197,25 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) {
             new CullableObject(line, empty_state, xform_data._render_transform);
           
           trav->get_cull_handler()->record_object(object);
+
+          // Also draw the depth vector.
+          if (point._depth != 0.0) {
+            PT(GeomLine) line = new GeomLine;
+            
+            PTA_Vertexf verts;
+            verts.push_back(point._point);
+            verts.push_back(point._point - point._normal * point._depth);
+            PTA_Colorf colors;
+            colors.push_back(Colorf(0.0f, 0.0f, 1.0f, 1.0f));
+
+            line->set_coords(verts);
+            line->set_colors(colors, G_OVERALL);
+            line->set_num_prims(1);
+            
+            CullableObject *object = 
+              new CullableObject(line, empty_state, xform_data._render_transform);
+            trav->get_cull_handler()->record_object(object);
+          }
         }
       }
     }
@@ -260,6 +279,10 @@ collision_tested(const CollisionEntry &entry, bool detected) {
         p._normal = entry.get_into_surface_normal();
       } else {
         p._normal = LVector3f::zero();
+        p._depth = 0.0;
+      }
+      if (entry.has_into_depth()) {
+        p._depth = entry.get_into_depth();
       }
       viz_info._points.push_back(p);
     }

panda/src/collide/collisionVisualizer.h

@@ -71,6 +71,7 @@ private:
   public:
     LPoint3f _point;
     LVector3f _normal;
+    float _depth;
   };
   typedef pvector<CollisionPoint> Points;