From af62a2ad3c3e8075630e0efc47d85b7cd955837a Mon Sep 17 00:00:00 2001 From: David Rose Date: Sat, 27 Sep 2003 00:09:06 +0000 Subject: [PATCH] introduce CollisionTube --- panda/src/collide/Sources.pp | 14 +- panda/src/collide/collide_composite2.cxx | 1 + panda/src/collide/collisionPlane.I | 2 +- panda/src/collide/collisionPolygon.I | 7 +- panda/src/collide/collisionPolygon.h | 9 +- panda/src/collide/collisionSphere.I | 7 +- panda/src/collide/collisionSphere.cxx | 12 +- panda/src/collide/collisionSphere.h | 8 +- panda/src/collide/collisionTube.I | 157 +++++ panda/src/collide/collisionTube.cxx | 691 ++++++++++++++++++++++ panda/src/collide/collisionTube.h | 131 ++++ panda/src/collide/collisionVisualizer.cxx | 56 +- panda/src/collide/config_collide.cxx | 2 + 13 files changed, 1058 insertions(+), 39 deletions(-) create mode 100644 panda/src/collide/collisionTube.I create mode 100644 panda/src/collide/collisionTube.cxx create mode 100644 panda/src/collide/collisionTube.h diff --git a/panda/src/collide/Sources.pp b/panda/src/collide/Sources.pp index e41dc01ab3..34b8972bf2 100644 --- a/panda/src/collide/Sources.pp +++ b/panda/src/collide/Sources.pp @@ -24,9 +24,10 @@ collisionRay.h \ collisionRecorder.I collisionRecorder.h \ collisionSegment.I collisionSegment.h \ - collisionSolid.I collisionSolid.h collisionSphere.I \ - collisionSphere.h \ + collisionSolid.I collisionSolid.h \ + collisionSphere.I collisionSphere.h \ collisionTraverser.I collisionTraverser.h \ + collisionTube.I collisionTube.h \ collisionVisualizer.I collisionVisualizer.h \ config_collide.h @@ -44,8 +45,10 @@ collisionPolygon.cxx collisionRay.cxx \ collisionRecorder.cxx \ collisionSegment.cxx \ - collisionSolid.cxx collisionSphere.cxx \ + collisionSolid.cxx \ + collisionSphere.cxx \ collisionTraverser.cxx \ + collisionTube.cxx \ collisionVisualizer.cxx \ config_collide.cxx @@ -63,9 +66,10 @@ collisionPolygon.I collisionPolygon.h collisionRay.I collisionRay.h \ collisionRecorder.I collisionRecorder.h \ collisionSegment.I collisionSegment.h \ - collisionSolid.I collisionSolid.h collisionSphere.I \ - collisionSphere.h \ + collisionSolid.I collisionSolid.h \ + collisionSphere.I collisionSphere.h \ collisionTraverser.I collisionTraverser.h \ + collisionTube.I collisionTube.h \ collisionVisualizer.I collisionVisualizer.h #define IGATESCAN all diff --git a/panda/src/collide/collide_composite2.cxx b/panda/src/collide/collide_composite2.cxx index cdfb18b786..36de6589ce 100644 --- a/panda/src/collide/collide_composite2.cxx +++ b/panda/src/collide/collide_composite2.cxx @@ -6,4 +6,5 @@ #include "collisionSolid.cxx" #include "collisionSphere.cxx" #include "collisionTraverser.cxx" +#include "collisionTube.cxx" #include "collisionVisualizer.cxx" diff --git a/panda/src/collide/collisionPlane.I b/panda/src/collide/collisionPlane.I index e1abac5295..efdbfba61a 100644 --- a/panda/src/collide/collisionPlane.I +++ b/panda/src/collide/collisionPlane.I @@ -18,7 +18,7 @@ //////////////////////////////////////////////////////////////////// // Function: CollisionPlane::Default Constructor -// Access: Public +// Access: Protected // Description: This is only for the convenience of CollisionPolygon. // Normally, you should not attempt to create an // uninitialized CollisionPlane. diff --git a/panda/src/collide/collisionPolygon.I b/panda/src/collide/collisionPolygon.I index 53b7058437..fff6e9f673 100644 --- a/panda/src/collide/collisionPolygon.I +++ b/panda/src/collide/collisionPolygon.I @@ -60,11 +60,12 @@ CollisionPolygon(const LPoint3f *begin, const LPoint3f *end) { //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::Constructor -// Access: Public -// Description: +// Access: Private +// Description: Creates an invalid polygon. Only used when reading +// from a bam file. //////////////////////////////////////////////////////////////////// INLINE CollisionPolygon:: -CollisionPolygon(void) { +CollisionPolygon() { } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionPolygon.h b/panda/src/collide/collisionPolygon.h index d14aa27a3f..160a82e4d0 100644 --- a/panda/src/collide/collisionPolygon.h +++ b/panda/src/collide/collisionPolygon.h @@ -19,11 +19,11 @@ #ifndef COLLISIONPOLYGON_H #define COLLISIONPOLYGON_H -#include +#include "pandabase.h" #include "collisionPlane.h" -#include +#include "vector_LPoint2f.h" /////////////////////////////////////////////////////////////////// // Class : CollisionPolygon @@ -37,6 +37,9 @@ PUBLISHED: const LPoint3f &c, const LPoint3f &d); INLINE CollisionPolygon(const LPoint3f *begin, const LPoint3f *end); +private: + INLINE CollisionPolygon(); + public: CollisionPolygon(const CollisionPolygon ©); @@ -56,10 +59,8 @@ public: virtual void write(ostream &out, int indent_level = 0) const; protected: - INLINE CollisionPolygon(void); virtual BoundingVolume *recompute_bound(); -protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) diff --git a/panda/src/collide/collisionSphere.I b/panda/src/collide/collisionSphere.I index e082b81815..2c4ec6e938 100644 --- a/panda/src/collide/collisionSphere.I +++ b/panda/src/collide/collisionSphere.I @@ -40,9 +40,10 @@ CollisionSphere(float cx, float cy, float cz, float radius) : } //////////////////////////////////////////////////////////////////// -// Function: CollisionSphere::Constructor -// Access: Public -// Description: +// Function: CollisionSphere::Default constructor +// Access: Private +// Description: Creates an invalid sphere. Only used when reading +// from a bam file. //////////////////////////////////////////////////////////////////// INLINE CollisionSphere:: CollisionSphere() { diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index 88fbf921c7..21f61f4bcb 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -64,7 +64,7 @@ xform(const LMatrix4f &mat) { _center = _center * mat; // This is a little cheesy and fails miserably in the presence of a - // non-proportionate scale. + // non-uniform scale. LVector3f radius_v = LVector3f(_radius, 0.0f, 0.0f) * mat; _radius = length(radius_v); @@ -200,8 +200,12 @@ test_intersection_from_ray(const CollisionEntry &entry) const { LPoint3f into_intersection_point; if (t1 < 0.0) { - into_intersection_point = from_origin + t2 * from_direction; + // Point a is within the sphere. The first intersection point is + // point a itself. + into_intersection_point = from_origin; } else { + // Point a is outside the sphere. The first intersection point is + // at t1. into_intersection_point = from_origin + t1 * from_direction; } new_entry->set_into_intersection_point(into_intersection_point); @@ -282,8 +286,8 @@ fill_viz_geom() { //////////////////////////////////////////////////////////////////// // Function: CollisionSphere::intersects_line -// Access: Protected -// Description: Determine the point(s) of intersect of a parametric +// Access: Private +// Description: Determine the point(s) of intersection of a parametric // line with the sphere. The line is infinite in both // directions, and passes through "from" and from+delta. // If the line does not intersect the sphere, the diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index 361270dcb0..42e76452b0 100644 --- a/panda/src/collide/collisionSphere.h +++ b/panda/src/collide/collisionSphere.h @@ -19,7 +19,7 @@ #ifndef COLLISIONSPHERE_H #define COLLISIONSPHERE_H -#include +#include "pandabase.h" #include "collisionSolid.h" @@ -32,6 +32,9 @@ PUBLISHED: INLINE CollisionSphere(const LPoint3f ¢er, float radius); INLINE CollisionSphere(float cx, float cy, float cz, float radius); +private: + INLINE CollisionSphere(); + public: INLINE CollisionSphere(const CollisionSphere ©); virtual CollisionSolid *make_copy(); @@ -53,10 +56,8 @@ PUBLISHED: INLINE float get_radius() const; protected: - INLINE CollisionSphere(void); virtual BoundingVolume *recompute_bound(); -protected: virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) @@ -66,6 +67,7 @@ protected: virtual void fill_viz_geom(); +private: bool intersects_line(double &t1, double &t2, const LPoint3f &from, const LVector3f &delta) const; diff --git a/panda/src/collide/collisionTube.I b/panda/src/collide/collisionTube.I new file mode 100644 index 0000000000..f52960810c --- /dev/null +++ b/panda/src/collide/collisionTube.I @@ -0,0 +1,157 @@ +// Filename: collisionTube.I +// Created by: drose (25Sep03) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001, 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://www.panda3d.org/license.txt . +// +// To contact the maintainers of this program write to +// panda3d@yahoogroups.com . +// +//////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionTube:: +CollisionTube(const LPoint3f &a, const LPoint3f &b, float radius) : + _a(a), _b(b), _radius(radius) +{ + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionTube:: +CollisionTube(float ax, float ay, float az, + float bx, float by, float bz, + float radius) : + _a(ax, ay, az), _b(bx, by, bz), _radius(radius) +{ + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::Default constructor +// Access: Private +// Description: Creates an invalid tube. Only used when reading +// from a bam file. +//////////////////////////////////////////////////////////////////// +INLINE CollisionTube:: +CollisionTube() { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionTube:: +CollisionTube(const CollisionTube ©) : + CollisionSolid(copy), + _a(copy._a), + _b(copy._b), + _radius(copy._radius) +{ + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::set_point_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionTube:: +set_point_a(const LPoint3f &a) { + _a = a; + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::set_point_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionTube:: +set_point_a(float x, float y, float z) { + set_point_a(LPoint3f(x, y, z)); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::get_point_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const LPoint3f &CollisionTube:: +get_point_a() const { + return _a; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::set_point_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionTube:: +set_point_b(const LPoint3f &b) { + _b = b; + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::set_point_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionTube:: +set_point_b(float x, float y, float z) { + set_point_b(LPoint3f(x, y, z)); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::get_point_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const LPoint3f &CollisionTube:: +get_point_b() const { + return _b; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::set_radius +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionTube:: +set_radius(float radius) { + _radius = radius; + + // We don't need to call recalc_internals(), since the radius + // doesn't change either of those properties. + mark_bound_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::get_radius +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE float CollisionTube:: +get_radius() const { + return _radius; +} + diff --git a/panda/src/collide/collisionTube.cxx b/panda/src/collide/collisionTube.cxx new file mode 100644 index 0000000000..042ebe15d9 --- /dev/null +++ b/panda/src/collide/collisionTube.cxx @@ -0,0 +1,691 @@ +// Filename: collisionTube.cxx +// Created by: drose (25Sep03) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001, 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://www.panda3d.org/license.txt . +// +// To contact the maintainers of this program write to +// panda3d@yahoogroups.com . +// +//////////////////////////////////////////////////////////////////// + +#include "collisionTube.h" +#include "collisionHandler.h" +#include "collisionEntry.h" +#include "config_collide.h" + +#include "look_at.h" +#include "geom.h" +#include "geomNode.h" +#include "geomTristrip.h" +#include "geometricBoundingVolume.h" +#include "datagram.h" +#include "datagramIterator.h" +#include "bamReader.h" +#include "bamWriter.h" + +TypeHandle CollisionTube::_type_handle; + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::make_copy +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +CollisionSolid *CollisionTube:: +make_copy() { + return new CollisionTube(*this); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::xform +// Access: Public, Virtual +// Description: Transforms the solid by the indicated matrix. +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +xform(const LMatrix4f &mat) { + _a = _a * mat; + _b = _b * mat; + + // This is a little cheesy and fails miserably in the presence of a + // non-uniform scale. + LVector3f radius_v = LVector3f(_radius, 0.0f, 0.0f) * mat; + _radius = length(radius_v); + + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::get_collision_origin +// Access: Public, Virtual +// Description: Returns the point in space deemed to be the "origin" +// of the solid for collision purposes. The closest +// intersection point to this origin point is considered +// to be the most significant. +//////////////////////////////////////////////////////////////////// +LPoint3f CollisionTube:: +get_collision_origin() const { + return get_point_a(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::output +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +output(ostream &out) const { + out << "tube, a (" << _a << "), b (" << _b << "), r " << _radius; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::recompute_bound +// Access: Protected, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +BoundingVolume *CollisionTube:: +recompute_bound() { + BoundingVolume *bound = BoundedObject::recompute_bound(); + + if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) { + GeometricBoundingVolume *gbound; + DCAST_INTO_R(gbound, bound, bound); + + LVector3f vec = (_b - _a); + if (vec.normalize()) { + // The bounding volume includes both endpoints, plus a little + // bit more to include the radius in both directions. + LPoint3f points[2]; + points[0] = _a - vec * _radius; + points[1] = _b + vec * _radius; + + gbound->around(points, points + 2); + + } else { + // Both endpoints are coincident; therefore, the bounding volume + // is a sphere. + BoundingSphere sphere(_a, _radius); + gbound->extend_by(&sphere); + } + } + + return bound; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::test_intersection_from_sphere +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionTube:: +test_intersection_from_sphere(const CollisionEntry &entry) const { + const CollisionSphere *sphere; + DCAST_INTO_R(sphere, entry.get_from(), 0); + + LPoint3f from_a = sphere->get_center() * entry.get_wrt_space(); + LPoint3f from_b = from_a; + + if (entry.has_from_velocity()) { + from_a = (sphere->get_center() - entry.get_from_velocity()) * entry.get_wrt_space(); + } + + LVector3f from_direction = from_b - from_a; + + LVector3f from_radius_v = + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space(); + float 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 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() << " into " + << entry.get_into_node_path() << "\n"; + } + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + + LPoint3f into_intersection_point; + if (t1 < 0.0) { + // Point a is within the tube. The first intersection point is + // point a itself. + into_intersection_point = from_a; + } else { + // Point a is outside the tube, and point b is either inside the + // 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); + + return new_entry; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::test_intersection_from_ray +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionTube:: +test_intersection_from_ray(const CollisionEntry &entry) const { + const CollisionRay *ray; + DCAST_INTO_R(ray, entry.get_from(), 0); + + LPoint3f from_origin = ray->get_origin() * entry.get_wrt_space(); + LVector3f from_direction = ray->get_direction() * entry.get_wrt_space(); + + double t1, t2; + if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { + // No intersection. + return NULL; + } + + if (t2 < 0.0) { + // Both intersection points are before the start of the ray. + return NULL; + } + + if (collide_cat.is_debug()) { + collide_cat.debug() + << "intersection detected from " << *entry.get_from_node() << " into " + << entry.get_into_node_path() << "\n"; + } + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + + LPoint3f into_intersection_point; + if (t1 < 0.0) { + // Point a is within the tube. The first intersection point is + // point a itself. + into_intersection_point = from_origin; + } else { + // Point a is outside the tube. The first intersection point is + // at t1. + into_intersection_point = from_origin + t1 * from_direction; + } + new_entry->set_into_intersection_point(into_intersection_point); + + return new_entry; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::test_intersection_from_segment +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionTube:: +test_intersection_from_segment(const CollisionEntry &entry) const { + const CollisionSegment *segment; + DCAST_INTO_R(segment, entry.get_from(), 0); + + LPoint3f from_a = segment->get_point_a() * entry.get_wrt_space(); + LPoint3f from_b = segment->get_point_b() * entry.get_wrt_space(); + LVector3f from_direction = from_b - from_a; + + double t1, t2; + if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) { + // No intersection. + return NULL; + } + + if (t2 < 0.0 || t1 > 1.0) { + // Both intersection points are 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() << " into " + << entry.get_into_node_path() << "\n"; + } + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + + LPoint3f into_intersection_point; + if (t1 < 0.0) { + // Point a is within the tube. The first intersection point is + // point a itself. + into_intersection_point = from_a; + } else { + // Point a is outside the tube, and point b is either inside the + // 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); + + return new_entry; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::fill_viz_geom +// Access: Protected, Virtual +// Description: Fills the _viz_geom GeomNode up with Geoms suitable +// for rendering this solid. +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +fill_viz_geom() { + if (collide_cat.is_debug()) { + collide_cat.debug() + << "Recomputing viz for " << *this << "\n"; + } + + // Generate the vertices such that we draw a tube with one endpoint + // at (0, 0, 0), and another at (0, length, 0). Then we'll rotate + // and translate it into place with the appropriate look_at matrix. + LVector3f direction = (_b - _a); + float length = direction.length(); + + PTA_Vertexf verts; + PTA_int lengths; + + // Generate the first endcap. + static const int num_slices = 8; + static const int num_rings = 4; + int ri, si; + for (ri = 0; ri < num_rings; ri++) { + for (si = 0; si <= num_slices; si++) { + verts.push_back(calc_sphere1_vertex(ri, si, num_rings, num_slices)); + verts.push_back(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices)); + } + lengths.push_back((num_slices + 1) * 2); + } + + // Now the cylinder sides. + for (si = 0; si <= num_slices; si++) { + verts.push_back(calc_sphere1_vertex(num_rings, si, num_rings, num_slices)); + verts.push_back(calc_sphere2_vertex(num_rings, si, num_rings, num_slices, + length)); + } + lengths.push_back((num_slices + 1) * 2); + + // And the second endcap. + for (ri = num_rings - 1; ri >= 0; ri--) { + for (si = 0; si <= num_slices; si++) { + verts.push_back(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length)); + verts.push_back(calc_sphere2_vertex(ri, si, num_rings, num_slices, length)); + } + lengths.push_back((num_slices + 1) * 2); + } + + // Now transform the vertices to their actual location. + LMatrix4f mat; + look_at(mat, direction, LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right); + mat.set_row(3, _a); + + for (size_t i = 0; i < verts.size(); i++) { + verts[i] = verts[i] * mat; + } + + GeomTristrip *tube = new GeomTristrip; + tube->set_coords(verts); + tube->set_num_prims(lengths.size()); + tube->set_lengths(lengths); + + _viz_geom->add_geom(tube, get_solid_viz_state()); + _viz_geom->add_geom(tube, get_wireframe_viz_state()); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::recalc_internals +// Access: Private +// Description: Should be called internally to recompute the matrix +// and length when the properties of the tube have +// changed. +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +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); + mat.set_row(3, _a); + _inv_mat.invert_from(mat); + + mark_viz_stale(); + mark_bound_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::calc_sphere1_vertex +// Access: Private +// Description: Calculates a particular vertex on the surface of the +// first endcap hemisphere, for use in generating the +// viz geometry. +//////////////////////////////////////////////////////////////////// +Vertexf CollisionTube:: +calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) { + float r = (float)ri / (float)num_rings; + float s = (float)si / (float)num_slices; + + // Find the point on the rim, based on the slice. + float theta = s * 2.0f * MathNumbers::pi_f; + float x_rim = cos(theta); + float z_rim = sin(theta); + + // Now pull that point in towards the pole, based on the ring. + float phi = r * 0.5f * MathNumbers::pi_f; + float to_pole = sin(phi); + + float x = _radius * x_rim * to_pole; + float y = -_radius * cos(phi); + float z = _radius * z_rim * to_pole; + + return Vertexf(x, y, z); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::calc_sphere2_vertex +// Access: Private +// Description: Calculates a particular vertex on the surface of the +// second endcap hemisphere, for use in generating the +// viz geometry. +//////////////////////////////////////////////////////////////////// +Vertexf CollisionTube:: +calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices, + float length) { + float r = (float)ri / (float)num_rings; + float s = (float)si / (float)num_slices; + + // Find the point on the rim, based on the slice. + float theta = s * 2.0f * MathNumbers::pi_f; + float x_rim = cos(theta); + float z_rim = sin(theta); + + // Now pull that point in towards the pole, based on the ring. + float phi = r * 0.5f * MathNumbers::pi_f; + float to_pole = sin(phi); + + float x = _radius * x_rim * to_pole; + float y = length + _radius * cos(phi); + float z = _radius * z_rim * to_pole; + + return Vertexf(x, y, z); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::intersects_line +// Access: Private +// Description: Determine the point(s) of intersection of a parametric +// line with the tube. The line is infinite in both +// directions, and passes through "from" and from+delta. +// If the line does not intersect the tube, the +// function returns false, and t1 and t2 are undefined. +// If it does intersect the tube, it returns true, and +// t1 and t2 are set to the points along the equation +// from+t*delta that correspond to the two points of +// intersection. +//////////////////////////////////////////////////////////////////// +bool CollisionTube:: +intersects_line(double &t1, double &t2, + LPoint3f from, LVector3f delta, float inflate_radius) const { + // Convert the line into our canonical coordinate space: the tube is + // aligned with the y axis. + from = from * _inv_mat; + delta = delta * _inv_mat; + + float radius = _radius + inflate_radius; + + // Now project the line into the X-Z plane to test for intersection + // with a 2-d circle around the origin. The equation for this is + // very similar to the formula for the intersection of a line with a + // sphere; see CollisionSphere::intersects_line() for the complete + // derivation. It's a little bit simpler because the circle is + // centered on the origin. + LVector2f from2(from[0], from[2]); + LVector2f delta2(delta[0], delta[2]); + + double A = dot(delta2, delta2); + + if (IS_NEARLY_ZERO(A)) { + // If the delta2 is 0, the line is perpendicular to the X-Z plane. + // The whole line intersects with the infinite cylinder if the + // point is within the circle. + if (from2.dot(from2) > radius * radius) { + // Nope, the 2-d point is outside the circle, so no + // intersection. + return false; + } + + if (IS_NEARLY_ZERO(delta[1])) { + // Actually, the whole delta vector is 0, so the line is just a + // point. In this case, (since we have already shown the point + // is within the infinite cylinder), we intersect if and only if + // the three-dimensional point is between the endcaps. + if (from[1] < -radius || from[1] > _length + radius) { + // Way out. + return false; + } + if (from[1] < 0.0f) { + // Possibly within the first endcap. + if (from.dot(from) > radius * radius) { + return false; + } + } else if (from[1] > _length) { + // Possibly within the second endcap. + from[1] -= _length; + if (from.dot(from) > radius * radius) { + return false; + } + } + + // The point is within the tube! + t1 = t2 = 0.0; + return true; + } + + // The 2-d point is within the circle, so compute our intersection + // points to include the entire vertical slice of the cylinder. + t1 = (-radius - from[1]) / delta[1]; + t2 = (_length + radius - from[1]) / delta[1]; + + } else { + // The line is not perpendicular to the X-Z plane, so its + // projection into the plane is 2-d line. Test that 2-d line for + // intersection with the circular projection of the cylinder. + + double B = 2.0f * dot(delta2, from2); + double fc_d2 = dot(from2, from2); + double C = fc_d2 - radius * radius; + + double radical = B*B - 4.0*A*C; + + if (IS_NEARLY_ZERO(radical)) { + // Tangent. + t1 = t2 = -B / (2.0*A); + + } else if (radical < 0.0) { + // No real roots: no intersection with the line. + return false; + + } else { + double reciprocal_2A = 1.0 / (2.0 * A); + double sqrt_radical = sqrtf(radical); + t1 = ( -B - sqrt_radical ) * reciprocal_2A; + t2 = ( -B + sqrt_radical ) * reciprocal_2A; + } + } + + // Now we need to verify that the intersection points fall within + // the length of the cylinder. + float t1_y = from[1] + t1 * delta[1]; + float t2_y = from[1] + t2 * delta[1]; + + if (t1_y < -radius && t2_y < -radius) { + // Both points are way off the bottom of the tube; no + // intersection. + return false; + } else if (t1_y > _length + radius && t2_y > _length + radius) { + // Both points are way off the top of the tube; no intersection. + return false; + } + + if (t1_y < 0.0f) { + // The starting point is off the bottom of the tube. Test the + // line against the first endcap. + double t1a, t2a; + if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) { + // If there's no intersection with the endcap, there can't be an + // intersection with the cylinder. + return false; + } + t1 = t1a; + + } else if (t1_y > _length) { + // The starting point is off the top of the tube. Test the + // line against the second endcap. + double t1b, t2b; + if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) { + // If there's no intersection with the endcap, there can't be an + // intersection with the cylinder. + return false; + } + t1 = t1b; + } + + if (t2_y < 0.0f) { + // The ending point is off the bottom of the tube. Test the + // line against the first endcap. + double t1a, t2a; + if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) { + // If there's no intersection with the endcap, there can't be an + // intersection with the cylinder. + return false; + } + t2 = t2a; + + } else if (t2_y > _length) { + // The ending point is off the top of the tube. Test the + // line against the second endcap. + double t1b, t2b; + if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) { + // If there's no intersection with the endcap, there can't be an + // intersection with the cylinder. + return false; + } + t2 = t2b; + } + + return true; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::sphere_intersects_line +// Access: Private +// Description: After confirming that the line intersects an infinite +// cylinder, test whether it intersects one or the other +// endcaps. The y parameter specifies the center of the +// sphere (and hence the particular endcap. +//////////////////////////////////////////////////////////////////// +bool CollisionTube:: +sphere_intersects_line(double &t1, double &t2, float center_y, + const LPoint3f &from, const LVector3f &delta, + float inflate_radius) const { + // See CollisionSphere::intersects_line() for a derivation of the + // formula here. + float radius = _radius + inflate_radius; + + double A = dot(delta, delta); + + nassertr(A != 0.0, false); + + LVector3f fc = from; + fc[1] -= center_y; + double B = 2.0f* dot(delta, fc); + double fc_d2 = dot(fc, fc); + double C = fc_d2 - radius * radius; + + double radical = B*B - 4.0*A*C; + + if (IS_NEARLY_ZERO(radical)) { + // Tangent. + t1 = t2 = -B / (2.0 * A); + return true; + + } else if (radical < 0.0) { + // No real roots: no intersection with the line. + return false; + } + + double reciprocal_2A = 1.0 / (2.0 * A); + double sqrt_radical = sqrtf(radical); + t1 = ( -B - sqrt_radical ) * reciprocal_2A; + t2 = ( -B + sqrt_radical ) * reciprocal_2A; + + return true; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::register_with_read_factory +// Access: Public, Static +// Description: Tells the BamReader how to create objects of type +// CollisionTube. +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +register_with_read_factory() { + BamReader::get_factory()->register_factory(get_class_type(), make_from_bam); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::write_datagram +// Access: Public, Virtual +// Description: Writes the contents of this object to the datagram +// for shipping out to a Bam file. +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +write_datagram(BamWriter *manager, Datagram &dg) { + CollisionSolid::write_datagram(manager, dg); + _a.write_datagram(dg); + _b.write_datagram(dg); + dg.add_float32(_radius); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::make_from_bam +// Access: Protected, Static +// Description: This function is called by the BamReader's factory +// when a new object of type CollisionTube is encountered +// in the Bam file. It should create the CollisionTube +// and extract its information from the file. +//////////////////////////////////////////////////////////////////// +TypedWritable *CollisionTube:: +make_from_bam(const FactoryParams ¶ms) { + CollisionTube *node = new CollisionTube(); + DatagramIterator scan; + BamReader *manager; + + parse_params(params, scan, manager); + node->fillin(scan, manager); + + return node; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::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 CollisionTube. +//////////////////////////////////////////////////////////////////// +void CollisionTube:: +fillin(DatagramIterator &scan, BamReader *manager) { + CollisionSolid::fillin(scan, manager); + _a.read_datagram(scan); + _b.read_datagram(scan); + _radius = scan.get_float32(); + recalc_internals(); +} diff --git a/panda/src/collide/collisionTube.h b/panda/src/collide/collisionTube.h new file mode 100644 index 0000000000..800b586f2b --- /dev/null +++ b/panda/src/collide/collisionTube.h @@ -0,0 +1,131 @@ +// Filename: collisionTube.h +// Created by: drose (25Sep03) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001, 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://www.panda3d.org/license.txt . +// +// To contact the maintainers of this program write to +// panda3d@yahoogroups.com . +// +//////////////////////////////////////////////////////////////////// + +#ifndef COLLISIONTUBE_H +#define COLLISIONTUBE_H + +#include "pandabase.h" + +#include "collisionSolid.h" + +/////////////////////////////////////////////////////////////////// +// Class : CollisionTube +// Description : This implements a solid roughly in cylindrical shape. +// It's not called a CollisionCylinder because it's not +// a true cylinder; specifically, it has rounded ends +// instead of flat ends. It looks more like a Contac +// pill. +//////////////////////////////////////////////////////////////////// +class EXPCL_PANDA CollisionTube : public CollisionSolid { +PUBLISHED: + INLINE CollisionTube(const LPoint3f &a, const LPoint3f &db, + float radius); + INLINE CollisionTube(float ax, float ay, float az, + float bx, float by, float bz, + float radius); + +private: + INLINE CollisionTube(); + +public: + INLINE CollisionTube(const CollisionTube ©); + virtual CollisionSolid *make_copy(); + + virtual void xform(const LMatrix4f &mat); + virtual LPoint3f get_collision_origin() const; + + virtual void output(ostream &out) const; + +PUBLISHED: + INLINE void set_point_a(const LPoint3f &a); + INLINE void set_point_a(float x, float y, float z); + INLINE const LPoint3f &get_point_a() const; + + INLINE void set_point_b(const LPoint3f &b); + INLINE void set_point_b(float x, float y, float z); + INLINE const LPoint3f &get_point_b() const; + + INLINE void set_radius(float radius); + INLINE float get_radius() const; + +protected: + virtual BoundingVolume *recompute_bound(); + +protected: + virtual PT(CollisionEntry) + test_intersection_from_sphere(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(); + +private: + void recalc_internals(); + + Vertexf calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices); + Vertexf calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices, + float length); + + bool intersects_line(double &t1, double &t2, + LPoint3f from, LVector3f delta, + float inflate_radius) const; + bool sphere_intersects_line(double &t1, double &t2, float center_y, + const LPoint3f &from, const LVector3f &delta, + float inflate_radius) const; + +private: + LPoint3f _a, _b; + float _radius; + + // These are derived from the above. + LMatrix4f _inv_mat; + float _length; + +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() { + CollisionSolid::init_type(); + register_type(_type_handle, "CollisionTube", + CollisionSolid::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 "collisionTube.I" + +#endif + + diff --git a/panda/src/collide/collisionVisualizer.cxx b/panda/src/collide/collisionVisualizer.cxx index 47c6223ad7..b4761e0cdb 100644 --- a/panda/src/collide/collisionVisualizer.cxx +++ b/panda/src/collide/collisionVisualizer.cxx @@ -27,6 +27,8 @@ #include "depthOffsetAttrib.h" #include "colorScaleAttrib.h" #include "transparencyAttrib.h" +#include "geomLine.h" +#include "geomSphere.h" #ifdef DO_COLLISION_RECORDING @@ -155,7 +157,7 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { // Now draw all of the detected points. if (!viz_info._points.empty()) { - CPT(RenderState) line_state = RenderState::make_empty(); + CPT(RenderState) empty_state = RenderState::make_empty(); PTA_Colorf colors; colors.push_back(Colorf(1.0f, 0.0f, 0.0f, 1.0f)); @@ -164,19 +166,38 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { Points::const_iterator pi; for (pi = viz_info._points.begin(); pi != viz_info._points.end(); ++pi) { const CollisionPoint &point = (*pi); - PT(GeomLine) line = new GeomLine; + + // Draw a tiny sphere at the collision point. + { + PT(GeomSphere) sphere = new GeomSphere; + PTA_Vertexf verts; + verts.push_back(point._point); + verts.push_back(point._point + LVector3f(0.1f, 0.0f, 0.0f)); + sphere->set_coords(verts); + sphere->set_colors(colors, G_OVERALL); + sphere->set_num_prims(1); + CullableObject *object = + new CullableObject(sphere, empty_state, xform_data._render_transform); + + trav->get_cull_handler()->record_object(object); + } + + // Draw the normal vector at the collision point. + if (!point._normal.almost_equal(LVector3f::zero())) { + PT(GeomLine) line = new GeomLine; - PTA_Vertexf verts; - verts.push_back(point._point); - verts.push_back(point._point + point._normal); - line->set_coords(verts); - line->set_colors(colors, G_PER_VERTEX); - line->set_num_prims(1); - - CullableObject *object = - new CullableObject(line, line_state, xform_data._render_transform); - - trav->get_cull_handler()->record_object(object); + PTA_Vertexf verts; + verts.push_back(point._point); + verts.push_back(point._point + point._normal); + line->set_coords(verts); + line->set_colors(colors, G_PER_VERTEX); + line->set_num_prims(1); + + CullableObject *object = + new CullableObject(line, empty_state, xform_data._render_transform); + + trav->get_cull_handler()->record_object(object); + } } } @@ -232,11 +253,14 @@ collision_tested(const CollisionEntry &entry, bool detected) { if (detected) { viz_info._solids[entry.get_into()]._detected_count++; - if (entry.has_into_intersection_point() && - entry.has_into_surface_normal()) { + if (entry.has_into_intersection_point()) { CollisionPoint p; p._point = entry.get_into_intersection_point(); - p._normal = entry.get_into_surface_normal(); + if (entry.has_into_surface_normal()) { + p._normal = entry.get_into_surface_normal(); + } else { + p._normal = LVector3f::zero(); + } viz_info._points.push_back(p); } diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index 94c280eddc..24502d9b33 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -32,6 +32,7 @@ #include "collisionSegment.h" #include "collisionSolid.h" #include "collisionSphere.h" +#include "collisionTube.h" #include "collisionVisualizer.h" #include "dconfig.h" @@ -77,6 +78,7 @@ init_libcollide() { CollisionSegment::init_type(); CollisionSolid::init_type(); CollisionSphere::init_type(); + CollisionTube::init_type(); #ifdef DO_COLLISION_RECORDING CollisionRecorder::init_type();