diff --git a/panda/src/collide/collisionEntry.I b/panda/src/collide/collisionEntry.I index 45d640cfc2..ec2c7c15b2 100644 --- a/panda/src/collide/collisionEntry.I +++ b/panda/src/collide/collisionEntry.I @@ -457,6 +457,21 @@ get_wrt_prev_mat() const { return get_wrt_prev_space()->get_mat(); } +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_into_clip_planes +// Access: Public +// Description: Returns the ClipPlaneAttrib, if any, that is applied +// to the into_node_path, or NULL if there is no clip +// plane in effect. +//////////////////////////////////////////////////////////////////// +INLINE const ClipPlaneAttrib *CollisionEntry:: +get_into_clip_planes() const { + if ((_flags & F_checked_clip_planes) == 0) { + ((CollisionEntry *)this)->check_clip_planes(); + } + return _into_clip_planes; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionEntry::test_intersection // Access: Private diff --git a/panda/src/collide/collisionEntry.cxx b/panda/src/collide/collisionEntry.cxx index d7a1582141..e68798d65c 100644 --- a/panda/src/collide/collisionEntry.cxx +++ b/panda/src/collide/collisionEntry.cxx @@ -17,6 +17,7 @@ //////////////////////////////////////////////////////////////////// #include "collisionEntry.h" +#include "dcast.h" TypeHandle CollisionEntry::_type_handle; @@ -33,6 +34,7 @@ CollisionEntry(const CollisionEntry ©) : _into_node(copy._into_node), _from_node_path(copy._from_node_path), _into_node_path(copy._into_node_path), + _into_clip_planes(copy._into_clip_planes), _flags(copy._flags), _surface_point(copy._surface_point), _surface_normal(copy._surface_normal), @@ -53,6 +55,7 @@ operator = (const CollisionEntry ©) { _into_node = copy._into_node; _from_node_path = copy._from_node_path; _into_node_path = copy._into_node_path; + _into_clip_planes = copy._into_clip_planes; _flags = copy._flags; _surface_point = copy._surface_point; _surface_normal = copy._surface_normal; @@ -151,3 +154,19 @@ get_all(const NodePath &space, LPoint3f &surface_point, return true; } + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::check_clip_planes +// Access: Private +// Description: Checks whether the into_node_path has a +// ClipPlaneAttrib defined. +//////////////////////////////////////////////////////////////////// +void CollisionEntry:: +check_clip_planes() { + const RenderAttrib *cpa_attrib = + _into_node_path.get_net_state()->get_attrib(ClipPlaneAttrib::get_class_type()); + if (cpa_attrib != (const RenderAttrib *)NULL) { + _into_clip_planes = DCAST(ClipPlaneAttrib, cpa_attrib); + } + _flags |= F_checked_clip_planes; +} diff --git a/panda/src/collide/collisionEntry.h b/panda/src/collide/collisionEntry.h index b0acf92c5e..2e19f7ec6b 100644 --- a/panda/src/collide/collisionEntry.h +++ b/panda/src/collide/collisionEntry.h @@ -32,6 +32,7 @@ #include "pointerTo.h" #include "pandaNode.h" #include "nodePath.h" +#include "clipPlaneAttrib.h" /////////////////////////////////////////////////////////////////// // Class : CollisionEntry @@ -111,12 +112,12 @@ public: INLINE const LMatrix4f &get_inv_wrt_mat() const; INLINE const LMatrix4f &get_wrt_prev_mat() const; - + INLINE const ClipPlaneAttrib *get_into_clip_planes() const; private: INLINE void test_intersection(CollisionHandler *record, const CollisionTraverser *trav) const; - void compute_from_surface_normal(); + void check_clip_planes(); CPT(CollisionSolid) _from; CPT(CollisionSolid) _into; @@ -125,12 +126,14 @@ private: PT(PandaNode) _into_node; NodePath _from_node_path; NodePath _into_node_path; + CPT(ClipPlaneAttrib) _into_clip_planes; enum Flags { F_has_surface_point = 0x0001, F_has_surface_normal = 0x0002, F_has_interior_point = 0x0004, F_respect_prev_transform = 0x0008, + F_checked_clip_planes = 0x0010, }; int _flags; diff --git a/panda/src/collide/collisionNode.cxx b/panda/src/collide/collisionNode.cxx index 0fda1e0907..fbb256b55c 100644 --- a/panda/src/collide/collisionNode.cxx +++ b/panda/src/collide/collisionNode.cxx @@ -207,13 +207,15 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { Solids::iterator si; for (si = _solids.begin(); si != _solids.end(); ++si) { CollisionSolid *solid = (*si); - PandaNode *node = solid->get_viz(); - CullTraverserData next_data(data, node); + PT(PandaNode) node = solid->get_viz(data); + if (node != (PandaNode *)NULL) { + CullTraverserData next_data(data, node); - // We don't want to inherit the render state from above for these - // guys. - next_data._state = RenderState::make_empty(); - trav->traverse(next_data); + // We don't want to inherit the render state from above for these + // guys. + next_data._state = RenderState::make_empty(); + trav->traverse(next_data); + } } // Determine the previous frame's position, relative to the @@ -227,13 +229,15 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { for (si = _solids.begin(); si != _solids.end(); ++si) { CollisionSolid *solid = (*si); - PandaNode *node = solid->get_viz(); - CullTraverserData next_data(data, node); + PT(PandaNode) node = solid->get_viz(data); + if (node != (PandaNode *)NULL) { + CullTraverserData next_data(data, node); - next_data._render_transform = - next_data._render_transform->compose(transform); - next_data._state = get_last_pos_state(); - trav->traverse(next_data); + next_data._render_transform = + next_data._render_transform->compose(transform); + next_data._state = get_last_pos_state(); + trav->traverse(next_data); + } } } diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index 303d33c860..7241dcd8af 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -24,6 +24,7 @@ #include "collisionSegment.h" #include "config_collide.h" +#include "cullTraverserData.h" #include "boundingSphere.h" #include "pointerToArray.h" #include "geomNode.h" @@ -34,6 +35,7 @@ #include "bamWriter.h" #include "geomPolygon.h" #include "transformState.h" +#include "clipPlaneAttrib.h" #include @@ -186,6 +188,50 @@ get_collision_origin() const { return to_3d(_median); } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::get_viz +// Access: Public, Virtual +// Description: Returns a GeomNode that may be rendered to visualize +// the CollisionSolid. This is used during the cull +// traversal to render the CollisionNodes that have been +// made visible. +//////////////////////////////////////////////////////////////////// +PT(PandaNode) CollisionPolygon:: +get_viz(const CullTraverserData &data) const { + const RenderAttrib *cpa_attrib = + data._state->get_attrib(ClipPlaneAttrib::get_class_type()); + if (cpa_attrib == (const RenderAttrib *)NULL) { + // Fortunately, the polygon is not clipped. This is the normal, + // easy case. + return CollisionSolid::get_viz(data); + } + + // The polygon is clipped. We need to render it clipped. We could + // just turn on the ClipPlaneAttrib state and render the full + // polygon, letting the hardware do the clipping, but we get fancy + // and clip it by hand instead, just to prove that our clipping + // algorithm works properly. This does require some more dynamic + // work. + const ClipPlaneAttrib *cpa = DCAST(ClipPlaneAttrib, cpa_attrib); + Points new_points; + if (apply_clip_plane(new_points, cpa, data._net_transform)) { + // All points are behind the clip plane; just draw the original + // polygon. + return CollisionSolid::get_viz(data); + } + + if (new_points.empty()) { + // All points are in front of the clip plane; draw nothing. + return NULL; + } + + // Draw the clipped polygon. + PT(GeomNode) geom_node = new GeomNode("viz"); + draw_polygon(geom_node, new_points); + + return geom_node.p(); +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::output // Access: Public, Virtual @@ -343,56 +389,37 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { LPoint2f p = to_2d(plane_point); - // Now we have found a point on the polygon's plane that corresponds - // to the point tangent to our collision sphere where it first - // touches the plane. We want to decide whether the sphere itself - // will intersect the polygon. We can approximate this by testing - // whether a circle of the given radius centered around this tangent - // point, in the plane of the polygon, would intersect. + 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; just do the default + // test. + if (!circle_is_inside(p, from_radius, _points, _median)) { + return NULL; + } - // But even this approximate test is too difficult. To approximate - // the approximation, we'll test two points: (1) the center itself. - // If this is inside the polygon, then certainly the circle - // intersects the polygon, and the sphere collides. (2) a point on - // the outside of the circle, nearest to the center of the polygon. - // If _this_ point is inside the polygon, then again the circle, and - // hence the sphere, intersects. If neither point is inside the - // polygon, chances are reasonably good the sphere doesn't intersect - // the polygon after all. + } else if (new_points.empty()) { + // The polygon is completely clipped. + return NULL; - if (is_inside(p)) { - // The circle's center is inside the polygon; we have a collision! + } else { + // Test against the clipped polygon. + LPoint2f new_median = new_points[0]; + for (int n = 1; n < (int)new_points.size(); n++) { + new_median += new_points[n]; + } + new_median /= new_points.size(); + if (!circle_is_inside(p, from_radius, new_points, new_median)) { + return NULL; + } + } } else { - - if (from_radius > 0.0f) { - // Now find the point on the rim of the circle nearest the - // polygon's center. - - // First, get a vector from the center of the circle to the center - // of the polygon. - LVector2f rim = _median - p; - float rim_length = length(rim); - - if (rim_length <= from_radius) { - // Here's a surprise: the center of the polygon is within the - // circle! Since the center is guaranteed to be interior to the - // polygon (the polygon is convex), it follows that the circle - // intersects the polygon. - - } else { - // Now scale this vector to length radius, and get the new point. - rim = (rim * from_radius / rim_length) + p; - - // Is the new point within the polygon? - if (is_inside(rim)) { - // It sure is! The circle intersects! - - } else { - // No intersection. - return NULL; - } - } + // No clip plane is in effect. Do the default test. + if (!circle_is_inside(p, from_radius, _points, _median)) { + return NULL; } } @@ -451,9 +478,25 @@ test_intersection_from_ray(const CollisionEntry &entry) const { } LPoint3f plane_point = from_origin + t * from_direction; - if (!is_inside(to_2d(plane_point))) { - // Outside the polygon's perimeter. - return NULL; + LPoint2f p = to_2d(plane_point); + + const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); + if (cpa != (ClipPlaneAttrib *)NULL) { + // We have a clip plane; apply it. + Points new_points; + apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform()); + if (new_points.size() < 3) { + return NULL; + } + if (!point_is_inside(p, new_points)) { + return NULL; + } + + } else { + // No clip plane is in effect. Do the default test. + if (!point_is_inside(p, _points)) { + return NULL; + } } if (collide_cat.is_debug()) { @@ -504,9 +547,25 @@ test_intersection_from_segment(const CollisionEntry &entry) const { } LPoint3f plane_point = from_a + t * from_direction; - if (!is_inside(to_2d(plane_point))) { - // Outside the polygon's perimeter. - return NULL; + LPoint2f p = to_2d(plane_point); + + const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); + if (cpa != (ClipPlaneAttrib *)NULL) { + // We have a clip plane; apply it. + Points new_points; + apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform()); + if (new_points.size() < 3) { + return NULL; + } + if (!point_is_inside(p, new_points)) { + return NULL; + } + + } else { + // No clip plane is in effect. Do the default test. + if (!point_is_inside(p, _points)) { + return NULL; + } } if (collide_cat.is_debug()) { @@ -534,8 +593,19 @@ fill_viz_geom() { collide_cat.debug() << "Recomputing viz for " << *this << "\n"; } + draw_polygon(_viz_geom, _points); +} - if (_points.size() < 3) { +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::draw_polygon +// Access: Private +// Description: Fills up the indicated GeomNode with the Geoms to +// draw the polygon indicated with the given set of 2-d +// points. +//////////////////////////////////////////////////////////////////// +void CollisionPolygon:: +draw_polygon(GeomNode *geom_node, const CollisionPolygon::Points &points) const { + if (points.size() < 3) { if (collide_cat.is_debug()) { collide_cat.debug() << "(Degenerate poly, ignoring.)\n"; @@ -545,7 +615,7 @@ fill_viz_geom() { PTA_Vertexf verts; Points::const_iterator pi; - for (pi = _points.begin(); pi != _points.end(); ++pi) { + for (pi = points.begin(); pi != points.end(); ++pi) { verts.push_back(to_3d(*pi)); } if (_reversed) { @@ -553,42 +623,109 @@ fill_viz_geom() { } PTA_int lengths; - lengths.push_back(_points.size()); + lengths.push_back(points.size()); GeomPolygon *polygon = new GeomPolygon; polygon->set_coords(verts); polygon->set_num_prims(1); polygon->set_lengths(lengths); - _viz_geom->add_geom(polygon, get_solid_viz_state()); - _viz_geom->add_geom(polygon, get_wireframe_viz_state()); + geom_node->add_geom(polygon, ((CollisionPolygon *)this)->get_solid_viz_state()); + geom_node->add_geom(polygon, ((CollisionPolygon *)this)->get_wireframe_viz_state()); } + //////////////////////////////////////////////////////////////////// -// Function: CollisionPolygon::is_inside +// Function: CollisionPolygon::point_is_inside // Access: Private -// Description: +// Description: Returns true if the indicated point is within the +// polygon's 2-d space, false otherwise. //////////////////////////////////////////////////////////////////// bool CollisionPolygon:: -is_inside(const LPoint2f &p) const { +point_is_inside(const LPoint2f &p, const CollisionPolygon::Points &points) const { // We insist that the polygon be convex. This makes things a bit simpler. // In the case of a convex polygon, defined with points in counterclockwise // order, a point is interior to the polygon iff the point is not right of // each of the edges. - - for (int i = 0; i < (int)_points.size() - 1; i++) { - if (is_right(p - _points[i], _points[i+1] - _points[i])) { + for (int i = 0; i < (int)points.size() - 1; i++) { + if (is_right(p - points[i], points[i+1] - points[i])) { return false; } } - if (is_right(p - _points[_points.size() - 1], - _points[0] - _points[_points.size() - 1])) { + if (is_right(p - points[points.size() - 1], + points[0] - points[points.size() - 1])) { return false; } return true; +} +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::circle_is_inside +// Access: Private +// Description: Returns true if the circle with the indicated center +// and radius intersects the polygon in its 2-d space, +// false otherwise. +//////////////////////////////////////////////////////////////////// +bool CollisionPolygon:: +circle_is_inside(const LPoint2f ¢er, float radius, + const CollisionPolygon::Points &points, + const LPoint2f &median) const { + // Now we have found a point on the polygon's plane that corresponds + // to the point tangent to our collision sphere where it first + // touches the plane. We want to decide whether the sphere itself + // will intersect the polygon. We can approximate this by testing + // whether a circle of the given radius centered around this tangent + // point, in the plane of the polygon, would intersect. + + // But even this approximate test is too difficult. To approximate + // the approximation, we'll test two points: (1) the center itself. + // If this is inside the polygon, then certainly the circle + // intersects the polygon, and the sphere collides. (2) a point on + // the outside of the circle, nearest to the center of the polygon. + // If _this_ point is inside the polygon, then again the circle, and + // hence the sphere, intersects. If neither point is inside the + // polygon, chances are reasonably good the sphere doesn't intersect + // the polygon after all. + + if (point_is_inside(center, points)) { + // The circle's center is inside the polygon; we have a collision! + + } else { + + if (radius > 0.0f) { + // Now find the point on the rim of the circle nearest the + // polygon's center. + + // First, get a vector from the center of the circle to the center + // of the polygon. + LVector2f rim = median - center; + float rim_length = length(rim); + + if (rim_length <= radius) { + // Here's a surprise: the center of the polygon is within the + // circle! Since the center is guaranteed to be interior to the + // polygon (the polygon is convex), it follows that the circle + // intersects the polygon. + + } else { + // Now scale this vector to length radius, and get the new point. + rim = (rim * radius / rim_length) + center; + + // Is the new point within the polygon? + if (point_is_inside(rim, points)) { + // It sure is! The circle intersects! + + } else { + // No intersection. + return false; + } + } + } + } + + return true; } //////////////////////////////////////////////////////////////////// @@ -776,7 +913,7 @@ setup_points(const LPoint3f *begin, const LPoint3f *end) { // in the polygon's 2-d definition space. //////////////////////////////////////////////////////////////////// LPoint2f CollisionPolygon:: -to_2d(const LPoint3f &point3d) const { +to_2d(const LVecBase3f &point3d) const { nassertr(!point3d.is_nan(), LPoint2f(0.0f, 0.0f)); // Project the point of intersection with the plane onto the @@ -804,7 +941,7 @@ to_2d(const LPoint3f &point3d) const { // definition space back into 3-d coordinates. //////////////////////////////////////////////////////////////////// LPoint3f CollisionPolygon:: -to_3d(const LPoint2f &point2d) const { +to_3d(const LVecBase2f &point2d) const { nassertr(!point2d.is_nan(), LPoint3f(0.0f, 0.0f, 0.0f)); LVector3f normal = get_normal(); @@ -832,6 +969,140 @@ to_3d(const LPoint2f &point2d) const { return LPoint3f(0.0f, 0.0f, 0.0f); } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::clip_polygon +// Access: Private +// Description: Clips the source_points of the polygon by the +// indicated clipping plane, and modifies new_points to +// reflect the new set of clipped points. +// +// The return value is true if the set of points is +// unmodified (all points are behind the clip plane), or +// false otherwise. +//////////////////////////////////////////////////////////////////// +bool CollisionPolygon:: +clip_polygon(CollisionPolygon::Points &new_points, + const CollisionPolygon::Points &source_points, + const Planef &plane) const { + new_points.clear(); + if (source_points.empty()) { + return true; + } + + LPoint3f from3d; + LVector3f delta3d; + if (!plane.intersects_plane(from3d, delta3d, get_plane())) { + // The clipping plane is parallel to the polygon. The polygon is + // either all in or all out. + if (plane.dist_to_plane(get_plane().get_point()) < 0.0) { + // A point within the polygon is behind the clipping plane: the + // polygon is all in. + new_points = source_points; + return true; + } + return false; + } + + // Project the line of intersection into the 2-d plane. Now we have + // a 2-d clipping line. + LPoint2f from2d = to_2d(from3d); + LVector2f delta2d = to_2d(delta3d); + if (_reversed) { + delta2d = -delta2d; + } + + float a = -delta2d[1]; + float b = delta2d[0]; + float c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0]; + + // Now walk through the points. Any point on the left of our line + // gets removed, and the line segment clipped at the point of + // intersection. + + // We might increase the number of vertices by as many as 1, if the + // plane clips off exactly one corner. (We might also decrease the + // number of vertices, or keep them the same number.) + new_points.reserve(source_points.size() + 1); + + LPoint2f last_point = source_points.back(); + bool last_is_in = !is_right(last_point - from2d, delta2d); + bool all_in = last_is_in; + Points::const_iterator pi; + for (pi = source_points.begin(); pi != source_points.end(); ++pi) { + const LPoint2f &this_point = (*pi); + bool this_is_in = !is_right(this_point - from2d, delta2d); + + if (this_is_in != last_is_in) { + // We have just crossed over the clipping line. Find the point + // of intersection. + LVector2f d = this_point - last_point; + float t = -(a * last_point[0] + b * last_point[1] + c) / (a * d[0] + b * d[1]); + LPoint2f p = last_point + t * d; + + new_points.push_back(p); + last_is_in = this_is_in; + } + + if (this_is_in) { + // We are behind the clipping line. Keep the point. + new_points.push_back(this_point); + } else { + all_in = false; + } + + last_point = this_point; + } + + return all_in; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::apply_clip_plane +// Access: Private +// Description: Clips the polygon by all of the clip planes named in +// the clip plane attribute and fills new_points up with +// the resulting points. +// +// The return value is true if the set of points is +// unmodified (all points are behind all the clip +// planes), or false otherwise. +//////////////////////////////////////////////////////////////////// +bool CollisionPolygon:: +apply_clip_plane(CollisionPolygon::Points &new_points, + const ClipPlaneAttrib *cpa, + const TransformState *net_transform) const { + bool all_in = true; + + int num_planes = cpa->get_num_planes(); + if (num_planes > 0) { + PlaneNode *plane_node = cpa->get_plane(0); + NodePath plane_path(plane_node); + CPT(TransformState) new_transform = + net_transform->invert_compose(plane_path.get_net_transform()); + + Planef plane = plane_node->get_plane() * new_transform->get_mat(); + if (!clip_polygon(new_points, _points, plane)) { + all_in = false; + } + + for (int i = 1; i < num_planes; i++) { + PlaneNode *plane_node = cpa->get_plane(i); + NodePath plane_path(plane_node); + CPT(TransformState) new_transform = + net_transform->invert_compose(plane_path.get_net_transform()); + + Planef plane = plane_node->get_plane() * new_transform->get_mat(); + Points last_points; + last_points.swap(new_points); + if (!clip_polygon(new_points, last_points, plane)) { + all_in = false; + } + } + } + + return all_in; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::write_datagram // Access: Public diff --git a/panda/src/collide/collisionPolygon.h b/panda/src/collide/collisionPolygon.h index 160a82e4d0..71b9a9d127 100644 --- a/panda/src/collide/collisionPolygon.h +++ b/panda/src/collide/collisionPolygon.h @@ -22,9 +22,12 @@ #include "pandabase.h" #include "collisionPlane.h" +#include "clipPlaneAttrib.h" #include "vector_LPoint2f.h" +class GeomNode; + /////////////////////////////////////////////////////////////////// // Class : CollisionPolygon // Description : @@ -55,6 +58,8 @@ public: virtual void xform(const LMatrix4f &mat); virtual LPoint3f get_collision_origin() const; + virtual PT(PandaNode) get_viz(const CullTraverserData &data) const; + virtual void output(ostream &out) const; virtual void write(ostream &out, int indent_level = 0) const; @@ -71,15 +76,26 @@ protected: virtual void fill_viz_geom(); private: - bool is_inside(const LPoint2f &p) const; + typedef vector_LPoint2f Points; + + void draw_polygon(GeomNode *geom_node, const Points &points) const; + + bool point_is_inside(const LPoint2f &p, const Points &points) const; + bool circle_is_inside(const LPoint2f ¢er, float radius, + const CollisionPolygon::Points &points, + const LPoint2f &median) const; bool is_concave() const; void setup_points(const LPoint3f *begin, const LPoint3f *end); - LPoint2f to_2d(const LPoint3f &point3d) const; - LPoint3f to_3d(const LPoint2f &point2d) const; + LPoint2f to_2d(const LVecBase3f &point3d) const; + LPoint3f to_3d(const LVecBase2f &point2d) const; + + bool clip_polygon(Points &new_points, const Points &source_points, + const Planef &plane) const; + bool apply_clip_plane(Points &new_points, const ClipPlaneAttrib *cpa, + const TransformState *net_transform) const; private: - typedef vector_LPoint2f Points; Points _points; LPoint2f _median; diff --git a/panda/src/collide/collisionSolid.cxx b/panda/src/collide/collisionSolid.cxx index fceb59e7e0..643db99552 100644 --- a/panda/src/collide/collisionSolid.cxx +++ b/panda/src/collide/collisionSolid.cxx @@ -86,14 +86,14 @@ test_intersection(const CollisionEntry &) const { //////////////////////////////////////////////////////////////////// // Function: CollisionSolid::get_viz -// Access: Public +// Access: Public, Virtual // Description: Returns a GeomNode that may be rendered to visualize // the CollisionSolid. This is used during the cull // traversal to render the CollisionNodes that have been // made visible. //////////////////////////////////////////////////////////////////// -GeomNode *CollisionSolid:: -get_viz() const { +PT(PandaNode) CollisionSolid:: +get_viz(const CullTraverserData &) const { if (_viz_geom_stale) { if (_viz_geom == (GeomNode *)NULL) { ((CollisionSolid *)this)->_viz_geom = new GeomNode("viz"); @@ -103,7 +103,7 @@ get_viz() const { ((CollisionSolid *)this)->fill_viz_geom(); ((CollisionSolid *)this)->_viz_geom_stale = false; } - return _viz_geom; + return _viz_geom.p(); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index 99f6ccebe1..f5aaa6c830 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -33,6 +33,7 @@ class CollisionEntry; class CollisionSphere; class GeomNode; class CollisionNode; +class CullTraverserData; /////////////////////////////////////////////////////////////////// // Class : CollisionSolid @@ -67,7 +68,7 @@ public: virtual void xform(const LMatrix4f &mat)=0; - GeomNode *get_viz() const; + virtual PT(PandaNode) get_viz(const CullTraverserData &data) const; PUBLISHED: virtual void output(ostream &out) const; diff --git a/panda/src/collide/collisionVisualizer.cxx b/panda/src/collide/collisionVisualizer.cxx index a0f3cc5c50..1b2ebaedce 100644 --- a/panda/src/collide/collisionVisualizer.cxx +++ b/panda/src/collide/collisionVisualizer.cxx @@ -124,26 +124,31 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { const VizInfo &viz_info = (*di).second; CullTraverserData xform_data(data); - - // We don't want to inherit the transform state! We ignore + + // We don't want to inherit the transform from above! We ignore // whatever transforms were above the CollisionVisualizer node; it // always renders its objects according to their appropriate net // transform. xform_data._net_transform = TransformState::make_identity(); xform_data._render_transform = trav->get_render_transform(); - xform_data.apply_transform_and_state(trav, net_transform, + xform_data.apply_transform_and_state(trav, net_transform, RenderState::make_empty(), RenderEffects::make_empty()); // Draw all the collision solids. Solids::const_iterator si; for (si = viz_info._solids.begin(); si != viz_info._solids.end(); ++si) { + // Note that we don't preserve the clip plane attribute from the + // collision solid. We always draw the whole polygon (or + // whatever) in the CollisionVisualizer. This is a deliberate + // decision; clipping the polygons may obscure many collision + // tests that are being made. const CollisionSolid *solid = (*si).first; const SolidInfo &solid_info = (*si).second; - PandaNode *node = solid->get_viz(); - + PT(PandaNode) node = solid->get_viz(xform_data); + CullTraverserData next_data(xform_data, node); - + // We don't want to inherit the render state from above for // these guys. Instead, we choose the state according to // whether a collision was detected or not. @@ -152,22 +157,22 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { } else { next_data._state = get_tested_state(); } - + trav->traverse(next_data); } // Now draw all of the detected points. if (!viz_info._points.empty()) { CPT(RenderState) empty_state = RenderState::make_empty(); - + PTA_Colorf colors; colors.push_back(Colorf(1.0f, 0.0f, 0.0f, 1.0f)); colors.push_back(Colorf(1.0f, 1.0f, 1.0f, 1.0f)); - + Points::const_iterator pi; for (pi = viz_info._points.begin(); pi != viz_info._points.end(); ++pi) { const CollisionPoint &point = (*pi); - + // Draw a small red sphere at the surface point, and a smaller // white sphere at the interior point. { @@ -179,14 +184,14 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { sphere->set_coords(verts); sphere->set_colors(colors, G_PER_PRIM); sphere->set_num_prims(1); - + if (point._interior_point != point._surface_point) { verts.push_back(point._interior_point); verts.push_back(point._interior_point + LVector3f(0.05f * _viz_scale, 0.0f, 0.0f)); sphere->set_num_prims(2); } - + CullableObject *object = new CullableObject(sphere, empty_state, xform_data._render_transform); @@ -212,7 +217,6 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { } } } - } // Now carry on to render our child nodes. @@ -261,7 +265,10 @@ void CollisionVisualizer:: collision_tested(const CollisionEntry &entry, bool detected) { CollisionRecorder::collision_tested(entry, detected); - VizInfo &viz_info = _data[entry.get_into_node_path().get_net_transform()]; + NodePath node_path = entry.get_into_node_path(); + CPT(TransformState) net_transform = node_path.get_net_transform(); + + VizInfo &viz_info = _data[net_transform]; if (detected) { viz_info._solids[entry.get_into()]._detected_count++; diff --git a/panda/src/collide/collisionVisualizer.h b/panda/src/collide/collisionVisualizer.h index ef9e5bcc57..75ee51769b 100644 --- a/panda/src/collide/collisionVisualizer.h +++ b/panda/src/collide/collisionVisualizer.h @@ -23,6 +23,7 @@ #include "pandaNode.h" #include "collisionRecorder.h" #include "nodePath.h" +#include "pmap.h" #ifdef DO_COLLISION_RECORDING @@ -84,7 +85,7 @@ private: Points _points; }; - typedef map Data; + typedef pmap Data; Data _data; float _viz_scale; diff --git a/panda/src/mathutil/Sources.pp b/panda/src/mathutil/Sources.pp index 82fb5be87d..6c700ca4b3 100644 --- a/panda/src/mathutil/Sources.pp +++ b/panda/src/mathutil/Sources.pp @@ -51,6 +51,7 @@ #define TARGET test_mathutil #define LOCAL_LIBS \ mathutil + #define OTHER_LIBS $[OTHER_LIBS] pystub #define SOURCES \ test_mathutil.cxx diff --git a/panda/src/mathutil/plane.h b/panda/src/mathutil/plane.h index 87e1520bad..71a3465bea 100644 --- a/panda/src/mathutil/plane.h +++ b/panda/src/mathutil/plane.h @@ -19,12 +19,12 @@ #ifndef PLANE_H #define PLANE_H -#include +#include "pandabase.h" -#include -#include -#include -#include +#include "luse.h" +#include "indent.h" +#include "nearly_zero.h" +#include "cmath.h" class Datagram; class DatagramIterator; diff --git a/panda/src/mathutil/plane_src.I b/panda/src/mathutil/plane_src.I index e1ca6989ab..587b3183c6 100644 --- a/panda/src/mathutil/plane_src.I +++ b/panda/src/mathutil/plane_src.I @@ -18,7 +18,7 @@ //////////////////////////////////////////////////////////////////// // Function: Plane::Constructor -// Access: Public +// Access: Published // Description: Creates a default plane. This plane happens to // intersect the origin, perpendicular to the Z axis. // It's not clear how useful a default plane is. @@ -33,7 +33,7 @@ FLOATNAME(Plane)(void) { //////////////////////////////////////////////////////////////////// // Function: Plane::Copy Constructor -// Access: Public +// Access: Published // Description: //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL FLOATNAME(Plane):: @@ -47,7 +47,7 @@ FLOATNAME(Plane)(const FLOATNAME(Plane) ©) : //////////////////////////////////////////////////////////////////// // Function: Plane::Constructor -// Access: Public +// Access: Published // Description: Constructs a plane given three counter-clockwise // points, as seen from the front of the plane (that is, // viewed from the end of the normal vector, looking @@ -68,7 +68,7 @@ FLOATNAME(Plane)(const FLOATNAME(LPoint3) &a, const FLOATNAME(LPoint3) &b, //////////////////////////////////////////////////////////////////// // Function: Plane::Constructor -// Access: Public +// Access: Published // Description: Constructs a plane given a surface normal vector and // a point within the plane. //////////////////////////////////////////////////////////////////// @@ -85,7 +85,7 @@ FLOATNAME(Plane)(const FLOATNAME(LVector3) &normal, //////////////////////////////////////////////////////////////////// // Function: Plane::Constructor -// Access: Public +// Access: Published // Description: Constructs a plane given the four terms of the plane // equation. //////////////////////////////////////////////////////////////////// @@ -100,7 +100,7 @@ FLOATNAME(Plane)(FLOATTYPE a, FLOATTYPE b, FLOATTYPE c, FLOATTYPE d) : //////////////////////////////////////////////////////////////////// // Function: Plane::Operator = -// Access: Public +// Access: Published // Description: //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL FLOATNAME(Plane)& FLOATNAME(Plane):: @@ -114,7 +114,7 @@ operator = (const FLOATNAME(Plane)& p) { //////////////////////////////////////////////////////////////////// // Function: Plane::Operator * LMatrix3 -// Access: Public +// Access: Published // Description: Transforms the plane by the indicated matrix. //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL FLOATNAME(Plane) FLOATNAME(Plane):: @@ -126,7 +126,7 @@ operator * (const FLOATNAME(LMatrix3) &mat) const { //////////////////////////////////////////////////////////////////// // Function: Plane::Operator * LMatrix4 -// Access: Public +// Access: Published // Description: Transforms the plane by the indicated matrix. //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL FLOATNAME(Plane) FLOATNAME(Plane):: @@ -138,7 +138,7 @@ operator * (const FLOATNAME(LMatrix4) &mat) const { //////////////////////////////////////////////////////////////////// // Function: Plane::get_normal -// Access: Public +// Access: Published // Description: Returns the surface normal of the plane. //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL FLOATNAME(LVector3) FLOATNAME(Plane):: @@ -148,7 +148,7 @@ get_normal() const { //////////////////////////////////////////////////////////////////// // Function: Plane::dist_to_plane -// Access: Public +// Access: Published // Description: Returns the straight-line shortest distance from the // point to the plane. The returned value is positive // if the point is in front of the plane (on the side @@ -163,7 +163,7 @@ dist_to_plane(const FLOATNAME(LPoint3) &point) const { //////////////////////////////////////////////////////////////////// // Function: Plane::intersects_line -// Access: Public +// Access: Published // Description: Returns true if the plane intersects the infinite // line passing through points p1 and p2, false if the // line is parallel. The points p1 and p2 are used only @@ -185,7 +185,7 @@ intersects_line(FLOATNAME(LPoint3) &intersection_point, //////////////////////////////////////////////////////////////////// // Function: Plane::intersects_line -// Access: Public +// Access: Published // Description: This flavor of intersects_line() returns a bit more // information about the nature of the intersecting // point. The line is defined via the parametric @@ -215,7 +215,7 @@ intersects_line(FLOATTYPE &t, //////////////////////////////////////////////////////////////////// // Function: Plane::get_data -// Access: Public +// Access: Published // Description: Returns the address of the first of the four data // elements in the plane equation. The remaining // elements occupy the next positions consecutively in @@ -228,7 +228,7 @@ get_data() const { //////////////////////////////////////////////////////////////////// // Function: Plane::get_num_components -// Access: Public +// Access: Published // Description: Returns the number of elements in the plane equation, // four. //////////////////////////////////////////////////////////////////// @@ -239,7 +239,7 @@ get_num_components() const { //////////////////////////////////////////////////////////////////// // Function: Plane::output -// Access: Public +// Access: Published // Description: //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL void FLOATNAME(Plane):: @@ -249,7 +249,7 @@ output(ostream &out) const { //////////////////////////////////////////////////////////////////// // Function: Plane::write -// Access: Public +// Access: Published // Description: //////////////////////////////////////////////////////////////////// INLINE_MATHUTIL void FLOATNAME(Plane):: diff --git a/panda/src/mathutil/plane_src.cxx b/panda/src/mathutil/plane_src.cxx index 64ec16efc1..8fe770e6e5 100644 --- a/panda/src/mathutil/plane_src.cxx +++ b/panda/src/mathutil/plane_src.cxx @@ -19,7 +19,7 @@ //////////////////////////////////////////////////////////////////// // Function: Plane::get_reflection_mat -// Access: Public +// Access: Published // Description: This computes a transform matrix that performs the // perspective transform defined by the frustum, // accordinate to the indicated coordinate system. @@ -39,7 +39,7 @@ get_reflection_mat(void) const { //////////////////////////////////////////////////////////////////// // Function: Plane::get_point -// Access: Public +// Access: Published // Description: Returns an arbitrary point in the plane. This can be // used along with the normal returned by get_normal() // to reconstruct the plane. @@ -58,3 +58,42 @@ get_point() const { return FLOATNAME(LPoint3)(0.0f, 0.0f, -_d / _c); } } + + +//////////////////////////////////////////////////////////////////// +// Function: Plane::intersects_plane +// Access: Published +// Description: Returns true if the two planes intersect, false if +// they do not. If they do intersect, then from and +// delta are filled in with the parametric +// representation of the line of intersection: that is, +// from is a point on that line, and delta is a vector +// showing the direction of the line. +//////////////////////////////////////////////////////////////////// +bool FLOATNAME(Plane):: +intersects_plane(FLOATNAME(LPoint3) &from, + FLOATNAME(LVector3) &delta, + const FLOATNAME(Plane) &other) const { + FLOATNAME(LVector3) n1 = get_normal(); + FLOATNAME(LVector3) n2 = other.get_normal(); + + // The delta will be the cross product of the planes' normals. + delta = cross(n1, n2); + + // If the delta came out to zero, the planes were parallel and do + // not intersect. + if (delta.almost_equal(FLOATNAME(LVector3)::zero())) { + return false; + } + + FLOATTYPE n1n1 = dot(n1, n1); + FLOATTYPE n2n2 = dot(n2, n2); + FLOATTYPE n1n2 = dot(n1, n2); + + FLOATTYPE determinant_inv = 1.0f / (n1n1 * n2n2 - n1n2 * n1n2); + FLOATTYPE c1 = (other._d * n1n2 - _d * n2n2) * determinant_inv; + FLOATTYPE c2 = (_d * n1n2 - other._d * n1n1) * determinant_inv; + from = n1 * c1 + n2 * c2; + + return true; +} diff --git a/panda/src/mathutil/plane_src.h b/panda/src/mathutil/plane_src.h index 4104ac8e15..fbacf3ccd5 100644 --- a/panda/src/mathutil/plane_src.h +++ b/panda/src/mathutil/plane_src.h @@ -18,7 +18,8 @@ //////////////////////////////////////////////////////////////////// // Class : Plane -// Description : +// Description : An abstract mathematical description of a plane. A +// plane is defined by the equation Ax + By + Cz + D = 0. //////////////////////////////////////////////////////////////////// class EXPCL_PANDA FLOATNAME(Plane) { PUBLISHED: @@ -50,6 +51,10 @@ PUBLISHED: const FLOATNAME(LPoint3) &from, const FLOATNAME(LVector3) &delta) const; + bool intersects_plane(FLOATNAME(LPoint3) &from, + FLOATNAME(LVector3) &delta, + const FLOATNAME(Plane) &other) const; + INLINE_MATHUTIL const FLOATTYPE *get_data() const; INLINE_MATHUTIL int get_num_components() const; diff --git a/panda/src/mathutil/test_mathutil.cxx b/panda/src/mathutil/test_mathutil.cxx index f4accfb8a5..70ab8ddcd8 100644 --- a/panda/src/mathutil/test_mathutil.cxx +++ b/panda/src/mathutil/test_mathutil.cxx @@ -16,13 +16,27 @@ // //////////////////////////////////////////////////////////////////// -#include +#include "luse.h" #include "rotate_to.h" #include "boundingLine.h" #include "boundingSphere.h" +#include "plane.h" + int main() { + Planef p1(LVector3f(1, 0, 0), LPoint3f(1, 0, 0)); + Planef p2(LVector3f(0, 0, 1), LPoint3f(1, 0, 0)); + + LPoint3f from; + LVector3f delta; + + if (p1.intersects_plane(from, delta, p2)) { + nout << "intersects, " << from << " to " << delta << "\n"; + } else { + nout << "no intersect\n"; + } + /* LVector3d a(1.0f, 0.0f, 0.0f); LVector3d b = normalize(LVector3d(0.5, 0.5, 0.0f)); @@ -43,6 +57,7 @@ main() { << " length " << length(a * invert(rot)) << "\n"; */ + /* BoundingLine line(LPoint3f(0, 0, 1), LPoint3f(0, 0, 0)); BoundingSphere s1(LPoint3f(0, 0, 10), 1); @@ -54,6 +69,7 @@ main() { line.contains(&s2); line.contains(&s3); line.contains(&s4); + */ /* s1.contains(LPoint3f(0, 0, 1), LPoint3f(0, 0, 0));