From 347c04032bfa32f95c5c2aaa992ef257d08096e9 Mon Sep 17 00:00:00 2001 From: David Rose Date: Wed, 5 Jan 2005 16:57:30 +0000 Subject: [PATCH] add CollisionLine, compute normals for ray, line, and segment intersection tests --- panda/src/collide/Sources.pp | 13 +- panda/src/collide/collide_composite1.cxx | 1 + panda/src/collide/collisionLine.I | 64 +++++++++ panda/src/collide/collisionLine.cxx | 161 +++++++++++++++++++++++ panda/src/collide/collisionLine.h | 81 ++++++++++++ panda/src/collide/collisionPlane.cxx | 89 ++++++++++++- panda/src/collide/collisionPlane.h | 4 + panda/src/collide/collisionPolygon.cxx | 73 +++++++++- panda/src/collide/collisionPolygon.h | 2 + panda/src/collide/collisionRay.I | 1 + panda/src/collide/collisionRay.cxx | 36 ++--- panda/src/collide/collisionSolid.cxx | 15 +++ panda/src/collide/collisionSolid.h | 3 + panda/src/collide/collisionSphere.cxx | 67 +++++++++- panda/src/collide/collisionSphere.h | 2 + panda/src/collide/collisionTube.cxx | 94 ++++++++++++- panda/src/collide/collisionTube.h | 2 + panda/src/collide/config_collide.cxx | 3 + 18 files changed, 677 insertions(+), 34 deletions(-) create mode 100644 panda/src/collide/collisionLine.I create mode 100644 panda/src/collide/collisionLine.cxx create mode 100644 panda/src/collide/collisionLine.h diff --git a/panda/src/collide/Sources.pp b/panda/src/collide/Sources.pp index aac22002c6..a25489470c 100644 --- a/panda/src/collide/Sources.pp +++ b/panda/src/collide/Sources.pp @@ -18,11 +18,12 @@ collisionHandlerPhysical.I collisionHandlerPhysical.h \ collisionHandlerPusher.I collisionHandlerPusher.h \ collisionHandlerQueue.h \ + collisionLine.I collisionLine.h \ collisionLevelState.I collisionLevelState.h \ collisionNode.I collisionNode.h \ collisionPlane.I collisionPlane.h \ - collisionPolygon.I collisionPolygon.h collisionRay.I \ - collisionRay.h \ + collisionPolygon.I collisionPolygon.h \ + collisionRay.I collisionRay.h \ collisionRecorder.I collisionRecorder.h \ collisionSegment.I collisionSegment.h \ collisionSolid.I collisionSolid.h \ @@ -42,9 +43,11 @@ collisionHandlerPusher.cxx \ collisionHandlerQueue.cxx \ collisionLevelState.cxx \ + collisionLine.cxx \ collisionNode.cxx \ collisionPlane.cxx \ - collisionPolygon.cxx collisionRay.cxx \ + collisionPolygon.cxx \ + collisionRay.cxx \ collisionRecorder.cxx \ collisionSegment.cxx \ collisionSolid.cxx \ @@ -64,9 +67,11 @@ collisionHandlerPusher.I collisionHandlerPusher.h \ collisionHandlerQueue.h \ collisionLevelState.I collisionLevelState.h \ + collisionLine.I collisionLine.h \ collisionNode.I collisionNode.h \ collisionPlane.I collisionPlane.h \ - collisionPolygon.I collisionPolygon.h collisionRay.I collisionRay.h \ + collisionPolygon.I collisionPolygon.h \ + collisionRay.I collisionRay.h \ collisionRecorder.I collisionRecorder.h \ collisionSegment.I collisionSegment.h \ collisionSolid.I collisionSolid.h \ diff --git a/panda/src/collide/collide_composite1.cxx b/panda/src/collide/collide_composite1.cxx index ca96ec2a65..fd99c24d78 100644 --- a/panda/src/collide/collide_composite1.cxx +++ b/panda/src/collide/collide_composite1.cxx @@ -8,5 +8,6 @@ #include "collisionHandlerPusher.cxx" #include "collisionHandlerQueue.cxx" #include "collisionLevelState.cxx" +#include "collisionLine.cxx" #include "collisionNode.cxx" diff --git a/panda/src/collide/collisionLine.I b/panda/src/collide/collisionLine.I new file mode 100644 index 0000000000..23cda80fea --- /dev/null +++ b/panda/src/collide/collisionLine.I @@ -0,0 +1,64 @@ +// Filename: collisionLine.I +// Created by: drose (05Jan05) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001 - 2004, Disney Enterprises, Inc. All rights reserved +// +// All use of this software is subject to the terms of the Panda 3d +// Software license. You should have received a copy of this license +// along with this source code; you will also find a current copy of +// the license at http://etc.cmu.edu/panda3d/docs/license/ . +// +// To contact the maintainers of this program write to +// panda3d-general@lists.sourceforge.net . +// +//////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::Default Constructor +// Access: Public +// Description: Creates an invalid line. This isn't terribly useful; +// it's expected that the user will subsequently adjust +// the line via set_origin()/set_direction() or +// set_from_lens(). +//////////////////////////////////////////////////////////////////// +INLINE CollisionLine:: +CollisionLine() { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionLine:: +CollisionLine(const LPoint3f &origin, const LVector3f &direction) : + CollisionRay(origin, direction) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionLine:: +CollisionLine(float ox, float oy, float oz, + float dx, float dy, float dz) : + CollisionRay(ox, oy, oz, dx, dy, dz) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionLine:: +CollisionLine(const CollisionLine ©) : + CollisionRay(copy) +{ +} diff --git a/panda/src/collide/collisionLine.cxx b/panda/src/collide/collisionLine.cxx new file mode 100644 index 0000000000..dbb0d2d813 --- /dev/null +++ b/panda/src/collide/collisionLine.cxx @@ -0,0 +1,161 @@ +// Filename: collisionLine.cxx +// Created by: drose (05Jan05) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001 - 2004, Disney Enterprises, Inc. All rights reserved +// +// All use of this software is subject to the terms of the Panda 3d +// Software license. You should have received a copy of this license +// along with this source code; you will also find a current copy of +// the license at http://etc.cmu.edu/panda3d/docs/license/ . +// +// To contact the maintainers of this program write to +// panda3d-general@lists.sourceforge.net . +// +//////////////////////////////////////////////////////////////////// + +#include "collisionLine.h" +#include "collisionHandler.h" +#include "collisionEntry.h" +#include "config_collide.h" +#include "geom.h" +#include "geomNode.h" +#include "geomLinestrip.h" +#include "boundingLine.h" +#include "lensNode.h" +#include "lens.h" +#include "datagram.h" +#include "datagramIterator.h" +#include "bamReader.h" +#include "bamWriter.h" + +TypeHandle CollisionLine::_type_handle; + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::make_copy +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +CollisionSolid *CollisionLine:: +make_copy() { + return new CollisionLine(*this); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::test_intersection +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionLine:: +test_intersection(const CollisionEntry &entry) const { + return entry.get_into()->test_intersection_from_line(entry); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::output +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +void CollisionLine:: +output(ostream &out) const { + out << "line, o (" << get_origin() << "), d (" << get_direction() << ")"; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::fill_viz_geom +// Access: Protected, Virtual +// Description: Fills the _viz_geom GeomNode up with Geoms suitable +// for rendering this solid. +//////////////////////////////////////////////////////////////////// +void CollisionLine:: +fill_viz_geom() { + if (collide_cat.is_debug()) { + collide_cat.debug() + << "Recomputing viz for " << *this << "\n"; + } + + GeomLinestrip *line = new GeomLinestrip; + PTA_Vertexf verts; + PTA_Colorf colors; + PTA_int lengths; + + static const int num_points = 100; + static const double scale = 100.0; + + verts.reserve(num_points); + colors.reserve(num_points); + + for (int i = 0; i < num_points; i++) { + double t = ((double)i / (double)num_points - 0.5) * 2.0; + verts.push_back(get_origin() + t * scale * get_direction()); + + colors.push_back(Colorf(1.0f, 1.0f, 1.0f, 1.0f) + + fabs(t) * Colorf(0.0f, 0.0f, 0.0f, -1.0f)); + } + line->set_coords(verts); + line->set_colors(colors, G_PER_VERTEX); + + lengths.push_back(num_points-1); + line->set_lengths(lengths); + + line->set_num_prims(1); + + _viz_geom->add_geom(line, get_other_viz_state()); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::register_with_read_factory +// Access: Public, Static +// Description: Tells the BamReader how to create objects of type +// CollisionLine. +//////////////////////////////////////////////////////////////////// +void CollisionLine:: +register_with_read_factory() { + BamReader::get_factory()->register_factory(get_class_type(), make_from_bam); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::write_datagram +// Access: Public, Virtual +// Description: Writes the contents of this object to the datagram +// for shipping out to a Bam file. +//////////////////////////////////////////////////////////////////// +void CollisionLine:: +write_datagram(BamWriter *manager, Datagram &dg) { + CollisionRay::write_datagram(manager, dg); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::make_from_bam +// Access: Protected, Static +// Description: This function is called by the BamReader's factory +// when a new object of type CollisionLine is encountered +// in the Bam file. It should create the CollisionLine +// and extract its information from the file. +//////////////////////////////////////////////////////////////////// +TypedWritable *CollisionLine:: +make_from_bam(const FactoryParams ¶ms) { + CollisionLine *node = new CollisionLine(); + DatagramIterator scan; + BamReader *manager; + + parse_params(params, scan, manager); + node->fillin(scan, manager); + + return node; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionLine::fillin +// Access: Protected +// Description: This internal function is called by make_from_bam to +// read in all of the relevant data from the BamFile for +// the new CollisionLine. +//////////////////////////////////////////////////////////////////// +void CollisionLine:: +fillin(DatagramIterator &scan, BamReader *manager) { + CollisionRay::fillin(scan, manager); +} diff --git a/panda/src/collide/collisionLine.h b/panda/src/collide/collisionLine.h new file mode 100644 index 0000000000..f1d4b10928 --- /dev/null +++ b/panda/src/collide/collisionLine.h @@ -0,0 +1,81 @@ +// Filename: collisionLine.h +// Created by: drose (05Jan05) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001 - 2004, Disney Enterprises, Inc. All rights reserved +// +// All use of this software is subject to the terms of the Panda 3d +// Software license. You should have received a copy of this license +// along with this source code; you will also find a current copy of +// the license at http://etc.cmu.edu/panda3d/docs/license/ . +// +// To contact the maintainers of this program write to +// panda3d-general@lists.sourceforge.net . +// +//////////////////////////////////////////////////////////////////// + +#ifndef COLLISIONLINE_H +#define COLLISIONLINE_H + +#include "pandabase.h" + +#include "collisionRay.h" + +/////////////////////////////////////////////////////////////////// +// Class : CollisionLine +// Description : An infinite line, similar to a CollisionRay, except +// that it extends in both directions. It is, however, +// directional. +//////////////////////////////////////////////////////////////////// +class EXPCL_PANDA CollisionLine : public CollisionRay { +PUBLISHED: + INLINE CollisionLine(); + INLINE CollisionLine(const LPoint3f &origin, const LVector3f &direction); + INLINE CollisionLine(float ox, float oy, float oz, + float dx, float dy, float dz); + +public: + INLINE CollisionLine(const CollisionLine ©); + virtual CollisionSolid *make_copy(); + + virtual PT(CollisionEntry) + test_intersection(const CollisionEntry &entry) const; + + virtual void output(ostream &out) const; + +protected: + virtual void fill_viz_geom(); + +public: + static void register_with_read_factory(); + virtual void write_datagram(BamWriter *manager, Datagram &dg); + +protected: + static TypedWritable *make_from_bam(const FactoryParams ¶ms); + void fillin(DatagramIterator &scan, BamReader *manager); + +public: + static TypeHandle get_class_type() { + return _type_handle; + } + static void init_type() { + CollisionRay::init_type(); + register_type(_type_handle, "CollisionLine", + CollisionRay::get_class_type()); + } + virtual TypeHandle get_type() const { + return get_class_type(); + } + virtual TypeHandle force_init_type() {init_type(); return get_class_type();} + +private: + static TypeHandle _type_handle; +}; + +#include "collisionLine.I" + +#endif + + diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index 4cb7ec6105..d233fb9682 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/panda/src/collide/collisionPlane.cxx @@ -21,7 +21,9 @@ #include "collisionHandler.h" #include "collisionEntry.h" #include "collisionSphere.h" +#include "collisionLine.h" #include "collisionRay.h" +#include "collisionSegment.h" #include "config_collide.h" #include "pointerToArray.h" @@ -138,6 +140,44 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPlane::test_intersection_from_line +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionPlane:: +test_intersection_from_line(const CollisionEntry &entry) const { + const CollisionLine *line; + DCAST_INTO_R(line, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = line->get_origin() * wrt_mat; + LVector3f from_direction = line->get_direction() * wrt_mat; + + float t; + if (!_plane.intersects_line(t, from_origin, from_direction)) { + // 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); + + LPoint3f into_intersection_point = from_origin + t * from_direction; + + LVector3f normal = (has_effective_normal() && line->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); + + new_entry->set_surface_normal(normal); + new_entry->set_surface_point(into_intersection_point); + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPlane::test_intersection_from_ray // Access: Public, Virtual @@ -166,8 +206,8 @@ test_intersection_from_ray(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); @@ -181,6 +221,51 @@ test_intersection_from_ray(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPlane::test_intersection_from_segment +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionPlane:: +test_intersection_from_segment(const CollisionEntry &entry) const { + const CollisionSegment *segment; + DCAST_INTO_R(segment, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_a = segment->get_point_a() * wrt_mat; + LPoint3f from_b = segment->get_point_b() * wrt_mat; + LVector3f from_direction = from_b - from_a; + + float t; + if (!_plane.intersects_line(t, from_a, from_direction)) { + // No intersection. + return NULL; + } + + if (t < 0.0f || t > 1.0f) { + // The intersection point is before the start of the segment or + // after the end of the segment. + 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); + + LPoint3f into_intersection_point = from_a + t * from_direction; + + LVector3f normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); + + new_entry->set_surface_normal(normal); + new_entry->set_surface_point(into_intersection_point); + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPlane::fill_viz_geom // Access: Protected, Virtual diff --git a/panda/src/collide/collisionPlane.h b/panda/src/collide/collisionPlane.h index bae95ed333..9c7a0ef153 100644 --- a/panda/src/collide/collisionPlane.h +++ b/panda/src/collide/collisionPlane.h @@ -60,7 +60,11 @@ protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) + test_intersection_from_line(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) test_intersection_from_ray(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_segment(const CollisionEntry &entry) const; virtual void fill_viz_geom(); diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index 7662ac899d..8a7e1723ae 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -20,6 +20,7 @@ #include "collisionHandler.h" #include "collisionEntry.h" #include "collisionSphere.h" +#include "collisionLine.h" #include "collisionRay.h" #include "collisionSegment.h" #include "config_collide.h" @@ -552,6 +553,70 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::test_intersection_from_line +// Access: Protected, Virtual +// Description: This is part of the double-dispatch implementation of +// test_intersection(). It is called when the "from" +// object is a line. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionPolygon:: +test_intersection_from_line(const CollisionEntry &entry) const { + if (_points.size() < 3) { + return NULL; + } + + const CollisionLine *line; + DCAST_INTO_R(line, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = line->get_origin() * wrt_mat; + LVector3f from_direction = line->get_direction() * wrt_mat; + + float t; + if (!get_plane().intersects_line(t, from_origin, from_direction)) { + // No intersection. + return NULL; + } + + LPoint3f plane_point = from_origin + t * from_direction; + LPoint2f p = to_2d(plane_point); + + const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); + if (cpa != (ClipPlaneAttrib *)NULL) { + // We have a clip plane; apply it. + Points new_points; + apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform()); + if (new_points.size() < 3) { + return NULL; + } + if (!point_is_inside(p, new_points)) { + return NULL; + } + + } else { + // No clip plane is in effect. Do the default test. + if (!point_is_inside(p, _points)) { + 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); + + LVector3f normal = (has_effective_normal() && line->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); + + new_entry->set_surface_normal(normal); + new_entry->set_surface_point(plane_point); + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::test_intersection_from_ray // Access: Protected, Virtual @@ -608,8 +673,8 @@ test_intersection_from_ray(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); @@ -679,8 +744,8 @@ test_intersection_from_segment(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); diff --git a/panda/src/collide/collisionPolygon.h b/panda/src/collide/collisionPolygon.h index 7e865a94f3..adf14c379b 100644 --- a/panda/src/collide/collisionPolygon.h +++ b/panda/src/collide/collisionPolygon.h @@ -72,6 +72,8 @@ protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) + test_intersection_from_line(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; diff --git a/panda/src/collide/collisionRay.I b/panda/src/collide/collisionRay.I index 18306ea640..d2cf18b49e 100644 --- a/panda/src/collide/collisionRay.I +++ b/panda/src/collide/collisionRay.I @@ -16,6 +16,7 @@ // //////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////// // Function: CollisionRay::Default Constructor // Access: Public diff --git a/panda/src/collide/collisionRay.cxx b/panda/src/collide/collisionRay.cxx index 1d3c0d25a2..288c220ce4 100644 --- a/panda/src/collide/collisionRay.cxx +++ b/panda/src/collide/collisionRay.cxx @@ -87,7 +87,7 @@ get_collision_origin() const { //////////////////////////////////////////////////////////////////// void CollisionRay:: output(ostream &out) const { - out << "ray, o (" << _origin << "), d (" << _direction << ")"; + out << "ray, o (" << get_origin() << "), d (" << get_direction() << ")"; } //////////////////////////////////////////////////////////////////// @@ -149,29 +149,33 @@ fill_viz_geom() { << "Recomputing viz for " << *this << "\n"; } - GeomLinestrip *ray = new GeomLinestrip; + GeomLinestrip *line = new GeomLinestrip; PTA_Vertexf verts; PTA_Colorf colors; PTA_int lengths; - - #define NUM_POINTS 100 - verts.reserve(NUM_POINTS); - colors.reserve(NUM_POINTS); - for (int i = 0; i < NUM_POINTS; i++) { - verts.push_back(_origin + (double)i * _direction); - colors.push_back(Colorf(1.0f, 1.0f, 1.0f, 1.0f)+ - ((double)i / 100.0) * Colorf(0.0f, 0.0f, 0.0f, -1.0f)); + static const int num_points = 100; + static const double scale = 100.0; + + verts.reserve(num_points); + colors.reserve(num_points); + + for (int i = 0; i < num_points; i++) { + double t = ((double)i / (double)num_points); + verts.push_back(get_origin() + t * scale * get_direction()); + + colors.push_back(Colorf(1.0f, 1.0f, 1.0f, 1.0f) + + t * Colorf(0.0f, 0.0f, 0.0f, -1.0f)); } - ray->set_coords(verts); - ray->set_colors(colors, G_PER_VERTEX); + line->set_coords(verts); + line->set_colors(colors, G_PER_VERTEX); - lengths.push_back(NUM_POINTS-1); - ray->set_lengths(lengths); + lengths.push_back(num_points-1); + line->set_lengths(lengths); - ray->set_num_prims(1); + line->set_num_prims(1); - _viz_geom->add_geom(ray, get_other_viz_state()); + _viz_geom->add_geom(line, get_other_viz_state()); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionSolid.cxx b/panda/src/collide/collisionSolid.cxx index 18bfd6b50a..03354b0967 100644 --- a/panda/src/collide/collisionSolid.cxx +++ b/panda/src/collide/collisionSolid.cxx @@ -19,6 +19,7 @@ #include "collisionSolid.h" #include "config_collide.h" #include "collisionSphere.h" +#include "collisionLine.h" #include "collisionRay.h" #include "collisionSegment.h" @@ -154,6 +155,20 @@ test_intersection_from_sphere(const CollisionEntry &) const { return NULL; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSolid::test_intersection_from_line +// Access: Protected, Virtual +// Description: This is part of the double-dispatch implementation of +// test_intersection(). It is called when the "from" +// object is a line. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSolid:: +test_intersection_from_line(const CollisionEntry &) const { + report_undefined_intersection_test(CollisionLine::get_class_type(), + get_type()); + return NULL; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionSolid::test_intersection_from_ray // Access: Protected, Virtual diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index 631d714940..182405c3af 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -87,6 +87,8 @@ protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) + test_intersection_from_line(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; @@ -143,6 +145,7 @@ private: static TypeHandle _type_handle; friend class CollisionSphere; + friend class CollisionLine; friend class CollisionRay; friend class CollisionSegment; }; diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index 0e6f469be0..8646cc4cf7 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -18,6 +18,7 @@ #include "collisionSphere.h" +#include "collisionLine.h" #include "collisionRay.h" #include "collisionSegment.h" #include "collisionHandler.h" @@ -164,6 +165,48 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSphere::test_intersection_from_line +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSphere:: +test_intersection_from_line(const CollisionEntry &entry) const { + const CollisionLine *line; + DCAST_INTO_R(line, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = line->get_origin() * wrt_mat; + LVector3f from_direction = line->get_direction() * wrt_mat; + + double t1, t2; + if (!intersects_line(t1, t2, from_origin, from_direction)) { + // 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); + + LPoint3f into_intersection_point = from_origin + t1 * from_direction; + new_entry->set_surface_point(into_intersection_point); + + if (has_effective_normal() && line->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + } else { + LVector3f normal = into_intersection_point - _center; + normal.normalize(); + new_entry->set_surface_normal(normal); + } + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionSphere::test_intersection_from_ray // Access: Public, Virtual @@ -192,8 +235,8 @@ test_intersection_from_ray(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); @@ -209,6 +252,14 @@ test_intersection_from_ray(const CollisionEntry &entry) const { } new_entry->set_surface_point(into_intersection_point); + if (has_effective_normal() && ray->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + } else { + LVector3f normal = into_intersection_point - _center; + normal.normalize(); + new_entry->set_surface_normal(normal); + } + return new_entry; } @@ -242,8 +293,8 @@ test_intersection_from_segment(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); @@ -259,6 +310,14 @@ test_intersection_from_segment(const CollisionEntry &entry) const { } new_entry->set_surface_point(into_intersection_point); + if (has_effective_normal() && segment->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + } else { + LVector3f normal = into_intersection_point - _center; + normal.normalize(); + new_entry->set_surface_normal(normal); + } + return new_entry; } diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index 6b0c0e8558..af9450169c 100644 --- a/panda/src/collide/collisionSphere.h +++ b/panda/src/collide/collisionSphere.h @@ -61,6 +61,8 @@ protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) + test_intersection_from_line(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; diff --git a/panda/src/collide/collisionTube.cxx b/panda/src/collide/collisionTube.cxx index 86626f6c0e..315d8ff6fc 100644 --- a/panda/src/collide/collisionTube.cxx +++ b/panda/src/collide/collisionTube.cxx @@ -17,6 +17,10 @@ //////////////////////////////////////////////////////////////////// #include "collisionTube.h" +#include "collisionSphere.h" +#include "collisionLine.h" +#include "collisionRay.h" +#include "collisionSegment.h" #include "collisionHandler.h" #include "collisionEntry.h" #include "config_collide.h" @@ -186,6 +190,56 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::test_intersection_from_line +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionTube:: +test_intersection_from_line(const CollisionEntry &entry) const { + const CollisionLine *line; + DCAST_INTO_R(line, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = line->get_origin() * wrt_mat; + LVector3f from_direction = line->get_direction() * wrt_mat; + + double t1, t2; + if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { + // 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); + + LPoint3f into_intersection_point = from_origin + t1 * from_direction; + set_intersection_point(new_entry, into_intersection_point, 0.0); + + if (has_effective_normal() && line->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + + } else { + LVector3f normal = into_intersection_point * _inv_mat; + if (normal[1] > _length) { + // The point is within the top endcap. + normal[1] -= _length; + } else if (normal[1] > 0.0f) { + // The point is within the cylinder body. + normal[1] = 0; + } + normal = normalize(normal * _mat); + new_entry->set_surface_normal(normal); + } + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionTube::test_intersection_from_ray // Access: Public, Virtual @@ -214,8 +268,8 @@ test_intersection_from_ray(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); @@ -231,6 +285,22 @@ test_intersection_from_ray(const CollisionEntry &entry) const { } set_intersection_point(new_entry, into_intersection_point, 0.0); + if (has_effective_normal() && ray->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + + } else { + LVector3f normal = into_intersection_point * _inv_mat; + if (normal[1] > _length) { + // The point is within the top endcap. + normal[1] -= _length; + } else if (normal[1] > 0.0f) { + // The point is within the cylinder body. + normal[1] = 0; + } + normal = normalize(normal * _mat); + new_entry->set_surface_normal(normal); + } + return new_entry; } @@ -264,8 +334,8 @@ test_intersection_from_segment(const CollisionEntry &entry) const { if (collide_cat.is_debug()) { collide_cat.debug() - << "intersection detected from " << entry.get_from_node_path() << " into " - << entry.get_into_node_path() << "\n"; + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; } PT(CollisionEntry) new_entry = new CollisionEntry(entry); @@ -281,6 +351,22 @@ test_intersection_from_segment(const CollisionEntry &entry) const { } set_intersection_point(new_entry, into_intersection_point, 0.0); + if (has_effective_normal() && segment->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + + } else { + LVector3f normal = into_intersection_point * _inv_mat; + if (normal[1] > _length) { + // The point is within the top endcap. + normal[1] -= _length; + } else if (normal[1] > 0.0f) { + // The point is within the cylinder body. + normal[1] = 0; + } + normal = normalize(normal * _mat); + new_entry->set_surface_normal(normal); + } + return new_entry; } diff --git a/panda/src/collide/collisionTube.h b/panda/src/collide/collisionTube.h index 89c728914e..56e5128db3 100644 --- a/panda/src/collide/collisionTube.h +++ b/panda/src/collide/collisionTube.h @@ -70,6 +70,8 @@ protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) + test_intersection_from_line(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index 59260e40ce..ed9ae41df9 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -25,6 +25,7 @@ #include "collisionHandlerPhysical.h" #include "collisionHandlerPusher.h" #include "collisionHandlerQueue.h" +#include "collisionLine.h" #include "collisionNode.h" #include "collisionPlane.h" #include "collisionPolygon.h" @@ -85,6 +86,7 @@ init_libcollide() { CollisionHandlerPhysical::init_type(); CollisionHandlerPusher::init_type(); CollisionHandlerQueue::init_type(); + CollisionLine::init_type(); CollisionNode::init_type(); CollisionPlane::init_type(); CollisionPolygon::init_type(); @@ -101,6 +103,7 @@ init_libcollide() { // Registration of writeable object's creation // functions with BamReader's factory + CollisionLine::register_with_read_factory(); CollisionNode::register_with_read_factory(); CollisionPlane::register_with_read_factory(); CollisionPolygon::register_with_read_factory();