From f66069aed41af9e7b32281eb0021dfb092956761 Mon Sep 17 00:00:00 2001 From: David Rose Date: Tue, 16 Oct 2007 17:02:48 +0000 Subject: [PATCH] parabola collisions --- panda/src/collide/Sources.pp | 3 + panda/src/collide/collide_composite2.cxx | 1 + panda/src/collide/collisionNode.I | 1 + panda/src/collide/collisionParabola.I | 120 +++++++++ panda/src/collide/collisionParabola.cxx | 283 +++++++++++++++++++++ panda/src/collide/collisionParabola.h | 113 ++++++++ panda/src/collide/collisionPlane.cxx | 121 +++++++-- panda/src/collide/collisionPlane.h | 2 + panda/src/collide/collisionPolygon.cxx | 90 +++++++ panda/src/collide/collisionPolygon.h | 2 + panda/src/collide/collisionSolid.cxx | 15 ++ panda/src/collide/collisionSolid.h | 3 + panda/src/collide/collisionSphere.cxx | 112 +++++++- panda/src/collide/collisionSphere.h | 7 +- panda/src/collide/collisionTraverser.cxx | 8 + panda/src/collide/collisionTube.cxx | 105 ++++++++ panda/src/collide/collisionTube.h | 7 +- panda/src/collide/config_collide.cxx | 3 + panda/src/mathutil/Sources.pp | 14 +- panda/src/mathutil/boundingPlane.cxx | 35 +++ panda/src/mathutil/boundingPlane.h | 1 + panda/src/mathutil/mathutil_composite1.cxx | 1 + panda/src/mathutil/parabola.cxx | 25 ++ panda/src/mathutil/parabola.h | 33 +++ panda/src/mathutil/parabola_src.I | 123 +++++++++ panda/src/mathutil/parabola_src.cxx | 75 ++++++ panda/src/mathutil/parabola_src.h | 63 +++++ panda/src/mathutil/plane.h | 1 + panda/src/mathutil/plane_src.cxx | 46 ++++ panda/src/mathutil/plane_src.h | 3 + 30 files changed, 1390 insertions(+), 26 deletions(-) create mode 100644 panda/src/collide/collisionParabola.I create mode 100644 panda/src/collide/collisionParabola.cxx create mode 100644 panda/src/collide/collisionParabola.h create mode 100644 panda/src/mathutil/parabola.cxx create mode 100644 panda/src/mathutil/parabola.h create mode 100644 panda/src/mathutil/parabola_src.I create mode 100644 panda/src/mathutil/parabola_src.cxx create mode 100644 panda/src/mathutil/parabola_src.h diff --git a/panda/src/collide/Sources.pp b/panda/src/collide/Sources.pp index 4078a6673f..4b6ff64592 100644 --- a/panda/src/collide/Sources.pp +++ b/panda/src/collide/Sources.pp @@ -26,6 +26,7 @@ collisionLevelStateBase.I collisionLevelStateBase.h \ collisionLevelState.I collisionLevelState.h \ collisionNode.I collisionNode.h \ + collisionParabola.I collisionParabola.h \ collisionPlane.I collisionPlane.h \ collisionPolygon.I collisionPolygon.h \ collisionFloorMesh.I collisionFloorMesh.h \ @@ -56,6 +57,7 @@ collisionInvSphere.cxx \ collisionLine.cxx \ collisionNode.cxx \ + collisionParabola.cxx \ collisionPlane.cxx \ collisionPolygon.cxx \ collisionFloorMesh.cxx \ @@ -86,6 +88,7 @@ collisionLevelState.I collisionLevelState.h \ collisionLine.I collisionLine.h \ collisionNode.I collisionNode.h \ + collisionParabola.I collisionParabola.h \ collisionPlane.I collisionPlane.h \ collisionPolygon.I collisionPolygon.h \ collisionFloorMesh.I collisionFloorMesh.h \ diff --git a/panda/src/collide/collide_composite2.cxx b/panda/src/collide/collide_composite2.cxx index 155aad71c5..a688af72da 100644 --- a/panda/src/collide/collide_composite2.cxx +++ b/panda/src/collide/collide_composite2.cxx @@ -1,4 +1,5 @@ #include "collisionNode.cxx" +#include "collisionParabola.cxx" #include "collisionPlane.cxx" #include "collisionPolygon.cxx" #include "collisionFloorMesh.cxx" diff --git a/panda/src/collide/collisionNode.I b/panda/src/collide/collisionNode.I index 776757f027..58d452b21f 100644 --- a/panda/src/collide/collisionNode.I +++ b/panda/src/collide/collisionNode.I @@ -113,6 +113,7 @@ get_solid(int n) const { INLINE PT(CollisionSolid) CollisionNode:: modify_solid(int n) { nassertr(n >= 0 && n < get_num_solids(), NULL); + mark_internal_bounds_stale(); return _solids[n].get_write_pointer(); } diff --git a/panda/src/collide/collisionParabola.I b/panda/src/collide/collisionParabola.I new file mode 100644 index 0000000000..21e5d2fa21 --- /dev/null +++ b/panda/src/collide/collisionParabola.I @@ -0,0 +1,120 @@ +// Filename: collisionParabola.I +// Created by: drose (11Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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: CollisionParabola::Default Constructor +// Access: Published +// Description: Creates an invalid parabola. +//////////////////////////////////////////////////////////////////// +INLINE CollisionParabola:: +CollisionParabola() : + _t1(0.0f), _t2(0.0f) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::Constructor +// Access: Published +// Description: Creates a parabola with the endpoints between t1 and +// t2 in the parametric space of the parabola. +//////////////////////////////////////////////////////////////////// +INLINE CollisionParabola:: +CollisionParabola(const Parabolaf ¶bola, float t1, float t2) : + _parabola(parabola), + _t1(t1), _t2(t2) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionParabola:: +CollisionParabola(const CollisionParabola ©) : + _parabola(copy._parabola), + _t1(copy._t1), _t2(copy._t2) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::set_parabola +// Access: Public +// Description: Replaces the parabola specified by this solid. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionParabola:: +set_parabola(const Parabolaf ¶bola) { + _parabola = parabola; + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::get_parabola +// Access: Public +// Description: Returns the parabola specified by this solid. +//////////////////////////////////////////////////////////////////// +INLINE const Parabolaf &CollisionParabola:: +get_parabola() const { + return _parabola; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::set_t1 +// Access: Public +// Description: Changes the starting point on the parabola. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionParabola:: +set_t1(float t1) { + _t1 = t1; + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::get_t1 +// Access: Public +// Description: Returns the starting point on the parabola. +//////////////////////////////////////////////////////////////////// +INLINE float CollisionParabola:: +get_t1() const { + return _t1; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::set_t2 +// Access: Public +// Description: Changes the ending point on the parabola. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionParabola:: +set_t2(float t2) { + _t2 = t2; + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::get_t2 +// Access: Public +// Description: Returns the ending point on the parabola. +//////////////////////////////////////////////////////////////////// +INLINE float CollisionParabola:: +get_t2() const { + return _t2; +} diff --git a/panda/src/collide/collisionParabola.cxx b/panda/src/collide/collisionParabola.cxx new file mode 100644 index 0000000000..ae477f273c --- /dev/null +++ b/panda/src/collide/collisionParabola.cxx @@ -0,0 +1,283 @@ +// Filename: collisionParabola.cxx +// Created by: drose (11Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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 "collisionParabola.h" +#include "collisionEntry.h" +#include "datagram.h" +#include "datagramIterator.h" +#include "bamReader.h" +#include "bamWriter.h" +#include "geom.h" +#include "geomLinestrips.h" +#include "geomVertexWriter.h" +#include "boundingHexahedron.h" +#include "look_at.h" + +PStatCollector CollisionParabola::_volume_pcollector( + "Collision Volumes:CollisionParabola"); +PStatCollector CollisionParabola::_test_pcollector( + "Collision Tests:CollisionParabola"); +TypeHandle CollisionParabola::_type_handle; + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::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 CollisionParabola:: +get_collision_origin() const { + return _parabola.calc_point(_t1); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::make_copy +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +CollisionSolid *CollisionParabola:: +make_copy() { + return new CollisionParabola(*this); +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::test_intersection +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionParabola:: +test_intersection(const CollisionEntry &entry) const { + return entry.get_into()->test_intersection_from_parabola(entry); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::xform +// Access: Public, Virtual +// Description: Transforms the solid by the indicated matrix. +//////////////////////////////////////////////////////////////////// +void CollisionParabola:: +xform(const LMatrix4f &mat) { + _parabola.xform(mat); + + mark_viz_stale(); + mark_internal_bounds_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::get_volume_pcollector +// Access: Public, Virtual +// Description: Returns a PStatCollector that is used to count the +// number of bounding volume tests made against a solid +// of this type in a given frame. +//////////////////////////////////////////////////////////////////// +PStatCollector &CollisionParabola:: +get_volume_pcollector() { + return _volume_pcollector; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::get_test_pcollector +// Access: Public, Virtual +// Description: Returns a PStatCollector that is used to count the +// number of intersection tests made against a solid +// of this type in a given frame. +//////////////////////////////////////////////////////////////////// +PStatCollector &CollisionParabola:: +get_test_pcollector() { + return _test_pcollector; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::output +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +void CollisionParabola:: +output(ostream &out) const { + out << _parabola << ", t1 = " << _t1 << ", t2 = " << _t2; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::compute_internal_bounds +// Access: Protected, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(BoundingVolume) CollisionParabola:: +compute_internal_bounds() const { + LPoint3f p1 = _parabola.calc_point(get_t1()); + LPoint3f p2 = _parabola.calc_point(get_t2()); + LVector3f pdelta = p2 - p1; + + // If p1 and p2 are sufficiently close, just put a sphere around + // them. + float d2 = pdelta.length_squared(); + if (d2 < 10.0f) { + LPoint3f pmid = (p1 + p2) * 0.5f; + return new BoundingSphere(pmid, csqrt(d2) * 0.5f); + } + + // OK, the more general bounding volume. We use BoundingHexahedron + // to define a very thin box that roughly bounds the parabola's arc. + // We must use BoundingHexahedron instead of BoundingBox, because + // the box will not be axis-aligned, and might be inflated too large + // if we insist on using the axis-aligned BoundingBox. + + // We first define "parabola space" as a coordinate space such that + // the YZ plane of parabola space corresponds to the plane of the + // parabola. + + // We have to be explicit about the coordinate system--we + // specifically mean CS_zup_right here, to make the YZ plane. + + LMatrix4f from_parabola; + look_at(from_parabola, pdelta, -_parabola.get_a(), CS_zup_right); + from_parabola.set_row(3, p1); + + // The matrix that computes from world space to parabola space is + // the inverse of that which we just computed. + LMatrix4f to_parabola; + to_parabola.invert_from(from_parabola); + + // Now convert the parabola itself into parabola space. + Parabolaf psp = _parabola; + psp.xform(to_parabola); + + LPoint3f pp2 = psp.calc_point(get_t2()); + float max_y = pp2[1]; + + // We compute a few points along the parabola to attempt to get the + // minmax. + float min_z = p1[2]; + float max_z = p1[2]; + static const int num_points = 4; + for (int i = 0; i < num_points; ++i) { + double t = (double)(i + 1) / (double)(num_points + 1); + LPoint3f p = psp.calc_point(get_t1() + t * (get_t2() - get_t1())); + min_z = min(min_z, p[2]); + max_z = max(max_z, p[2]); + } + + // That gives us a simple bounding volume in parabola space. + PT(BoundingHexahedron) volume = + new BoundingHexahedron(LPoint3f(-0.01, max_y, min_z), LPoint3f(0.01, max_y, min_z), + LPoint3f(0.01, max_y, max_z), LPoint3f(-0.01, max_y, max_z), + LPoint3f(-0.01, 0, min_z), LPoint3f(0.01, 0, min_z), + LPoint3f(0.01, 0, max_z), LPoint3f(-0.01, 0, max_z)); + // And convert that back into real space. + volume->xform(from_parabola); + return volume.p(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::fill_viz_geom +// Access: Protected, Virtual +// Description: Fills the _viz_geom GeomNode up with Geoms suitable +// for rendering this solid. +//////////////////////////////////////////////////////////////////// +void CollisionParabola:: +fill_viz_geom() { + if (collide_cat.is_debug()) { + collide_cat.debug() + << "Recomputing viz for " << *this << "\n"; + } + + static const int num_points = 100; + + PT(GeomVertexData) vdata = new GeomVertexData + ("collision", GeomVertexFormat::get_v3cp(), + Geom::UH_static); + GeomVertexWriter vertex(vdata, InternalName::get_vertex()); + GeomVertexWriter color(vdata, InternalName::get_color()); + + for (int i = 0; i < num_points; i++) { + double t = ((double)i / (double)num_points); + vertex.add_data3f(_parabola.calc_point(_t1 + t * (_t2 - _t1))); + + color.add_data4f(Colorf(1.0f, 1.0f, 1.0f, 0.0f) + + t * Colorf(0.0f, 0.0f, 0.0f, 1.0f)); + } + + PT(GeomLinestrips) line = new GeomLinestrips(Geom::UH_static); + line->add_next_vertices(num_points); + line->close_primitive(); + + PT(Geom) geom = new Geom(vdata); + geom->add_primitive(line); + + _viz_geom->add_geom(geom, get_other_viz_state()); + _bounds_viz_geom->add_geom(geom, get_other_bounds_viz_state()); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::register_with_read_factory +// Access: Public, Static +// Description: Factory method to generate a CollisionParabola object +//////////////////////////////////////////////////////////////////// +void CollisionParabola:: +register_with_read_factory() { + BamReader::get_factory()->register_factory(get_class_type(), make_from_bam); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::write_datagram +// Access: Public +// Description: Function to write the important information in +// the particular object to a Datagram +//////////////////////////////////////////////////////////////////// +void CollisionParabola:: +write_datagram(BamWriter *manager, Datagram &me) { + CollisionSolid::write_datagram(manager, me); + _parabola.write_datagram(me); + me.add_float32(_t1); + me.add_float32(_t2); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::make_from_bam +// Access: Protected +// Description: Factory method to generate a CollisionParabola object +//////////////////////////////////////////////////////////////////// +TypedWritable *CollisionParabola:: +make_from_bam(const FactoryParams ¶ms) { + CollisionParabola *me = new CollisionParabola; + DatagramIterator scan; + BamReader *manager; + + parse_params(params, scan, manager); + me->fillin(scan, manager); + return me; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionParabola::fillin +// Access: Protected +// Description: Function that reads out of the datagram (or asks +// manager to read) all of the data that is needed to +// re-create this object and stores it in the appropiate +// place +//////////////////////////////////////////////////////////////////// +void CollisionParabola:: +fillin(DatagramIterator& scan, BamReader* manager) { + CollisionSolid::fillin(scan, manager); + _parabola.read_datagram(scan); + _t1 = scan.get_float32(); + _t2 = scan.get_float32(); +} diff --git a/panda/src/collide/collisionParabola.h b/panda/src/collide/collisionParabola.h new file mode 100644 index 0000000000..a3dbb66a4c --- /dev/null +++ b/panda/src/collide/collisionParabola.h @@ -0,0 +1,113 @@ +// Filename: collisionParabola.h +// Created by: drose (11Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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 COLLISIONPARABOLA_H +#define COLLISIONPARABOLA_H + +#include "pandabase.h" + +#include "collisionSolid.h" +#include "parabola.h" + +class LensNode; + +//////////////////////////////////////////////////////////////////// +// Class : CollisionParabola +// Description : This defines a parabolic arc, or subset of an arc, +// similar to the path of a projectile or falling +// object. It is finite, having a specific beginning +// and end, but it is infinitely thin. +// +// Think of it as a wire bending from point t1 to point +// t2 along the path of a pre-defined parabola. +//////////////////////////////////////////////////////////////////// +class EXPCL_PANDA_COLLIDE CollisionParabola : public CollisionSolid { +PUBLISHED: + INLINE CollisionParabola(); + INLINE CollisionParabola(const Parabolaf ¶bola, float t1, float t2); + + virtual LPoint3f get_collision_origin() const; + +public: + INLINE CollisionParabola(const CollisionParabola ©); + virtual CollisionSolid *make_copy(); + + virtual PT(CollisionEntry) + test_intersection(const CollisionEntry &entry) const; + + virtual void xform(const LMatrix4f &mat); + + virtual PStatCollector &get_volume_pcollector(); + virtual PStatCollector &get_test_pcollector(); + + virtual void output(ostream &out) const; + +PUBLISHED: + INLINE void set_parabola(const Parabolaf ¶bola); + INLINE const Parabolaf &get_parabola() const; + + INLINE void set_t1(float t1); + INLINE float get_t1() const; + + INLINE void set_t2(float t2); + INLINE float get_t2() const; + +protected: + virtual PT(BoundingVolume) compute_internal_bounds() const; + +protected: + virtual void fill_viz_geom(); + +private: + Parabolaf _parabola; + float _t1, _t2; + + static PStatCollector _volume_pcollector; + static PStatCollector _test_pcollector; + +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, "CollisionParabola", + 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 "collisionParabola.I" + +#endif + + diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index 50387012ba..0b0f7010b2 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/panda/src/collide/collisionPlane.cxx @@ -183,8 +183,15 @@ test_intersection_from_line(const CollisionEntry &entry) const { float t; if (!_plane.intersects_line(t, from_origin, from_direction)) { - // No intersection. - return NULL; + // No intersection. The line is parallel to the plane. + + if (_plane.dist_to_plane(from_origin) > 0.0f) { + // The line is entirely in front of the plane. + return NULL; + } + + // The line is entirely behind the plane. + t = 0.0f; } if (collide_cat.is_debug()) { @@ -222,14 +229,23 @@ test_intersection_from_ray(const CollisionEntry &entry) const { LVector3f from_direction = ray->get_direction() * wrt_mat; float t; - if (!_plane.intersects_line(t, from_origin, from_direction)) { - // No intersection. - return NULL; - } - if (t < 0.0f) { - // The intersection point is before the start of the ray. - return NULL; + if (_plane.dist_to_plane(from_origin) < 0.0f) { + // The origin of the ray is behind the plane, so we don't need to + // test further. + t = 0.0f; + + } else { + if (!_plane.intersects_line(t, from_origin, from_direction)) { + // No intersection. The ray is parallel to the plane. + return NULL; + } + + if (t < 0.0f) { + // The intersection point is before the start of the ray, and so + // the ray is entirely in front of the plane. + return NULL; + } } if (collide_cat.is_debug()) { @@ -268,15 +284,23 @@ test_intersection_from_segment(const CollisionEntry &entry) const { LVector3f from_direction = from_b - from_a; float t; - if (!_plane.intersects_line(t, from_a, from_direction)) { - // No intersection. - return NULL; - } + if (_plane.dist_to_plane(from_a) < 0.0f) { + // The first point of the line segment is behind the plane, so we + // don't need to test further. + t = 0.0f; - 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; + } else { + if (!_plane.intersects_line(t, from_a, from_direction)) { + // No intersection. The line segment is parallel to the plane. + 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. Therefore, the line segment is + // entirely in front of the plane. + return NULL; + } } if (collide_cat.is_debug()) { @@ -296,6 +320,69 @@ test_intersection_from_segment(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPlane::test_intersection_from_parabola +// Access: Public, Virtual +// Description: This is part of the double-dispatch implementation of +// test_intersection(). It is called when the "from" +// object is a parabola. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionPlane:: +test_intersection_from_parabola(const CollisionEntry &entry) const { + const CollisionParabola *parabola; + DCAST_INTO_R(parabola, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + // Convert the parabola into local coordinate space. + Parabolaf local_p(parabola->get_parabola()); + local_p.xform(wrt_mat); + + float t; + if (_plane.dist_to_plane(local_p.calc_point(parabola->get_t1())) < 0.0f) { + // The first point in the parabola is behind the plane, so we + // don't need to test further. + t = parabola->get_t1(); + + } else { + float t1, t2; + if (!get_plane().intersects_parabola(t1, t2, local_p)) { + // No intersection. The infinite parabola is entirely in front + // of the plane. + return NULL; + } + + if (t2 < parabola->get_t1() || t1 > parabola->get_t2()) { + // The intersection points are before the start of the parabola + // or after the end of the parabola. The finite subset of the + // parabola is entirely in front of the plane. + return NULL; + } + + // Choose one of the intersecting points. + t = t1; + if (t < parabola->get_t1()) { + t = t2; + } + } + + 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 = local_p.calc_point(t); + + LVector3f normal = (has_effective_normal() && parabola->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 2d9c2d45b9..79c8f1f8a1 100644 --- a/panda/src/collide/collisionPlane.h +++ b/panda/src/collide/collisionPlane.h @@ -71,6 +71,8 @@ protected: test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_parabola(const CollisionEntry &entry) const; virtual void fill_viz_geom(); diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index a6c1b4c231..466c10a589 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -803,6 +803,96 @@ test_intersection_from_segment(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::test_intersection_from_parabola +// Access: Public, Virtual +// Description: This is part of the double-dispatch implementation of +// test_intersection(). It is called when the "from" +// object is a parabola. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionPolygon:: +test_intersection_from_parabola(const CollisionEntry &entry) const { + if (_points.size() < 3) { + return NULL; + } + + const CollisionParabola *parabola; + DCAST_INTO_R(parabola, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + // Convert the parabola into local coordinate space. + Parabolaf local_p(parabola->get_parabola()); + local_p.xform(wrt_mat); + + float t1, t2; + if (!get_plane().intersects_parabola(t1, t2, local_p)) { + // No intersection. + return NULL; + } + + // No guarantee that t1 < t2. Enforce this. + if (t2 < t1) { + float tx = t1; + t1 = t2; + t2 = tx; + } + + if (t2 < parabola->get_t1() || t1 > parabola->get_t2()) { + // The intersection points are before the start of the parabola + // or after the end of the parabola. + return NULL; + } + + float t = t1; + if (t < parabola->get_t1()) { + t = t2; + } + + LPoint3f plane_point = local_p.calc_point(t); + 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; + if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { + // All points are behind the clip plane. + if (!point_is_inside(p, _points)) { + return NULL; + } + + } else { + 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() && parabola->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::fill_viz_geom // Access: Protected, Virtual diff --git a/panda/src/collide/collisionPolygon.h b/panda/src/collide/collisionPolygon.h index 8badd84b6b..af81441c55 100644 --- a/panda/src/collide/collisionPolygon.h +++ b/panda/src/collide/collisionPolygon.h @@ -90,6 +90,8 @@ protected: test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_parabola(const CollisionEntry &entry) const; virtual void fill_viz_geom(); diff --git a/panda/src/collide/collisionSolid.cxx b/panda/src/collide/collisionSolid.cxx index 1921407cbb..304ce6c83c 100644 --- a/panda/src/collide/collisionSolid.cxx +++ b/panda/src/collide/collisionSolid.cxx @@ -22,6 +22,7 @@ #include "collisionLine.h" #include "collisionRay.h" #include "collisionSegment.h" +#include "collisionParabola.h" #include "collisionEntry.h" #include "boundingSphere.h" #include "datagram.h" @@ -298,6 +299,20 @@ test_intersection_from_segment(const CollisionEntry &) const { return NULL; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSolid::test_intersection_from_parabola +// Access: Protected, Virtual +// Description: This is part of the double-dispatch implementation of +// test_intersection(). It is called when the "from" +// object is a parabola. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSolid:: +test_intersection_from_parabola(const CollisionEntry &) const { + report_undefined_intersection_test(CollisionParabola::get_class_type(), + get_type()); + return NULL; +} + #ifndef NDEBUG class CollisionSolidUndefinedPair { public: diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index d1006d402b..5726b96015 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -112,6 +112,8 @@ protected: test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_parabola(const CollisionEntry &entry) const; static void report_undefined_intersection_test(TypeHandle from_type, TypeHandle into_type); @@ -178,6 +180,7 @@ private: friend class CollisionLine; friend class CollisionRay; friend class CollisionSegment; + friend class CollisionParabola; friend class CollisionHandlerFluidPusher; }; diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index b54d1e7cc8..59f7b91234 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -561,6 +561,51 @@ test_intersection_from_segment(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSphere::test_intersection_from_parabola +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSphere:: +test_intersection_from_parabola(const CollisionEntry &entry) const { + const CollisionParabola *parabola; + DCAST_INTO_R(parabola, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + // Convert the parabola into local coordinate space. + Parabolaf local_p(parabola->get_parabola()); + local_p.xform(wrt_mat); + + double t; + if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(), + local_p.calc_point(parabola->get_t1()), + local_p.calc_point(parabola->get_t2()))) { + // 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 = local_p.calc_point(t); + new_entry->set_surface_point(into_intersection_point); + + if (has_effective_normal() && parabola->get_respect_effective_normal()) { + new_entry->set_surface_normal(get_effective_normal()); + } else { + LVector3f normal = into_intersection_point - get_center(); + normal.normalize(); + new_entry->set_surface_normal(normal); + } + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionSphere::fill_viz_geom // Access: Protected, Virtual @@ -631,7 +676,7 @@ intersects_line(double &t1, double &t2, // A sphere with radius r about point c is defined as all P such // that r^2 = (P - c)^2. - // Subsituting P in the above we have: + // Substituting P in the above we have: // r^2 = (f + td - c)^2 = // (f^2 + ftd - fc + ftd + t^2d^2 - tdc - fc - tdc + c^2) = @@ -682,6 +727,71 @@ intersects_line(double &t1, double &t2, return true; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSphere::intersects_parabola +// Access: Protected +// Description: Determine a point of intersection of a parametric +// parabola with the sphere. +// +// We only consider the segment of the parabola between +// t1 and t2, which has already been computed as +// corresponding to points p1 and p2. If there is an +// intersection, t is set to the parametric point of +// intersection, and true is returned; otherwise, false +// is returned. +//////////////////////////////////////////////////////////////////// +bool CollisionSphere:: +intersects_parabola(double &t, const Parabolaf ¶bola, + double t1, double t2, + const LPoint3f &p1, const LPoint3f &p2) const { + if (t1 == t2) { + // Special case: a single point. + if ((p1 - _center).length_squared() > _radius * _radius) { + // No intersection. + return false; + } + t = t1; + return true; + } + + // To directly test for intersection between a parabola (quadratic) + // and a sphere (also quadratic) requires solving a quartic + // equation. Doable, but hard, and I'm a programmer, not a + // mathematician. So I'll solve it the programmer's way instead, by + // approximating the parabola with a series of line segments. + // Hence, this function works by recursively subdividing the + // parabola as necessary. + + // First, see if the line segment (p1 - p2) comes sufficiently close + // to the parabola. Do this by computing the parametric intervening + // point and comparing its distance from the linear intervening + // point. + double tmid = (t1 + t2) * 0.5; + LPoint3f pmid = parabola.calc_point(tmid); + LPoint3f pmid2 = (p1 + p2) * 0.5f; + + if ((pmid - pmid2).length_squared() > 0.001f) { + // Subdivide. + if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) { + return true; + } + return intersects_parabola(t, parabola, tmid, t2, pmid, p2); + } + + // The line segment is sufficiently close; compare the segment itself. + double t1a, t2a; + if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) { + return false; + } + + if (t2a < 0.0 || t1a > 1.0) { + return false; + } + + t = max(t1a, 0.0); + return true; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionSphere::compute_point // Access: Protected diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index 2b53657364..fd7243388d 100644 --- a/panda/src/collide/collisionSphere.h +++ b/panda/src/collide/collisionSphere.h @@ -20,8 +20,8 @@ #define COLLISIONSPHERE_H #include "pandabase.h" - #include "collisionSolid.h" +#include "parabola.h" //////////////////////////////////////////////////////////////////// // Class : CollisionSphere @@ -74,6 +74,8 @@ protected: test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_parabola(const CollisionEntry &entry) const; virtual void fill_viz_geom(); @@ -81,6 +83,9 @@ protected: bool intersects_line(double &t1, double &t2, const LPoint3f &from, const LVector3f &delta, float inflate_radius) const; + bool intersects_parabola(double &t, const Parabolaf ¶bola, + double t1, double t2, + const LPoint3f &p1, const LPoint3f &p2) const; Vertexf compute_point(float latitude, float longitude) const; private: diff --git a/panda/src/collide/collisionTraverser.cxx b/panda/src/collide/collisionTraverser.cxx index b36993f986..619a7897f8 100644 --- a/panda/src/collide/collisionTraverser.cxx +++ b/panda/src/collide/collisionTraverser.cxx @@ -1268,6 +1268,14 @@ compare_collider_to_solid(CollisionEntry &entry, #ifdef DO_PSTATS ((CollisionSolid *)entry.get_into())->get_volume_pcollector().add_level(1); #endif // DO_PSTATS +#ifndef NDEBUG + if (collide_cat.is_spam()) { + collide_cat.spam(false) + << "Comparing to solid: " << *from_node_gbv + << " to " << *solid_gbv << ", within_solid_bounds = " + << within_solid_bounds << "\n"; + } +#endif // NDEBUG } if (within_solid_bounds) { Colliders::const_iterator ci; diff --git a/panda/src/collide/collisionTube.cxx b/panda/src/collide/collisionTube.cxx index 7e4aa508fe..07844a59f6 100644 --- a/panda/src/collide/collisionTube.cxx +++ b/panda/src/collide/collisionTube.cxx @@ -414,6 +414,59 @@ test_intersection_from_segment(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::test_intersection_from_parabola +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionTube:: +test_intersection_from_parabola(const CollisionEntry &entry) const { + const CollisionParabola *parabola; + DCAST_INTO_R(parabola, entry.get_from(), 0); + + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + // Convert the parabola into local coordinate space. + Parabolaf local_p(parabola->get_parabola()); + local_p.xform(wrt_mat); + + double t; + if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(), + local_p.calc_point(parabola->get_t1()), + local_p.calc_point(parabola->get_t2()))) { + // 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 = local_p.calc_point(t); + set_intersection_point(new_entry, into_intersection_point, 0.0); + + if (has_effective_normal() && parabola->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::fill_viz_geom // Access: Protected, Virtual @@ -773,6 +826,58 @@ sphere_intersects_line(double &t1, double &t2, float center_y, return true; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionTube::intersects_parabola +// Access: Protected +// Description: Determine a point of intersection of a parametric +// parabola with the tube. +// +// We only consider the segment of the parabola between +// t1 and t2, which has already been computed as +// corresponding to points p1 and p2. If there is an +// intersection, t is set to the parametric point of +// intersection, and true is returned; otherwise, false +// is returned. +//////////////////////////////////////////////////////////////////// +bool CollisionTube:: +intersects_parabola(double &t, const Parabolaf ¶bola, + double t1, double t2, + const LPoint3f &p1, const LPoint3f &p2) const { + // I don't even want to think about the math to do this calculation + // directly--it's even worse than sphere-parabola. So I'll use the + // recursive subdivision solution again, just like I did for + // sphere-parabola. + + // First, see if the line segment (p1 - p2) comes sufficiently close + // to the parabola. Do this by computing the parametric intervening + // point and comparing its distance from the linear intervening + // point. + double tmid = (t1 + t2) * 0.5; + LPoint3f pmid = parabola.calc_point(tmid); + LPoint3f pmid2 = (p1 + p2) * 0.5f; + + if ((pmid - pmid2).length_squared() > 0.001f) { + // Subdivide. + if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) { + return true; + } + return intersects_parabola(t, parabola, tmid, t2, pmid, p2); + } + + // The line segment is sufficiently close; compare the segment itself. + double t1a, t2a; + if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) { + return false; + } + + if (t2a < 0.0 || t1a > 1.0) { + return false; + } + + t = max(t1a, 0.0); + return true; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionTube::calculate_surface_point_and_normal // Access: Private diff --git a/panda/src/collide/collisionTube.h b/panda/src/collide/collisionTube.h index 34e97f6944..9a6a676ba3 100644 --- a/panda/src/collide/collisionTube.h +++ b/panda/src/collide/collisionTube.h @@ -20,8 +20,8 @@ #define COLLISIONTUBE_H #include "pandabase.h" - #include "collisionSolid.h" +#include "parabola.h" //////////////////////////////////////////////////////////////////// // Class : CollisionTube @@ -81,6 +81,8 @@ protected: test_intersection_from_ray(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_segment(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_parabola(const CollisionEntry &entry) const; virtual void fill_viz_geom(); @@ -97,6 +99,9 @@ private: bool sphere_intersects_line(double &t1, double &t2, float center_y, const LPoint3f &from, const LVector3f &delta, float inflate_radius) const; + bool intersects_parabola(double &t, const Parabolaf ¶bola, + double t1, double t2, + const LPoint3f &p1, const LPoint3f &p2) const; void calculate_surface_point_and_normal(const LPoint3f &surface_point, double extra_radius, LPoint3f &result_point, diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index e5ef8cced4..75ec8aea9e 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -31,6 +31,7 @@ #include "collisionLine.h" #include "collisionLevelStateBase.h" #include "collisionNode.h" +#include "collisionParabola.h" #include "collisionPlane.h" #include "collisionPolygon.h" #include "collisionFloorMesh.h" @@ -120,6 +121,7 @@ init_libcollide() { CollisionLine::init_type(); CollisionLevelStateBase::init_type(); CollisionNode::init_type(); + CollisionParabola::init_type(); CollisionPlane::init_type(); CollisionPolygon::init_type(); CollisionFloorMesh::init_type(); @@ -139,6 +141,7 @@ init_libcollide() { CollisionInvSphere::register_with_read_factory(); CollisionLine::register_with_read_factory(); CollisionNode::register_with_read_factory(); + CollisionParabola::register_with_read_factory(); CollisionPlane::register_with_read_factory(); CollisionPolygon::register_with_read_factory(); CollisionFloorMesh::register_with_read_factory(); diff --git a/panda/src/mathutil/Sources.pp b/panda/src/mathutil/Sources.pp index 424c530bb9..7fe52e2090 100644 --- a/panda/src/mathutil/Sources.pp +++ b/panda/src/mathutil/Sources.pp @@ -27,11 +27,11 @@ mersenne.h \ omniBoundingVolume.I \ omniBoundingVolume.h \ + parabola.h parabola_src.I parabola_src.cxx parabola_src.h \ perlinNoise.h perlinNoise.I \ perlinNoise2.h perlinNoise2.I \ perlinNoise3.h perlinNoise3.I \ - plane.h plane_src.I plane_src.cxx \ - plane_src.h \ + plane.h plane_src.I plane_src.cxx plane_src.h \ randomizer.h randomizer.I \ rotate_to.h rotate_to_src.cxx \ stackedPerlinNoise2.h stackedPerlinNoise2.I \ @@ -49,14 +49,16 @@ linmath_events.cxx \ mersenne.cxx \ omniBoundingVolume.cxx \ + parabola.cxx \ perlinNoise.cxx \ perlinNoise2.cxx \ perlinNoise3.cxx \ + plane.cxx \ randomizer.cxx \ + rotate_to.cxx \ stackedPerlinNoise2.cxx \ stackedPerlinNoise3.cxx \ - triangulator.cxx \ - plane.cxx rotate_to.cxx + triangulator.cxx #define INSTALL_HEADERS \ boundingHexahedron.I boundingHexahedron.h boundingLine.I \ @@ -71,11 +73,11 @@ linmath_events.h \ mersenne.h \ omniBoundingVolume.I omniBoundingVolume.h \ + parabola.h parabola_src.I parabola_src.cxx parabola_src.h \ perlinNoise.h perlinNoise.I \ perlinNoise2.h perlinNoise2.I \ perlinNoise3.h perlinNoise3.I \ - plane.h plane_src.I plane_src.cxx \ - plane_src.h \ + plane.h plane_src.I plane_src.cxx plane_src.h \ randomizer.h randomizer.I \ rotate_to.h rotate_to_src.cxx \ stackedPerlinNoise2.h stackedPerlinNoise2.I \ diff --git a/panda/src/mathutil/boundingPlane.cxx b/panda/src/mathutil/boundingPlane.cxx index b06a7b882c..6a230ca7d9 100644 --- a/panda/src/mathutil/boundingPlane.cxx +++ b/panda/src/mathutil/boundingPlane.cxx @@ -235,3 +235,38 @@ int BoundingPlane:: contains_plane(const BoundingPlane *plane) const { return IF_possible; } + +//////////////////////////////////////////////////////////////////// +// Function: BoundingPlane::contains_hexahedron +// Access: Protected, Virtual +// Description: Double-dispatch support: called by contains_other() +// when the type we're testing for intersection is known +// to be a hexahedron. +//////////////////////////////////////////////////////////////////// +int BoundingPlane:: +contains_hexahedron(const BoundingHexahedron *hexahedron) const { + nassertr(!is_empty() && !is_infinite(), 0); + nassertr(!hexahedron->is_empty() && !hexahedron->is_infinite(), 0); + + int result = IF_possible | IF_some | IF_all; + + bool all_in = true; + bool all_out = true; + for (int i = 0; i < 8 && (all_in || all_out) ; ++i) { + if (_plane.dist_to_plane(hexahedron->get_point(i)) < 0.0f) { + // This point is inside the plane. + all_out = false; + } else { + // This point is outside the plane. + all_in = false; + } + } + + if (all_out) { + return IF_no_intersection; + } else if (!all_in) { + result &= ~IF_all; + } + + return result; +} diff --git a/panda/src/mathutil/boundingPlane.h b/panda/src/mathutil/boundingPlane.h index f765e0c46a..211332b7ea 100644 --- a/panda/src/mathutil/boundingPlane.h +++ b/panda/src/mathutil/boundingPlane.h @@ -65,6 +65,7 @@ protected: virtual int contains_box(const BoundingBox *box) const; virtual int contains_line(const BoundingLine *line) const; virtual int contains_plane(const BoundingPlane *plane) const; + virtual int contains_hexahedron(const BoundingHexahedron *hexahedron) const; private: Planef _plane; diff --git a/panda/src/mathutil/mathutil_composite1.cxx b/panda/src/mathutil/mathutil_composite1.cxx index 9097ad375d..d315b5901b 100644 --- a/panda/src/mathutil/mathutil_composite1.cxx +++ b/panda/src/mathutil/mathutil_composite1.cxx @@ -7,4 +7,5 @@ #include "finiteBoundingVolume.cxx" #include "geometricBoundingVolume.cxx" #include "omniBoundingVolume.cxx" +#include "parabola.cxx" #include "config_mathutil.cxx" diff --git a/panda/src/mathutil/parabola.cxx b/panda/src/mathutil/parabola.cxx new file mode 100644 index 0000000000..5451cf24b0 --- /dev/null +++ b/panda/src/mathutil/parabola.cxx @@ -0,0 +1,25 @@ +// Filename: parabola.cxx +// Created by: drose (10Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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 "parabola.h" + +#include "fltnames.h" +#include "parabola_src.cxx" + +#include "dblnames.h" +#include "parabola_src.cxx" diff --git a/panda/src/mathutil/parabola.h b/panda/src/mathutil/parabola.h new file mode 100644 index 0000000000..83a96d8162 --- /dev/null +++ b/panda/src/mathutil/parabola.h @@ -0,0 +1,33 @@ +// Filename: parabola.h +// Created by: drose (10Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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 PARABOLA_H +#define PARABOLA_H + +#include "pandabase.h" + +#include "luse.h" +#include "indent.h" + +#include "fltnames.h" +#include "parabola_src.h" + +#include "dblnames.h" +#include "parabola_src.h" + +#endif diff --git a/panda/src/mathutil/parabola_src.I b/panda/src/mathutil/parabola_src.I new file mode 100644 index 0000000000..25ba33efd3 --- /dev/null +++ b/panda/src/mathutil/parabola_src.I @@ -0,0 +1,123 @@ +// Filename: parabola_src.I +// Created by: drose (10Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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: Parabola::Default Constructor +// Access: Published +// Description: Constructs a meaningless degenerate parabola. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL FLOATNAME(Parabola):: +FLOATNAME(Parabola)() : + _a(FLOATNAME(LVecBase3)::zero()), + _b(FLOATNAME(LVecBase3)::zero()), + _c(FLOATNAME(LVecBase3)::zero()) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::Constructor +// Access: Published +// Description: Constructs a parabola given the three points of the +// parametric equation: the acceleration, initial +// velocity, and start point. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL FLOATNAME(Parabola):: +FLOATNAME(Parabola)(const FLOATNAME(LVecBase3) &a, + const FLOATNAME(LVecBase3) &b, + const FLOATNAME(LVecBase3) &c) : + _a(a), _b(b), _c(c) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::Copy Constructor +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL FLOATNAME(Parabola):: +FLOATNAME(Parabola)(const FLOATNAME(Parabola) ©) : + _a(copy._a), + _b(copy._b), + _c(copy._c) +{ +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::Copy Assignment Operator +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL void FLOATNAME(Parabola):: +operator = (const FLOATNAME(Parabola) ©) { + _a = copy._a; + _b = copy._b; + _c = copy._c; +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::Destructor +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL FLOATNAME(Parabola):: +~FLOATNAME(Parabola)() { +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::get_a +// Access: Published +// Description: Returns the first point of the parabola's parametric +// equation: the acceleration. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL const FLOATNAME(LVecBase3) &FLOATNAME(Parabola):: +get_a() const { + return _a; +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::get_b +// Access: Published +// Description: Returns the second point of the parabola's parametric +// equation: the initial velocity. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL const FLOATNAME(LVecBase3) &FLOATNAME(Parabola):: +get_b() const { + return _b; +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::get_c +// Access: Published +// Description: Returns the third point of the parabola's parametric +// equation: the start point. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL const FLOATNAME(LVecBase3) &FLOATNAME(Parabola):: +get_c() const { + return _c; +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::calc_point +// Access: Published +// Description: Computes the point on the parabola at time t. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL FLOATNAME(LPoint3) FLOATNAME(Parabola):: +calc_point(FLOATTYPE t) const { + return _a * t * t + _b * t + _c; +} diff --git a/panda/src/mathutil/parabola_src.cxx b/panda/src/mathutil/parabola_src.cxx new file mode 100644 index 0000000000..ba4fa88bca --- /dev/null +++ b/panda/src/mathutil/parabola_src.cxx @@ -0,0 +1,75 @@ +// Filename: parabola_src.cxx +// Created by: drose (10Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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: Parabola::xform +// Access: Published +// Description: Transforms the parabola by the indicated matrix. +//////////////////////////////////////////////////////////////////// +void FLOATNAME(Parabola):: +xform(const FLOATNAME(LMatrix4) &mat) { + // I'm not really sure if this is the right thing to do here. + _a = mat.xform_vec_general(_a); + _b = mat.xform_vec_general(_b); + _c = mat.xform_point(_c); +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::output +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +void FLOATNAME(Parabola):: +output(ostream &out) const { + out << "Parabola(" << _a << ", " << _b << ", " << _c << ")"; +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::write +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +void FLOATNAME(Parabola):: +write(ostream &out, int indent_level) const { + indent(out, indent_level) << *this << "\n"; +} + +//////////////////////////////////////////////////////////////////// +// Function: Parabola::write_datagram +// Access: Public +// Description: Function to write itself into a datagram +//////////////////////////////////////////////////////////////////// +void FLOATNAME(Parabola):: +write_datagram(Datagram &destination) const { + _a.write_datagram(destination); + _b.write_datagram(destination); + _c.write_datagram(destination); +} + +//////////////////////////////////////////////////////////////////// +// Function: LVecBase4::read_datagram +// Access: Public +// Description: Function to read itself from a datagramIterator +//////////////////////////////////////////////////////////////////// +void FLOATNAME(Parabola):: +read_datagram(DatagramIterator &source) { + _a.read_datagram(source); + _b.read_datagram(source); + _c.read_datagram(source); +} diff --git a/panda/src/mathutil/parabola_src.h b/panda/src/mathutil/parabola_src.h new file mode 100644 index 0000000000..04b294948e --- /dev/null +++ b/panda/src/mathutil/parabola_src.h @@ -0,0 +1,63 @@ +// Filename: parabola_src.h +// Created by: drose (10Oct07) +// +//////////////////////////////////////////////////////////////////// +// +// 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 . +// +//////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////// +// Class : Parabola +// Description : An abstract mathematical description of a parabola, +// particularly useful for describing arcs of +// projectiles. +// +// The parabolic equation, given parametrically here, is +// P = At^2 + Bt + C. +//////////////////////////////////////////////////////////////////// +class EXPCL_PANDA_MATHUTIL FLOATNAME(Parabola) { +PUBLISHED: + INLINE_MATHUTIL FLOATNAME(Parabola)(); + INLINE_MATHUTIL FLOATNAME(Parabola)(const FLOATNAME(LVecBase3) &a, + const FLOATNAME(LVecBase3) &b, + const FLOATNAME(LVecBase3) &c); + INLINE_MATHUTIL FLOATNAME(Parabola)(const FLOATNAME(Parabola) ©); + INLINE_MATHUTIL void operator = (const FLOATNAME(Parabola) ©); + INLINE_MATHUTIL ~FLOATNAME(Parabola)(); + + void xform(const FLOATNAME(LMatrix4) &mat); + + INLINE_MATHUTIL const FLOATNAME(LVecBase3) &get_a() const; + INLINE_MATHUTIL const FLOATNAME(LVecBase3) &get_b() const; + INLINE_MATHUTIL const FLOATNAME(LVecBase3) &get_c() const; + + INLINE_MATHUTIL FLOATNAME(LPoint3) calc_point(FLOATTYPE t) const; + + void output(ostream &out) const; + void write(ostream &out, int indent_level = 0) const; + +public: + void write_datagram(Datagram &destination) const; + void read_datagram(DatagramIterator &source); + +private: + FLOATNAME(LVecBase3) _a, _b, _c; +}; + +inline ostream & +operator << (ostream &out, const FLOATNAME(Parabola) &p) { + p.output(out); + return out; +} + +#include "parabola_src.I" diff --git a/panda/src/mathutil/plane.h b/panda/src/mathutil/plane.h index e96ce5f278..a5406ca2bd 100644 --- a/panda/src/mathutil/plane.h +++ b/panda/src/mathutil/plane.h @@ -25,6 +25,7 @@ #include "indent.h" #include "nearly_zero.h" #include "cmath.h" +#include "parabola.h" class Datagram; class DatagramIterator; diff --git a/panda/src/mathutil/plane_src.cxx b/panda/src/mathutil/plane_src.cxx index 0fa8fa6ad2..e12cc93705 100644 --- a/panda/src/mathutil/plane_src.cxx +++ b/panda/src/mathutil/plane_src.cxx @@ -103,6 +103,52 @@ intersects_plane(FLOATNAME(LPoint3) &from, return true; } +//////////////////////////////////////////////////////////////////// +// Function: Plane::intersects_parabola +// Access: Published +// Description: Determines whether and where the indicated parabola +// intersects with the plane. +// +// If there is no intersection with the plane, the +// function returns false and leaves t1 and t2 +// undefined. If there is an intersection with the +// plane, the function returns true and sets t1 and t2 +// to the parametric value that defines the two points +// of intersection. If the parabola is exactly tangent +// to the plane, then t1 == t2. +//////////////////////////////////////////////////////////////////// +bool FLOATNAME(Plane):: +intersects_parabola(FLOATTYPE &t1, FLOATTYPE &t2, + const FLOATNAME(Parabola) ¶bola) const { + // + // The parabola intersects the plane wherever: + // + // a * t^2 + b * t + c == 0 + // + // where a = normal dot parabola.get_a(), + // b = normal dot parabola.get_b(), + // c = normal dot parabola.get_c() + d. + // + + FLOATNAME(LVector3) normal = get_normal(); + FLOATTYPE a = normal.dot(parabola.get_a()); + FLOATTYPE b = normal.dot(parabola.get_b()); + FLOATTYPE c = normal.dot(parabola.get_c()) + _v.v._3; + + // Now use the quadratic equation to solve for t. + FLOATTYPE discriminant = b * b - 4.0 * a * c; + if (discriminant < 0.0f) { + // No intersection. + return false; + } + + FLOATTYPE sqrd = csqrt(discriminant); + + t1 = (-b - sqrd) / (2.0 * a); + t2 = (-b + sqrd) / (2.0 * a); + return true; +} + //////////////////////////////////////////////////////////////////// // Function: Plane::output // Access: Published diff --git a/panda/src/mathutil/plane_src.h b/panda/src/mathutil/plane_src.h index 639fd8e343..92b7b6bfb8 100644 --- a/panda/src/mathutil/plane_src.h +++ b/panda/src/mathutil/plane_src.h @@ -59,6 +59,9 @@ PUBLISHED: FLOATNAME(LVector3) &delta, const FLOATNAME(Plane) &other) const; + bool intersects_parabola(FLOATTYPE &t1, FLOATTYPE &t2, + const FLOATNAME(Parabola) ¶bola) const; + void output(ostream &out) const; void write(ostream &out, int indent_level = 0) const; };