Add tube-into-plane and tube-into-sphere tests

panda/src/collide/collisionPlane.cxx

@@ -18,6 +18,7 @@
 #include "collisionLine.h"
 #include "collisionRay.h"
 #include "collisionSegment.h"
+#include "collisionTube.h"
 #include "collisionParabola.h"
 #include "config_collide.h"
 #include "pointerToArray.h"
@@ -293,6 +294,76 @@ test_intersection_from_segment(const CollisionEntry &entry) const {
   return new_entry;
 }
 
+/**
+ *
+ */
+PT(CollisionEntry) CollisionPlane::
+test_intersection_from_tube(const CollisionEntry &entry) const {
+  const CollisionTube *tube;
+  DCAST_INTO_R(tube, entry.get_from(), 0);
+
+  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
+
+  LPoint3 from_a = tube->get_point_a() * wrt_mat;
+  LPoint3 from_b = tube->get_point_b() * wrt_mat;
+  LVector3 from_radius_v =
+    LVector3(tube->get_radius(), 0.0f, 0.0f) * wrt_mat;
+  PN_stdfloat from_radius = length(from_radius_v);
+
+  PN_stdfloat dist_a = _plane.dist_to_plane(from_a);
+  PN_stdfloat dist_b = _plane.dist_to_plane(from_b);
+
+  if (dist_a >= from_radius && dist_b >= from_radius) {
+    // Entirely in front of the plane means no intersection.
+    return NULL;
+  }
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << entry.get_from_node_path()
+      << " into " << entry.get_into_node_path() << "\n";
+  }
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  LVector3 normal = (has_effective_normal() && tube->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
+  new_entry->set_surface_normal(normal);
+
+  PN_stdfloat t;
+  LVector3 from_direction = from_b - from_a;
+  if (_plane.intersects_line(t, from_a, from_direction)) {
+    // It intersects the plane.
+    if (t >= 1.0f) {
+      new_entry->set_surface_point(from_b - get_normal() * dist_b);
+
+    } else if (t <= 0.0f) {
+      new_entry->set_surface_point(from_a - get_normal() * dist_a);
+
+    } else {
+      // Within the tube!  Yay, that means we have a surface point.
+      new_entry->set_surface_point(from_a + t * from_direction);
+    }
+  } else {
+    // If it's completely parallel, pretend it's colliding in the center of
+    // the tube.
+    new_entry->set_surface_point(from_a + 0.5f * from_direction - get_normal() * dist_a);
+  }
+
+  if (IS_NEARLY_EQUAL(dist_a, dist_b)) {
+    // Let's be fair and choose the center of the tube.
+    new_entry->set_interior_point(from_a + 0.5f * from_direction - get_normal() * from_radius);
+
+  } else if (dist_a < dist_b) {
+    // Point A penetrates deeper.
+    new_entry->set_interior_point(from_a - get_normal() * from_radius);
+
+  } else if (dist_b < dist_a) {
+    // No, point B does.
+    new_entry->set_interior_point(from_b - get_normal() * from_radius);
+  }
+
+  return new_entry;
+}
+
 /**
  * This is part of the double-dispatch implementation of test_intersection().
  * It is called when the "from" object is a parabola.

panda/src/collide/collisionPlane.h

@@ -72,6 +72,8 @@ protected:
   virtual PT(CollisionEntry)
   test_intersection_from_segment(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
+  test_intersection_from_tube(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
   test_intersection_from_parabola(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
   test_intersection_from_box(const CollisionEntry &entry) const;

panda/src/collide/collisionSolid.cxx

@@ -17,6 +17,7 @@
 #include "collisionLine.h"
 #include "collisionRay.h"
 #include "collisionSegment.h"
+#include "collisionTube.h"
 #include "collisionParabola.h"
 #include "collisionBox.h"
 #include "collisionEntry.h"
@@ -237,6 +238,17 @@ test_intersection_from_segment(const CollisionEntry &) const {
   return NULL;
 }
 
+/**
+ * This is part of the double-dispatch implementation of test_intersection().
+ * It is called when the "from" object is a tube.
+ */
+PT(CollisionEntry) CollisionSolid::
+test_intersection_from_tube(const CollisionEntry &) const {
+  report_undefined_intersection_test(CollisionTube::get_class_type(),
+                                     get_type());
+  return NULL;
+}
+
 /**
  * This is part of the double-dispatch implementation of test_intersection().
  * It is called when the "from" object is a parabola.

panda/src/collide/collisionSolid.h

@@ -108,6 +108,8 @@ protected:
   virtual PT(CollisionEntry)
   test_intersection_from_segment(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
+  test_intersection_from_tube(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
   test_intersection_from_parabola(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
   test_intersection_from_box(const CollisionEntry &entry) const;
@@ -175,6 +177,7 @@ private:
   friend class CollisionLine;
   friend class CollisionRay;
   friend class CollisionSegment;
+  friend class CollisionTube;
   friend class CollisionParabola;
   friend class CollisionHandlerFluidPusher;
   friend class CollisionBox;

panda/src/collide/collisionSphere.cxx

@@ -14,10 +14,12 @@
 #include "collisionSphere.h"
 #include "collisionLine.h"
 #include "collisionRay.h"
-#include "collisionSegment.h"
 #include "collisionHandler.h"
 #include "collisionEntry.h"
+#include "collisionSegment.h"
+#include "collisionTube.h"
 #include "collisionParabola.h"
+#include "collisionBox.h"
 #include "config_collide.h"
 #include "boundingSphere.h"
 #include "datagram.h"
@@ -435,6 +437,62 @@ test_intersection_from_segment(const CollisionEntry &entry) const {
   return new_entry;
 }
 
+/**
+ *
+ */
+PT(CollisionEntry) CollisionSphere::
+test_intersection_from_tube(const CollisionEntry &entry) const {
+  const CollisionTube *tube;
+  DCAST_INTO_R(tube, entry.get_from(), 0);
+
+  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
+
+  LPoint3 from_a = tube->get_point_a() * wrt_mat;
+  LPoint3 from_b = tube->get_point_b() * wrt_mat;
+  LVector3 from_direction = from_b - from_a;
+
+  LVector3 from_radius_v =
+    LVector3(tube->get_radius(), 0.0f, 0.0f) * wrt_mat;
+  PN_stdfloat from_radius = length(from_radius_v);
+
+  double t1, t2;
+  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 tube or after
+    // the end of the tube.
+    return NULL;
+  }
+
+  PN_stdfloat t = (t1 + t2) * (PN_stdfloat)0.5;
+  t = max(t, (PN_stdfloat)0.0);
+  t = min(t, (PN_stdfloat)1.0);
+  LPoint3 inner_point = from_a + t * from_direction;
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << entry.get_from_node_path()
+      << " into " << entry.get_into_node_path() << "\n";
+  }
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  LVector3 normal = inner_point - get_center();
+  normal.normalize();
+  new_entry->set_surface_point(get_center() + normal * get_radius());
+  new_entry->set_interior_point(inner_point - normal * from_radius);
+
+  if (has_effective_normal() && tube->get_respect_effective_normal()) {
+    new_entry->set_surface_normal(get_effective_normal());
+  } else {
+    new_entry->set_surface_normal(normal);
+  }
+
+  return new_entry;
+}
+
 /**
  *
  */

panda/src/collide/collisionSphere.h

@@ -72,6 +72,8 @@ protected:
   virtual PT(CollisionEntry)
   test_intersection_from_segment(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
+  test_intersection_from_tube(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
   test_intersection_from_parabola(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
   test_intersection_from_box(const CollisionEntry &entry) const;

panda/src/collide/collisionTube.cxx

@@ -47,6 +47,14 @@ make_copy() {
   return new CollisionTube(*this);
 }
 
+/**
+ *
+ */
+PT(CollisionEntry) CollisionTube::
+test_intersection(const CollisionEntry &entry) const {
+  return entry.get_into()->test_intersection_from_tube(entry);
+}
+
 /**
  * Transforms the solid by the indicated matrix.
  */

panda/src/collide/collisionTube.h

@@ -40,6 +40,9 @@ public:
   INLINE CollisionTube(const CollisionTube &copy);
   virtual CollisionSolid *make_copy();
 
+  virtual PT(CollisionEntry)
+  test_intersection(const CollisionEntry &entry) const;
+
   virtual void xform(const LMatrix4 &mat);
 
   virtual PStatCollector &get_volume_pcollector();