From 8278b963bd7720e7125baf11d7d8ddf6245d2be9 Mon Sep 17 00:00:00 2001 From: Mike Christel Date: Fri, 31 Jul 2009 18:06:36 +0000 Subject: [PATCH] Added CollisionBox courtesy CMU ETC student Amith Tudur --- panda/src/collide/collide_composite2.cxx | 1 + panda/src/collide/collisionBox.I | 362 ++++++++ panda/src/collide/collisionBox.cxx | 1044 ++++++++++++++++++++++ panda/src/collide/collisionBox.h | 175 ++++ panda/src/collide/collisionSolid.cxx | 16 + panda/src/collide/collisionSolid.h | 3 + panda/src/collide/collisionSphere.cxx | 126 +++ panda/src/collide/collisionSphere.h | 3 + panda/src/collide/collisionTraverser.cxx | 2 + panda/src/collide/config_collide.cxx | 3 + 10 files changed, 1735 insertions(+) create mode 100644 panda/src/collide/collisionBox.I create mode 100644 panda/src/collide/collisionBox.cxx create mode 100644 panda/src/collide/collisionBox.h diff --git a/panda/src/collide/collide_composite2.cxx b/panda/src/collide/collide_composite2.cxx index a688af72da..22364269d1 100644 --- a/panda/src/collide/collide_composite2.cxx +++ b/panda/src/collide/collide_composite2.cxx @@ -11,3 +11,4 @@ #include "collisionTraverser.cxx" #include "collisionTube.cxx" #include "collisionVisualizer.cxx" +#include "collisionBox.cxx" diff --git a/panda/src/collide/collisionBox.I b/panda/src/collide/collisionBox.I new file mode 100644 index 0000000000..709394ca1c --- /dev/null +++ b/panda/src/collide/collisionBox.I @@ -0,0 +1,362 @@ +// Filename: collisionBox.I +// Created by: amith tudur (31Jul09) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) Carnegie Mellon University. All rights reserved. +// +// All use of this software is subject to the terms of the revised BSD +// license. You should have received a copy of this license along +// with this source code in a file named "LICENSE." +// +//////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::Constructor +// Access: Public +// Description: Create the Box by giving a Center and distances of +// of each of the sides of box from the Center. +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox:: +CollisionBox(const LPoint3f ¢er, float x, float y, float z) : + _center(center), _x(x), _y(y), _z(z) +{ + _min = LPoint3f(_center.get_x() - _x, _center.get_y() - _y, _center.get_z() - _z); + _max = LPoint3f(_center.get_x() + _x, _center.get_y() + _y, _center.get_z() + _z); + _radius = sqrt(_x*_x + _y*_y + _z*_z); + for(int v = 0; v < 8; v++) + _vertex[v] = get_point_aabb(v); + for(int p = 0; p < 6; p++) + _planes[p] = set_plane(p); + setup_box(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::Constructor +// Access: Public +// Description: Create the Box by Specifying the Diagonal Points +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox:: +CollisionBox(const LPoint3f &min, const LPoint3f &max) : + _min(min), _max(max) +{ + _center = (_min + _max) / 2; + _x = _center.get_x() - _min.get_x(); + _y = _center.get_y() - _min.get_y(); + _z = _center.get_z() - _min.get_z(); + _radius = sqrt(_x*_x + _y*_y + _z*_z); + for(int v = 0; v < 8; v++) + _vertex[v] = get_point_aabb(v); + for(int p = 0; p < 6; p++) + _planes[p] = set_plane(p); + setup_box(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::Constructor +// Access: Public +// Description: Center as three separate co-ordinate points +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox:: +CollisionBox(float cx, float cy, float cz, float x, float y, float z) +{ + CollisionBox(LPoint3f(cx, cy, cz), x, y, z); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::Default constructor +// Access: Protected +// Description: Creates an invalid Box. Only used when reading +// from a bam file. +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox:: +CollisionBox() { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox:: +CollisionBox(const CollisionBox ©) : + CollisionSolid(copy), + _center(copy._center), + _x( copy._x ), + _y( copy._y ), + _z( copy._z ), + _min( copy._min), + _max( copy._max), + _radius( copy._radius ) +{ + for(int v = 0; v < 8; v++) + _vertex[v] = copy._vertex[v]; + for(int p = 0; p < 6; p++) + _planes[p] = copy._planes[p]; + setup_box(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::flush_level +// Access: Public, Static +// Description: Flushes the PStatCollectors used during traversal. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionBox:: +flush_level() { + _volume_pcollector.flush_level(); + _test_pcollector.flush_level(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::set_center +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionBox:: +set_center(const LPoint3f ¢er) { + _center = center; + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::set_center +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionBox:: +set_center(float x, float y, float z) { + set_center(LPoint3f(x, y, z)); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_center +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const LPoint3f &CollisionBox:: +get_center() const { + return _center; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_radius +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE float CollisionBox:: +get_radius() const { + return _radius; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_num_points +// Access: Published +// Description: Returns 8: the number of vertices of a rectangular solid. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL int CollisionBox:: +get_num_points() const { + return 8; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_point +// Access: Published +// Description: Returns the nth vertex of the OBB. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL LPoint3f CollisionBox:: +get_point(int n) const { + nassertr(n >= 0 && n < 8, LPoint3f::zero()); + return _vertex[n]; +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_point_aabb +// Access: Published +// Description: Returns the nth vertex of the Axis Aligned Bounding Box. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL LPoint3f CollisionBox:: +get_point_aabb(int n) const { + nassertr(n >= 0 && n < 8, LPoint3f::zero()); + + // We do some trickery assuming that _min and _max are consecutive + // in memory. + const LPoint3f *a = &_min; + return LPoint3f(a[(n>>2)&1][0], a[(n>>1)&1][1], a[(n)&1][2]); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_num_planes +// Access: Published +// Description: Returns 6: the number of faces of a rectangular solid. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL int CollisionBox:: +get_num_planes() const { + return 6; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_plane +// Access: Published +// Description: Returns the nth face of the rectangular solid. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL Planef CollisionBox:: +get_plane(int n) const { + nassertr(n >= 0 && n < 6, Planef()); + return _planes[n]; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::set_plane +// Access: Published +// Description: Creates the nth face of the rectangular solid. +//////////////////////////////////////////////////////////////////// +INLINE_MATHUTIL Planef CollisionBox:: +set_plane(int n) const { + nassertr(n >= 0 && n < 6, Planef()); + return Planef(get_point(plane_def[n][0]), + get_point(plane_def[n][1]), + get_point(plane_def[n][2])); +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::is_right +// Access: Private, Static +// Description: Returns true if the 2-d v1 is to the right of v2. +//////////////////////////////////////////////////////////////////// +INLINE bool CollisionBox:: +is_right(const LVector2f &v1, const LVector2f &v2) { + return (v1[0] * v2[1] - v1[1] * v2[0]) > 1.0e-6f; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::dist_to_line +// Access: Private, Static +// Description: Returns the linear distance of p to the line defined +// by f and f+v, where v is a normalized vector. The +// result is negative if p is left of the line, positive +// if it is right of the line. +//////////////////////////////////////////////////////////////////// +INLINE float CollisionBox:: +dist_to_line(const LPoint2f &p, + const LPoint2f &f, const LVector2f &v) { + LVector2f v1 = (p - f); + return (v1[0] * v[1] - v1[1] * v[0]); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::to_2d +// Access: Private +// Description: Assuming the indicated point in 3-d space lies within +// the polygon's plane, returns the corresponding point +// in the polygon's 2-d definition space. +//////////////////////////////////////////////////////////////////// +INLINE LPoint2f CollisionBox:: +to_2d(const LVecBase3f &point3d, int plane) const { + LPoint3f point = LPoint3f(point3d) * _to_2d_mat[plane]; + return LPoint2f(point[0], point[2]); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::calc_to_3d_mat +// Access: Private +// Description: Fills the indicated matrix with the appropriate +// rotation transform to move points from the 2-d plane +// into the 3-d (X, 0, Z) plane. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionBox:: +calc_to_3d_mat(LMatrix4f &to_3d_mat,int plane) const { + // We have to be explicit about the coordinate system--we + // specifically mean CS_zup_right, because that points the forward + // vector down the Y axis and moves the coords in (X, 0, Z). We + // want this effect regardless of the user's coordinate system of + // choice. + + // The up vector, on the other hand, is completely arbitrary. + + look_at(to_3d_mat, -get_plane(plane).get_normal(), + LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right); + to_3d_mat.set_row(3, get_plane(plane).get_point()); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::rederive_to_3d_mat +// Access: Private +// Description: Fills the indicated matrix with the appropriate +// rotation transform to move points from the 2-d plane +// into the 3-d (X, 0, Z) plane. +// +// This is essentially similar to calc_to_3d_mat, except +// that the matrix is rederived from whatever is stored +// in _to_2d_mat, guaranteeing that it will match +// whatever algorithm produced that one, even if it was +// produced on a different machine with different +// numerical precision. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionBox:: +rederive_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const { + to_3d_mat.invert_from(_to_2d_mat[plane]); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::to_3d +// Access: Private, Static +// Description: Extrude the indicated point in the polygon's 2-d +// definition space back into 3-d coordinates. +//////////////////////////////////////////////////////////////////// +INLINE LPoint3f CollisionBox:: +to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat) { + return LPoint3f(point2d[0], 0.0f, point2d[1]) * to_3d_mat; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::PointDef::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox::PointDef:: +PointDef(const LPoint2f &p, const LVector2f &v) : _p(p), _v(v) { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::PointDef::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox::PointDef:: +PointDef(float x, float y) : _p(x, y), _v(0.0f, 0.0f) { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::PointDef::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionBox::PointDef:: +PointDef(const CollisionBox::PointDef ©) : _p(copy._p), _v(copy._v) { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::PointDef::Copy Assignment Operator +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionBox::PointDef:: +operator = (const CollisionBox::PointDef ©) { + _p = copy._p; + _v = copy._v; +} +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_plane_points +// Access: Public +// Description: returns the points that form the nth plane +//////////////////////////////////////////////////////////////////// + +INLINE CollisionBox::Points CollisionBox:: +get_plane_points( int n ){ + return _points[n]; +} diff --git a/panda/src/collide/collisionBox.cxx b/panda/src/collide/collisionBox.cxx new file mode 100644 index 0000000000..cc0e59af20 --- /dev/null +++ b/panda/src/collide/collisionBox.cxx @@ -0,0 +1,1044 @@ +// Filename: collisionBox.cxx +// Created by: amith tudur (31Jul09) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) Carnegie Mellon University. All rights reserved. +// +// All use of this software is subject to the terms of the revised BSD +// license. You should have received a copy of this license along +// with this source code in a file named "LICENSE." +// +//////////////////////////////////////////////////////////////////// + + +#include "collisionDSSolid.h" +#include "collisionBox.h" +#include "collisionLine.h" +#include "collisionRay.h" +#include "collisionSegment.h" +#include "collisionHandler.h" +#include "collisionEntry.h" +#include "config_collide.h" +#include "boundingBox.h" +#include "datagram.h" +#include "datagramIterator.h" +#include "bamReader.h" +#include "bamWriter.h" +#include "nearly_zero.h" +#include "cmath.h" +#include "mathNumbers.h" +#include "geom.h" +#include "geomTristrips.h" +#include "geomVertexWriter.h" +#include "config_mathutil.h" +#include "dcast.h" + +#include + +PStatCollector CollisionBox::_volume_pcollector( + "Collision Volumes:CollisionBox"); +PStatCollector CollisionBox::_test_pcollector( + "Collision Tests:CollisionBox"); +TypeHandle CollisionBox::_type_handle; + +const int CollisionBox::plane_def[6][4] = { + {0, 4, 5, 1}, + {4, 6, 7, 5}, + {6, 2, 3, 7}, + {2, 0, 1, 3}, + {1, 5, 7, 3}, + {2, 6, 4, 0}, +}; + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::make_copy +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +CollisionSolid *CollisionBox:: +make_copy() { + return new CollisionBox(*this); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::setup_box +// Access: Public, Virtual +// Description: Compute parameters for each of the box's sides +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +setup_box(){ + for(int plane = 0; plane < 6; plane++) { + LPoint3f array[4]; + array[0] = get_point(plane_def[plane][0]); + array[1] = get_point(plane_def[plane][1]); + array[2] = get_point(plane_def[plane][2]); + array[3] = get_point(plane_def[plane][3]); + setup_points(array, array+4, plane); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::setup_points +// Access: Private +// Description: Computes the plane and 2d projection of points that +// make up this side +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +setup_points(const LPoint3f *begin, const LPoint3f *end, int plane) { + int num_points = end - begin; + nassertv(num_points >= 3); + + _points[plane].clear(); + + // Construct a matrix that rotates the points from the (X,0,Z) plane + // into the 3-d plane. + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat, plane); + + // And the inverse matrix rotates points from 3-d space into the 2-d + // plane. + _to_2d_mat[plane].invert_from(to_3d_mat); + + // Now project all of the points onto the 2-d plane. + + const LPoint3f *pi; + for (pi = begin; pi != end; ++pi) { + LPoint3f point = (*pi) * _to_2d_mat[plane]; + _points[plane].push_back(PointDef(point[0], point[2])); + } + + nassertv(_points[plane].size() >= 3); + +#ifndef NDEBUG + /* + // Now make sure the points define a convex polygon. + if (is_concave()) { + collide_cat.error() << "Invalid concave CollisionPolygon defined:\n"; + const LPoint3f *pi; + for (pi = begin; pi != end; ++pi) { + collide_cat.error(false) << " " << (*pi) << "\n"; + } + collide_cat.error(false) + << " normal " << normal << " with length " << normal.length() << "\n"; + _points.clear(); + } + */ +#endif + + compute_vectors(_points[plane]); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::test_intersection +// Access: Public, Virtual +// Description: First Dispatch point for box as a FROM object +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionBox:: +test_intersection(const CollisionEntry &entry) const { + return entry.get_into()->test_intersection_from_box(entry); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::xform +// Access: Public, Virtual +// Description: Transforms the solid by the indicated matrix. +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +xform(const LMatrix4f &mat) { + _center = _center * mat; + for(int v = 0; v < 8; v++) + _vertex[v] = _vertex[v] * mat; + for(int p = 0; p < 6 ; p++) + _planes[p] = set_plane(p); + _x = _vertex[0].get_x()-_center.get_x(); + _y = _vertex[0].get_y()-_center.get_y(); + _z = _vertex[0].get_z()-_center.get_z(); + _radius = sqrt( _x*_x + _y*_y + _z*_z ); + setup_box(); + mark_viz_stale(); + mark_internal_bounds_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::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 CollisionBox:: +get_collision_origin() const { + return _center; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_min +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +LPoint3f CollisionBox:: +get_min() const { + return _min; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_max +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +LPoint3f CollisionBox:: +get_max() const { + return _max; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::get_approx_center +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +LPoint3f CollisionBox:: +get_approx_center() const { + return (_min + _max) * 0.5f; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::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 &CollisionBox:: +get_volume_pcollector() { + return _volume_pcollector; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::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 &CollisionBox:: +get_test_pcollector() { + return _test_pcollector; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::output +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +output(ostream &out) const { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::compute_internal_bounds +// Access: Protected, Virtual +// Description: Sphere is chosen as the Bounding Volume type for +// speed and efficiency +//////////////////////////////////////////////////////////////////// +PT(BoundingVolume) CollisionBox:: +compute_internal_bounds() const { + return new BoundingSphere(_center, _radius); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::test_intersection_from_sphere +// Access: Public, Virtual +// Description: Double dispatch point for sphere as FROM object +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionBox:: +test_intersection_from_sphere(const CollisionEntry &entry) const { + + const CollisionSphere *sphere; + DCAST_INTO_R(sphere, entry.get_from(), 0); + + CPT(TransformState) wrt_space = entry.get_wrt_space(); + CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + LPoint3f orig_center = sphere->get_center() * wrt_mat; + LPoint3f from_center = orig_center; + bool moved_from_center = false; + float t = 1.0f; + LPoint3f contact_point(from_center); + float actual_t = 1.0f; + + LVector3f from_radius_v = + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; + float from_radius_2 = from_radius_v.length_squared(); + float from_radius = csqrt(from_radius_2); + + int ip; + float max_dist,dist; + bool intersect; + Planef plane; + LVector3f normal; + + for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) { + plane = get_plane( ip ); + if (_points[ip].size() < 3) { + continue; + } + if (wrt_prev_space != wrt_space) { + // If we have a delta between the previous position and the + // current position, we use that to determine some more properties + // of the collision. + LPoint3f b = from_center; + LPoint3f a = sphere->get_center() * wrt_prev_space->get_mat(); + LVector3f delta = b - a; + + // First, there is no collision if the "from" object is definitely + // moving in the same direction as the plane's normal. + float dot = delta.dot(plane.get_normal()); + if (dot > 0.1f) { + continue; // no intersection + } + + if (IS_NEARLY_ZERO(dot)) { + // If we're moving parallel to the plane, the sphere is tested + // at its final point. Leave it as it is. + + } else { + // Otherwise, we're moving into the plane; the sphere is tested + // at the point along its path that is closest to intersecting + // the plane. This may be the actual intersection point, or it + // may be the starting point or the final point. + // dot is equal to the (negative) magnitude of 'delta' along the + // direction of the plane normal + // t = ratio of (distance from start pos to plane) to (distance + // from start pos to end pos), along axis of plane normal + float dist_to_p = plane.dist_to_plane(a); + t = (dist_to_p / -dot); + + // also compute the actual contact point and time of contact + // for handlers that need it + actual_t = ((dist_to_p - from_radius) / -dot); + actual_t = min(1.0f, max(0.0f, actual_t)); + contact_point = a + (actual_t * delta); + + if (t >= 1.0f) { + // Leave it where it is. + + } else if (t < 0.0f) { + from_center = a; + moved_from_center = true; + } else { + from_center = a + t * delta; + moved_from_center = true; + } + } + } + + normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal(); + + #ifndef NDEBUG + /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) { + std::cout + << "polygon within " << entry.get_into_node_path() + << " has normal " << normal << " of length " << normal.length() + << "\n"; + normal.normalize(); + }*/ + #endif + + // The nearest point within the plane to our center is the + // intersection of the line (center, center - normal) with the plane. + + if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) { + // No intersection with plane? This means the plane's effective + // normal was within the plane itself. A useless polygon. + continue; + } + + if (dist > from_radius || dist < -from_radius) { + // No intersection with the plane. + continue; + } + + LPoint2f p = to_2d(from_center - dist * plane.get_normal(), ip); + float edge_dist = 0.0f; + + 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(),ip)) { + // All points are behind the clip plane; just do the default + // test. + edge_dist = dist_to_polygon(p, _points[ip]); + } else if (new_points.empty()) { + // The polygon is completely clipped. + continue; + } else { + // Test against the clipped polygon. + edge_dist = dist_to_polygon(p, new_points); + } + } else { + // No clip plane is in effect. Do the default test. + edge_dist = dist_to_polygon(p, _points[ip]); + } + + // Now we have edge_dist, which is the distance from the sphere + // center to the nearest edge of the polygon, within the polygon's + // plane. edge_dist<0 means the point is within the polygon. + if(edge_dist < 0) { + intersect = true; + continue; + } + + if((edge_dist > 0) && + ((edge_dist * edge_dist + dist * dist) > from_radius_2)) { + // No intersection; the circle is outside the polygon. + continue; + } + + // The sphere appears to intersect the polygon. If the edge is less + // than from_radius away, the sphere may be resting on an edge of + // the polygon. Determine how far the center of the sphere must + // remain from the plane, based on its distance from the nearest + // edge. + + max_dist = from_radius; + if (edge_dist >= 0.0f) { + float max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, 0.0f); + max_dist = csqrt(max_dist_2); + } + + if (dist > max_dist) { + // There's no intersection: the sphere is hanging off the edge. + continue; + } + intersect = true; + } + if( !intersect ) + 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); + + float into_depth = max_dist - dist; + if (moved_from_center) { + // We have to base the depth of intersection on the sphere's final + // resting point, not the point from which we tested the + // intersection. + float orig_dist; + plane.intersects_line(orig_dist, orig_center, -normal); + into_depth = max_dist - orig_dist; + } + + new_entry->set_surface_normal(normal); + new_entry->set_surface_point(from_center - normal * dist); + new_entry->set_interior_point(from_center - normal * (dist + into_depth)); + new_entry->set_contact_pos(contact_point); + new_entry->set_contact_normal(plane.get_normal()); + new_entry->set_t(actual_t); + + return new_entry; +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::test_intersection_from_ray +// Access: Public, Virtual +// Description: Double dispatch point for ray as a FROM object +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionBox:: +test_intersection_from_ray(const CollisionEntry &entry) const { + const CollisionRay *ray; + DCAST_INTO_R(ray, entry.get_from(), 0); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = ray->get_origin() * wrt_mat; + LVector3f from_direction = ray->get_direction() * wrt_mat; + + int i, j; + float t, near_t; + bool intersect; + Planef plane; + Planef near_plane; + + //Returns the details about the first plane of the box that the ray + //intersects. + for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) { + plane = get_plane(i); + + if (!plane.intersects_line(t, from_origin, from_direction)) { + // No intersection. The ray is parallel to the plane. + continue; + } + + 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. + continue; + } + LPoint3f plane_point = from_origin + t * from_direction; + LPoint2f p = to_2d(plane_point, i); + + if (!point_is_inside(p, _points[i])){ + continue; + } + intersect = true; + if(j) { + if(t < near_t) { + near_plane = plane; + near_t = t; + } + } + else { + near_plane = plane; + near_t = t; + } + ++j; + } + + + if(!intersect) { + //No intersection with ANY of the box's planes has been detected + return NULL; + } + + if (collide_cat.is_debug()) { + collide_cat.debug() + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; + } + + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + + LPoint3f into_intersection_point = from_origin + near_t * from_direction; + + LVector3f normal = + (has_effective_normal() && ray->get_respect_effective_normal()) + ? get_effective_normal() : near_plane.get_normal(); + + new_entry->set_surface_normal(normal); + new_entry->set_surface_point(into_intersection_point); + + return new_entry; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::test_intersection_from_box +// Access: Public, Virtual +// Description: Double dispatch point for box as a FROM object +// NOT Implemented. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionBox:: +test_intersection_from_box(const CollisionEntry &entry) const { + return NULL; +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::fill_viz_geom +// Access: Protected, Virtual +// Description: Fills the _viz_geom GeomNode up with Geoms suitable +// for rendering this solid. +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +fill_viz_geom() { + if (collide_cat.is_debug()) { + collide_cat.debug() + << "Recomputing viz for " << *this << "\n"; + } + + PT(GeomVertexData) vdata = new GeomVertexData + ("collision", GeomVertexFormat::get_v3(), + Geom::UH_static); + GeomVertexWriter vertex(vdata, InternalName::get_vertex()); + + for(int i = 0; i < 6; i++) { + for(int j = 0; j < 4; j++) + vertex.add_data3f(get_point(plane_def[i][j])); + + PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static); + body->add_consecutive_vertices(i*4, 4); + body->close_primitive(); + + PT(Geom) geom = new Geom(vdata); + geom->add_primitive(body); + + _viz_geom->add_geom(geom, get_solid_viz_state()); + _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state()); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::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 CollisionBox:: +apply_clip_plane(CollisionBox::Points &new_points, + const ClipPlaneAttrib *cpa, + const TransformState *net_transform, int plane_no) const { + bool all_in = true; + + int num_planes = cpa->get_num_on_planes(); + bool first_plane = true; + + for (int i = 0; i < num_planes; i++) { + NodePath plane_path = cpa->get_on_plane(0); + PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node()); + if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) { + CPT(TransformState) new_transform = + net_transform->invert_compose(plane_path.get_net_transform()); + + Planef plane = plane_node->get_plane() * new_transform->get_mat(); + if (first_plane) { + first_plane = false; + if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) { + all_in = false; + } + } else { + Points last_points; + last_points.swap(new_points); + if (!clip_polygon(new_points, last_points, plane, plane_no)) { + all_in = false; + } + } + } + } + + if (!all_in) { + compute_vectors(new_points); + } + + return all_in; +} +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::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 (but does not +// compute the vectors in new_points). +// +// The return value is true if the set of points is +// unmodified (all points are behind the clip plane), or +// false otherwise. +//////////////////////////////////////////////////////////////////// +bool CollisionBox:: +clip_polygon(CollisionBox::Points &new_points, + const CollisionBox::Points &source_points, + const Planef &plane, int plane_no) const { + new_points.clear(); + if (source_points.empty()) { + return true; + } + + LPoint3f from3d; + LVector3f delta3d; + if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) { + // The clipping plane is parallel to the polygon. The polygon is + // either all in or all out. + if (plane.dist_to_plane(get_plane(plane_no).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,plane_no); + LVector2f delta2d = to_2d(delta3d,plane_no); + + 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()._p; + 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)._p; + bool this_is_in = !is_right(this_point - from2d, delta2d); + + // There appears to be a compiler bug in gcc 4.0: we need to + // extract this comparison outside of the if statement. + bool crossed_over = (this_is_in != last_is_in); + if (crossed_over) { + // We have just crossed over the clipping line. Find the point + // of intersection. + LVector2f d = this_point - last_point; + float denom = (a * d[0] + b * d[1]); + if (denom != 0.0) { + float t = -(a * last_point[0] + b * last_point[1] + c) / denom; + LPoint2f p = last_point + t * d; + + new_points.push_back(PointDef(p[0], p[1])); + last_is_in = this_is_in; + } + } + + if (this_is_in) { + // We are behind the clipping line. Keep the point. + new_points.push_back(PointDef(this_point[0], this_point[1])); + } else { + all_in = false; + } + + last_point = this_point; + } + + return all_in; +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox:: +// Access: Private +// Description: Returns the linear distance from the 2-d point to the +// nearest part of the polygon defined by the points +// vector. The result is negative if the point is +// within the polygon. +//////////////////////////////////////////////////////////////////// +float CollisionBox:: +dist_to_polygon(const LPoint2f &p, const CollisionBox::Points &points) const { + + // We know that that the polygon is convex and is defined with the + // points in counterclockwise order. Therefore, we simply compare + // the signed distance to each line segment; we ignore any negative + // values, and take the minimum of all the positive values. + + // If all values are negative, the point is within the polygon; we + // therefore return an arbitrary negative result. + + bool got_dist = false; + float best_dist = -1.0f; + + size_t num_points = points.size(); + for (size_t i = 0; i < num_points - 1; ++i) { + float d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p, + points[i]._v); + if (d >= 0.0f) { + if (!got_dist || d < best_dist) { + best_dist = d; + got_dist = true; + } + } + } + + float d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p, + points[num_points - 1]._v); + if (d >= 0.0f) { + if (!got_dist || d < best_dist) { + best_dist = d; + got_dist = true; + } + } + + return best_dist; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::dist_to_line_segment +// Access: Private, Static +// Description: Returns the linear distance of p to the line segment +// defined by f and t, where v = (t - f).normalize(). +// The result is negative if p is left of the line, +// positive if it is right of the line. If the result +// is positive, it is constrained by endpoints of the +// line segment (i.e. the result might be larger than it +// would be for a straight distance-to-line test). If +// the result is negative, we don't bother. +//////////////////////////////////////////////////////////////////// +float CollisionBox:: +dist_to_line_segment(const LPoint2f &p, + const LPoint2f &f, const LPoint2f &t, + const LVector2f &v) { + LVector2f v1 = (p - f); + float d = (v1[0] * v[1] - v1[1] * v[0]); + if (d < 0.0f) { + return d; + } + + // Compute the nearest point on the line. + LPoint2f q = p + LVector2f(-v[1], v[0]) * d; + + // Now constrain that point to the line segment. + if (v[0] > 0.0f) { + // X+ + if (v[1] > 0.0f) { + // Y+ + if (v[0] > v[1]) { + // X-dominant. + if (q[0] < f[0]) { + return (p - f).length(); + } if (q[0] > t[0]) { + return (p - t).length(); + } else { + return d; + } + } else { + // Y-dominant. + if (q[1] < f[1]) { + return (p - f).length(); + } if (q[1] > t[1]) { + return (p - t).length(); + } else { + return d; + } + } + } else { + // Y- + if (v[0] > -v[1]) { + // X-dominant. + if (q[0] < f[0]) { + return (p - f).length(); + } if (q[0] > t[0]) { + return (p - t).length(); + } else { + return d; + } + } else { + // Y-dominant. + if (q[1] > f[1]) { + return (p - f).length(); + } if (q[1] < t[1]) { + return (p - t).length(); + } else { + return d; + } + } + } + } else { + // X- + if (v[1] > 0.0f) { + // Y+ + if (-v[0] > v[1]) { + // X-dominant. + if (q[0] > f[0]) { + return (p - f).length(); + } if (q[0] < t[0]) { + return (p - t).length(); + } else { + return d; + } + } else { + // Y-dominant. + if (q[1] < f[1]) { + return (p - f).length(); + } if (q[1] > t[1]) { + return (p - t).length(); + } else { + return d; + } + } + } else { + // Y- + if (-v[0] > -v[1]) { + // X-dominant. + if (q[0] > f[0]) { + return (p - f).length(); + } if (q[0] < t[0]) { + return (p - t).length(); + } else { + return d; + } + } else { + // Y-dominant. + if (q[1] > f[1]) { + return (p - f).length(); + } if (q[1] < t[1]) { + return (p - t).length(); + } else { + return d; + } + } + } + } +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::point_is_inside +// Access: Private +// Description: Returns true if the indicated point is within the +// polygon's 2-d space, false otherwise. +//////////////////////////////////////////////////////////////////// +bool CollisionBox:: +point_is_inside(const LPoint2f &p, const CollisionBox::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]._p, points[i+1]._p - points[i]._p)) { + return false; + } + } + if (is_right(p - points[points.size() - 1]._p, + points[0]._p - points[points.size() - 1]._p)) { + return false; + } + + return true; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::compute_point +// Access: Protected +// Description: Returns a point on the surface of the sphere. +// latitude and longitude range from 0.0 to 1.0. This +// is used by fill_viz_geom() to create a visible +// representation of the sphere. +//////////////////////////////////////////////////////////////////// +Vertexf CollisionBox:: +compute_point(float latitude, float longitude) const { + float s1, c1; + csincos(latitude * MathNumbers::pi_f, &s1, &c1); + + float s2, c2; + csincos(longitude * 2.0f * MathNumbers::pi_f, &s2, &c2); + + Vertexf p(s1 * c2, s1 * s2, c1); + return p * get_radius() + get_center(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::compute_vectors +// Access: Private, Static +// Description: Now that the _p members of the given points array +// have been computed, go back and compute all of the _v +// members. +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +compute_vectors(Points &points) { + size_t num_points = points.size(); + for (size_t i = 0; i < num_points; i++) { + points[i]._v = points[(i + 1) % num_points]._p - points[i]._p; + points[i]._v.normalize(); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::register_with_read_factory +// Access: Public, Static +// Description: Factory method to generate a CollisionBox object +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +register_with_read_factory() { + BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::write_datagram +// Access: Public +// Description: Function to write the important information in +// the particular object to a Datagram +//////////////////////////////////////////////////////////////////// +void CollisionBox:: +write_datagram(BamWriter *manager, Datagram &me) { + CollisionSolid::write_datagram(manager, me); + _center.write_datagram(me); + _min.write_datagram(me); + _max.write_datagram(me); + for(int i=0; i < 8; i++) { + _vertex[i].write_datagram(me); + } + me.add_float32(_radius); + me.add_float32(_x); + me.add_float32(_y); + me.add_float32(_z); + for(int i=0; i < 6; i++) { + _planes[i].write_datagram(me); + } + for(int i=0; i < 6; i++) { + _to_2d_mat[i].write_datagram(me); + } + for(int i=0; i < 6; i++) { + me.add_uint16(_points[i].size()); + for (size_t j = 0; j < _points[i].size(); j++) { + _points[i][j]._p.write_datagram(me); + _points[i][j]._v.write_datagram(me); + } + } +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::make_CollisionBox +// Access: Protected +// Description: Factory method to generate a CollisionBox object +//////////////////////////////////////////////////////////////////// +TypedWritable *CollisionBox:: +make_CollisionBox(const FactoryParams ¶ms) { + CollisionBox *me = new CollisionBox; + DatagramIterator scan; + BamReader *manager; + + parse_params(params, scan, manager); + me->fillin(scan, manager); + return me; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionBox::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 CollisionBox:: +fillin(DatagramIterator& scan, BamReader* manager) { + CollisionSolid::fillin(scan, manager); + _center.read_datagram(scan); + _min.read_datagram(scan); + _max.read_datagram(scan); + for(int i=0; i < 8; i++) { + _vertex[i].read_datagram(scan); + } + _radius = scan.get_float32(); + _x = scan.get_float32(); + _y = scan.get_float32(); + _z = scan.get_float32(); + for(int i=0; i < 6; i++) { + _planes[i].read_datagram(scan); + } + for(int i=0; i < 6; i++) { + _to_2d_mat[i].read_datagram(scan); + } + for(int i=0; i < 6; i++) { + size_t size = scan.get_uint16(); + for (size_t j = 0; j < size; j++) { + LPoint2f p; + LVector2f v; + p.read_datagram(scan); + v.read_datagram(scan); + _points[i].push_back(PointDef(p, v)); + } + } +} diff --git a/panda/src/collide/collisionBox.h b/panda/src/collide/collisionBox.h new file mode 100644 index 0000000000..328c5ec712 --- /dev/null +++ b/panda/src/collide/collisionBox.h @@ -0,0 +1,175 @@ +// Filename: collisionBox.h +// Created by: amith tudur( 31Jul09 ) +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) Carnegie Mellon University. All rights reserved. +// +// All use of this software is subject to the terms of the revised BSD +// license. You should have received a copy of this license along +// with this source code in a file named "LICENSE." +// +//////////////////////////////////////////////////////////////////// + +#ifndef COLLISIONBOX_H +#define COLLISIONBOX_H + +#include "pandabase.h" +#include "collisionSolid.h" +#include "parabola.h" +#include "plane.h" +#include "look_at.h" +#include "clipPlaneAttrib.h" + +//////////////////////////////////////////////////////////////////// +// Class : CollisionBox +// Description : A cuboid collision volume or object. +//////////////////////////////////////////////////////////////////// +class EXPCL_PANDA_COLLIDE CollisionBox : public CollisionSolid { +PUBLISHED: + INLINE CollisionBox(const LPoint3f ¢er, + float x, float y, float z); + INLINE CollisionBox(float cx, float cy, float cz, + float x, float y, float z); + INLINE CollisionBox(const LPoint3f &min, const LPoint3f &max); + + virtual LPoint3f get_collision_origin() const; + +protected: + INLINE CollisionBox(); + +public: + INLINE CollisionBox(const CollisionBox ©); + 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; + + virtual LPoint3f get_approx_center() const; + virtual LPoint3f get_min() const; + virtual LPoint3f get_max() const; + + INLINE static void flush_level(); + void setup_box(); + +PUBLISHED: + INLINE_MATHUTIL int get_num_points() const; + INLINE_MATHUTIL LPoint3f get_point_aabb(int n) const; + INLINE_MATHUTIL LPoint3f get_point(int n) const; + INLINE_MATHUTIL int get_num_planes() const; + INLINE_MATHUTIL Planef set_plane(int n) const; + INLINE_MATHUTIL Planef get_plane(int n) const; + INLINE void set_center(const LPoint3f ¢er); + INLINE void set_center(float x, float y, float z); + INLINE const LPoint3f &get_center() const; + INLINE float get_radius() const; + +protected: + virtual PT(BoundingVolume) compute_internal_bounds() const; + virtual PT(CollisionEntry) + test_intersection_from_sphere(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_ray(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_box(const CollisionEntry &entry) const; + + virtual void fill_viz_geom(); + +protected: + Vertexf compute_point(float latitude, float longitude) const; + +private: + LPoint3f _center; + LPoint3f _min; + LPoint3f _max; + float _x, _y, _z, _radius; + LPoint3f _vertex[8]; // Each of the Eight Vertices of the Box + Planef _planes[6]; //Points to each of the six sides of the Box + + static const int plane_def[6][4]; + + static PStatCollector _volume_pcollector; + static PStatCollector _test_pcollector; + +private: + INLINE static bool is_right(const LVector2f &v1, const LVector2f &v2); + INLINE static float dist_to_line(const LPoint2f &p, + const LPoint2f &f, const LVector2f &v); + static float dist_to_line_segment(const LPoint2f &p, + const LPoint2f &f, const LPoint2f &t, + const LVector2f &v); + +public: + class PointDef { + public: + INLINE PointDef(const LPoint2f &p, const LVector2f &v); + INLINE PointDef(float x, float y); + INLINE PointDef(const PointDef ©); + INLINE void operator = (const PointDef ©); + + LPoint2f _p; // the point in 2-d space + LVector2f _v; // the normalized vector to the next point + }; + typedef pvector Points; + + static void compute_vectors(Points &points); + void draw_polygon(GeomNode *viz_geom_node, GeomNode *bounds_viz_geom_node, + const Points &points) const; + + bool point_is_inside(const LPoint2f &p, const Points &points) const; + float dist_to_polygon(const LPoint2f &p, const Points &points) const; + + void setup_points(const LPoint3f *begin, const LPoint3f *end, int plane); + INLINE LPoint2f to_2d(const LVecBase3f &point3d, int plane) const; + INLINE void calc_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const; + INLINE void rederive_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const; + INLINE static LPoint3f to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat); + LPoint3f legacy_to_3d(const LVecBase2f &point2d, int axis) const; + bool clip_polygon(Points &new_points, const Points &source_points, + const Planef &plane,int plane_no) const; + bool apply_clip_plane(Points &new_points, const ClipPlaneAttrib *cpa, + const TransformState *net_transform, int plane_no) const; + +private: + Points _points[6]; // one set of points for each of the six planes that make up the box + LMatrix4f _to_2d_mat[6]; + +public: + INLINE Points get_plane_points( int n ); + +public: + static void register_with_read_factory(); + virtual void write_datagram(BamWriter *manager, Datagram &me); + +protected: + static TypedWritable *make_CollisionBox(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, "CollisionBox", + 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 "collisionBox.I" + +#endif /* COLLISIONBOX_H */ diff --git a/panda/src/collide/collisionSolid.cxx b/panda/src/collide/collisionSolid.cxx index b3aa064ae6..9c64741e8c 100644 --- a/panda/src/collide/collisionSolid.cxx +++ b/panda/src/collide/collisionSolid.cxx @@ -19,6 +19,7 @@ #include "collisionRay.h" #include "collisionSegment.h" #include "collisionParabola.h" +#include "collisionBox.h" #include "collisionEntry.h" #include "boundingSphere.h" #include "datagram.h" @@ -309,6 +310,21 @@ test_intersection_from_parabola(const CollisionEntry &) const { return NULL; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSolid::test_intersection_from_box +// Access: Protected, Virtual +// Description: This is part of the double-dispatch implementation of +// test_intersection(). It is called when the "from" +// object is a box. +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSolid:: +test_intersection_from_box(const CollisionEntry &) const { + report_undefined_intersection_test(CollisionBox::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 ee4b173a08..7fc45759af 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -110,6 +110,8 @@ protected: test_intersection_from_segment(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_parabola(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_box(const CollisionEntry &entry) const; static void report_undefined_intersection_test(TypeHandle from_type, TypeHandle into_type); @@ -178,6 +180,7 @@ private: friend class CollisionSegment; friend class CollisionParabola; friend class CollisionHandlerFluidPusher; + friend class CollisionBox; }; INLINE ostream &operator << (ostream &out, const CollisionSolid &cs) { diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index 618f98b141..39e0f3535b 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -456,6 +456,132 @@ test_intersection_from_line(const CollisionEntry &entry) const { return new_entry; } +//////////////////////////////////////////////////////////////////// +// Function: CollisionSphere::test_intersection_from_box +// Access: Public, Virtual +// Description: Double dispatch point for box as a FROM object +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSphere:: +test_intersection_from_box(const CollisionEntry &entry) const { + const CollisionBox *box; + DCAST_INTO_R(box, entry.get_from(), 0); + + CPT(TransformState) wrt_space = entry.get_wrt_space(); + CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + CollisionBox local_b( *box ); + local_b.xform( wrt_mat ); + + LPoint3f from_center = local_b.get_center(); + + LPoint3f orig_center = get_center(); + LPoint3f to_center = orig_center; + bool moved_from_center = false; + float t = 1.0f; + LPoint3f contact_point(from_center); + float actual_t = 1.0f; + + float to_radius = get_radius(); + float to_radius_2 = to_radius * to_radius; + + int ip; + float max_dist,dist; + bool intersect; + Planef plane; + LVector3f normal; + + for( ip = 0, intersect=false; ip < 6 && !intersect; ip++ ){ + plane = local_b.get_plane( ip ); + if (local_b.get_plane_points(ip).size() < 3) { + continue; + } + normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal(); + + #ifndef NDEBUG + /* + if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) { + collide_cat.info() + << "polygon being collided with " << entry.get_into_node_path() + << " has normal " << normal << " of length " << normal.length() + << "\n"; + normal.normalize(); + } + */ + #endif + + // The nearest point within the plane to our center is the + // intersection of the line (center, center - normal) with the plane. + + if (!plane.intersects_line(dist, to_center, -(plane.get_normal()))) { + // No intersection with plane? This means the plane's effective + // normal was within the plane itself. A useless polygon. + continue; + } + + if (dist > to_radius || dist < -to_radius) { + // No intersection with the plane. + continue; + } + + LPoint2f p = local_b.to_2d(to_center - dist * plane.get_normal(), ip); + float edge_dist = 0.0f; + + edge_dist = local_b.dist_to_polygon(p, local_b.get_plane_points(ip)); + + if(edge_dist < 0) { + intersect = true; + continue; + } + + if((edge_dist > 0) && + ((edge_dist * edge_dist + dist * dist) > to_radius_2)) { + // No intersection; the circle is outside the polygon. + continue; + } + + // The sphere appears to intersect the polygon. If the edge is less + // than to_radius away, the sphere may be resting on an edge of + // the polygon. Determine how far the center of the sphere must + // remain from the plane, based on its distance from the nearest + // edge. + + max_dist = to_radius; + if (edge_dist >= 0.0f) { + float max_dist_2 = max(to_radius_2 - edge_dist * edge_dist, 0.0f); + max_dist = csqrt(max_dist_2); + } + + if (dist > max_dist) { + // There's no intersection: the sphere is hanging off the edge. + continue; + } + intersect = true; + } + if( !intersect ) + 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); + + float into_depth = max_dist - dist; + + new_entry->set_surface_normal(normal); + new_entry->set_surface_point(to_center - normal * dist); + new_entry->set_interior_point(to_center - normal * (dist + into_depth)); + new_entry->set_contact_pos(contact_point); + new_entry->set_contact_normal(plane.get_normal()); + new_entry->set_t(actual_t); + + return new_entry; +} + //////////////////////////////////////////////////////////////////// // Function: CollisionSphere::test_intersection_from_ray // Access: Public, Virtual diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index 7952ffa83a..9045cfa9ce 100644 --- a/panda/src/collide/collisionSphere.h +++ b/panda/src/collide/collisionSphere.h @@ -18,6 +18,7 @@ #include "pandabase.h" #include "collisionSolid.h" #include "parabola.h" +#include "collisionBox.h" //////////////////////////////////////////////////////////////////// // Class : CollisionSphere @@ -72,6 +73,8 @@ protected: test_intersection_from_segment(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_parabola(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_box(const CollisionEntry &entry) const; virtual void fill_viz_geom(); diff --git a/panda/src/collide/collisionTraverser.cxx b/panda/src/collide/collisionTraverser.cxx index d041ef7116..b2f6b05e40 100644 --- a/panda/src/collide/collisionTraverser.cxx +++ b/panda/src/collide/collisionTraverser.cxx @@ -20,6 +20,7 @@ #include "collisionRecorder.h" #include "collisionVisualizer.h" #include "collisionSphere.h" +#include "collisionBox.h" #include "collisionTube.h" #include "collisionPolygon.h" #include "collisionPlane.h" @@ -370,6 +371,7 @@ traverse(const NodePath &root) { CollisionTube::flush_level(); CollisionPolygon::flush_level(); CollisionPlane::flush_level(); + CollisionBox::flush_level(); } #ifdef DO_COLLISION_RECORDING diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index 68b1754418..f49d2e2c79 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -39,6 +39,7 @@ #include "collisionSphere.h" #include "collisionTraverser.h" #include "collisionTube.h" +#include "collisionBox.h" #include "collisionVisualizer.h" #include "dconfig.h" @@ -142,6 +143,7 @@ init_libcollide() { CollisionSphere::init_type(); CollisionTraverser::init_type(); CollisionTube::init_type(); + CollisionBox::init_type(); #ifdef DO_COLLISION_RECORDING CollisionRecorder::init_type(); @@ -160,4 +162,5 @@ init_libcollide() { CollisionSegment::register_with_read_factory(); CollisionSphere::register_with_read_factory(); CollisionTube::register_with_read_factory(); + CollisionBox::register_with_read_factory(); }