diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index 1d7b4ed758..c551464960 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/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. diff --git a/panda/src/collide/collisionPlane.h b/panda/src/collide/collisionPlane.h index 232b52c650..4130cf2151 100644 --- a/panda/src/collide/collisionPlane.h +++ b/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; diff --git a/panda/src/collide/collisionSolid.cxx b/panda/src/collide/collisionSolid.cxx index 54a4d36a3f..5f27903b45 100644 --- a/panda/src/collide/collisionSolid.cxx +++ b/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. diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index 2aa2c082d3..4f80cb922a 100644 --- a/panda/src/collide/collisionSolid.h +++ b/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; diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index 7c2a5f2c15..2e0802b350 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/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; +} + /** * */ diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index fe5d1d2d7b..f6cb45d804 100644 --- a/panda/src/collide/collisionSphere.h +++ b/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; diff --git a/panda/src/collide/collisionTube.cxx b/panda/src/collide/collisionTube.cxx index 17e9cd5c3f..cb3f24f737 100644 --- a/panda/src/collide/collisionTube.cxx +++ b/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. */ diff --git a/panda/src/collide/collisionTube.h b/panda/src/collide/collisionTube.h index afc14d71dc..fe6a57c981 100644 --- a/panda/src/collide/collisionTube.h +++ b/panda/src/collide/collisionTube.h @@ -40,6 +40,9 @@ public: INLINE CollisionTube(const CollisionTube ©); virtual CollisionSolid *make_copy(); + virtual PT(CollisionEntry) + test_intersection(const CollisionEntry &entry) const; + virtual void xform(const LMatrix4 &mat); virtual PStatCollector &get_volume_pcollector();