diff --git a/panda/src/collide/collisionBox.I b/panda/src/collide/collisionBox.I index 86473473cf..385152c2a1 100644 --- a/panda/src/collide/collisionBox.I +++ b/panda/src/collide/collisionBox.I @@ -21,11 +21,9 @@ CollisionBox(const LPoint3 ¢er, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z) { _min = LPoint3(_center.get_x() - x, _center.get_y() - y, _center.get_z() - z); _max = LPoint3(_center.get_x() + x, _center.get_y() + y, _center.get_z() + z); - for(int v = 0; v < 8; v++) - _vertex[v] = get_point_aabb(v); - for(int p = 0; p < 6; p++) + for (int p = 0; p < 6; ++p) { _planes[p] = set_plane(p); - setup_box(); + } } /** @@ -33,14 +31,11 @@ CollisionBox(const LPoint3 ¢er, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z) */ INLINE CollisionBox:: CollisionBox(const LPoint3 &min, const LPoint3 &max) : - _min(min), _max(max) + _center((min + max) / 2), _min(min), _max(max) { - _center = (_min + _max) / 2; - for(int v = 0; v < 8; v++) - _vertex[v] = get_point_aabb(v); - for(int p = 0; p < 6; p++) + for (int p = 0; p < 6; ++p) { _planes[p] = set_plane(p); - setup_box(); + } } /** @@ -60,11 +55,9 @@ CollisionBox(const CollisionBox ©) : _min(copy._min), _max(copy._max) { - for(int v = 0; v < 8; v++) - _vertex[v] = copy._vertex[v]; - for(int p = 0; p < 6; p++) + for (int p = 0; p < 6; ++p) { _planes[p] = copy._planes[p]; - setup_box(); + } } /** @@ -139,8 +132,7 @@ get_num_points() const { */ INLINE LPoint3 CollisionBox:: get_point(int n) const { - nassertr(n >= 0 && n < 8, LPoint3::zero()); - return _vertex[n]; + return get_point_aabb(n); } /** @@ -175,6 +167,8 @@ get_plane(int n) const { /** * Creates the nth face of the rectangular solid. + * + * @deprecated Same as get_plane(). */ INLINE LPlane CollisionBox:: set_plane(int n) const { @@ -183,91 +177,3 @@ set_plane(int n) const { get_point(plane_def[n][1]), get_point(plane_def[n][2])); } - - -/** - * Returns true if the 2-d v1 is to the right of v2. - */ -INLINE bool CollisionBox:: -is_right(const LVector2 &v1, const LVector2 &v2) { - return (v1[0] * v2[1] - v1[1] * v2[0]) > 1.0e-6f; -} - -/** - * 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 PN_stdfloat CollisionBox:: -dist_to_line(const LPoint2 &p, - const LPoint2 &f, const LVector2 &v) { - LVector2 v1 = (p - f); - return (v1[0] * v[1] - v1[1] * v[0]); -} - -/** - * 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 LPoint2 CollisionBox:: -to_2d(const LVecBase3 &point3d, int plane) const { - LPoint3 point = LPoint3(point3d) * _to_2d_mat[plane]; - return LPoint2(point[0], point[2]); -} - -/** - * 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(LMatrix4 &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(), - LVector3(0.0f, 0.0f, 1.0f), CS_zup_right); - to_3d_mat.set_row(3, get_plane(plane).get_point()); -} - -/** - * Extrude the indicated point in the polygon's 2-d definition space back into - * 3-d coordinates. - */ -INLINE LPoint3 CollisionBox:: -to_3d(const LVecBase2 &point2d, const LMatrix4 &to_3d_mat) { - return LPoint3(point2d[0], 0.0f, point2d[1]) * to_3d_mat; -} - -/** - * - */ -INLINE CollisionBox::PointDef:: -PointDef(const LPoint2 &p, const LVector2 &v) : _p(p), _v(v) { -} - -/** - * - */ -INLINE CollisionBox::PointDef:: -PointDef(PN_stdfloat x, PN_stdfloat y) : _p(x, y), _v(0.0f, 0.0f) { -} - -/** - * - */ -INLINE CollisionBox::PointDef:: -PointDef(const CollisionBox::PointDef ©) : _p(copy._p), _v(copy._v) { -} - -/** - * - */ -INLINE void CollisionBox::PointDef:: -operator = (const CollisionBox::PointDef ©) { - _p = copy._p; - _v = copy._v; -} diff --git a/panda/src/collide/collisionBox.cxx b/panda/src/collide/collisionBox.cxx index a3e5cbca40..5058dfaf37 100644 --- a/panda/src/collide/collisionBox.cxx +++ b/panda/src/collide/collisionBox.cxx @@ -158,45 +158,6 @@ make_copy() { return new CollisionBox(*this); } -/** - * Compute parameters for each of the box's sides - */ -void CollisionBox:: -setup_box() { - assert(sizeof(_points) / sizeof(_points[0]) == 6); - assert(sizeof(_points[0]) / sizeof(_points[0][0]) == 4); - for (int plane = 0; plane < 6; plane++) { - setup_points(plane); - } -} - -/** - * Computes the plane and 2d projection of points that make up this side. - */ -void CollisionBox:: -setup_points(int plane) { - PointDef *points = _points[plane]; - - // Construct a matrix that rotates the points from the (X,0,Z) plane into - // the 3-d plane. - LMatrix4 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. - for (size_t i = 0; i < 4; ++i) { - LPoint3 point = get_point(plane_def[plane][i]) * _to_2d_mat[plane]; - points[i] = PointDef(point[0], point[2]); - } - - for (size_t i = 0; i < 4; i++) { - points[i]._v = points[(i + 1) % 4]._p - points[i]._p; - points[i]._v.normalize(); - } -} - /** * First Dispatch point for box as a FROM object */ @@ -213,13 +174,9 @@ xform(const LMatrix4 &mat) { _min = _min * mat; _max = _max * 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); } - setup_box(); mark_viz_stale(); mark_internal_bounds_stale(); } @@ -265,9 +222,10 @@ output(std::ostream &out) const { */ PT(BoundingVolume) CollisionBox:: compute_internal_bounds() const { - PN_stdfloat x = _vertex[0].get_x() - _center.get_x(); - PN_stdfloat y = _vertex[0].get_y() - _center.get_y(); - PN_stdfloat z = _vertex[0].get_z() - _center.get_z(); + LPoint3 vertex = get_point_aabb(0); + PN_stdfloat x = vertex.get_x() - _center.get_x(); + PN_stdfloat y = vertex.get_y() - _center.get_y(); + PN_stdfloat z = vertex.get_z() - _center.get_z(); PN_stdfloat radius = sqrt(x * x + y * y + z * z); return new BoundingSphere(_center, radius); } @@ -1134,328 +1092,6 @@ intersects_capsule(double &t, const LPoint3 &from, const LVector3 &delta, return true; } -/** - * 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(i); - 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()); - - LPlane plane = plane_node->get_plane() * new_transform->get_mat(); - if (first_plane) { - first_plane = false; - if (!clip_polygon(new_points, _points[plane_no], 4, plane, plane_no)) { - all_in = false; - } - } else { - Points last_points; - last_points.swap(new_points); - if (!clip_polygon(new_points, last_points.data(), last_points.size(), plane, plane_no)) { - all_in = false; - } - } - } - } - - if (!all_in) { - compute_vectors(new_points); - } - - return all_in; -} -/** - * 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 PointDef *source_points, size_t num_source_points, - const LPlane &plane, int plane_no) const { - new_points.clear(); - if (num_source_points == 0) { - return true; - } - - LPoint3 from3d; - LVector3 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.insert(new_points.end(), source_points, source_points + num_source_points); - return true; - } - return false; - } - - // Project the line of intersection into the 2-d plane. Now we have a 2-d - // clipping line. - LPoint2 from2d = to_2d(from3d,plane_no); - LVector2 delta2d = to_2d(delta3d,plane_no); - - PN_stdfloat a = -delta2d[1]; - PN_stdfloat b = delta2d[0]; - PN_stdfloat 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(num_source_points + 1); - - LPoint2 last_point = source_points[num_source_points - 1]._p; - bool last_is_in = !is_right(last_point - from2d, delta2d); - bool all_in = last_is_in; - for (size_t pi = 0; pi < num_source_points; ++pi) { - const LPoint2 &this_point = source_points[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. - LVector2 d = this_point - last_point; - PN_stdfloat denom = (a * d[0] + b * d[1]); - if (denom != 0.0) { - PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom; - LPoint2 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; -} - -/** - * 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. - */ -PN_stdfloat CollisionBox:: -dist_to_polygon(const LPoint2 &p, const PointDef *points, size_t num_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; - PN_stdfloat best_dist = -1.0f; - - for (size_t i = 0; i < num_points - 1; ++i) { - PN_stdfloat 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; - } - } - } - - PN_stdfloat 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; -} - -/** - * 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. - */ -PN_stdfloat CollisionBox:: -dist_to_line_segment(const LPoint2 &p, - const LPoint2 &f, const LPoint2 &t, - const LVector2 &v) { - LVector2 v1 = (p - f); - PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]); - if (d < 0.0f) { - return d; - } - - // Compute the nearest point on the line. - LPoint2 q = p + LVector2(-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; - } - } - } - } -} - -/** - * Returns true if the indicated point is within the polygon's 2-d space, - * false otherwise. - */ -bool CollisionBox:: -point_is_inside(const LPoint2 &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; -} - -/** - * 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(); - } -} - /** * Factory method to generate a CollisionBox object */ @@ -1474,28 +1110,42 @@ write_datagram(BamWriter *manager, Datagram &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); + for (int i = 0; i < 8; ++i) { + get_point_aabb(i).write_datagram(me); } - PN_stdfloat x = _vertex[0].get_x() - _center.get_x(); - PN_stdfloat y = _vertex[0].get_y() - _center.get_y(); - PN_stdfloat z = _vertex[0].get_z() - _center.get_z(); + LPoint3 vertex = get_point_aabb(0); + PN_stdfloat x = vertex.get_x() - _center.get_x(); + PN_stdfloat y = vertex.get_y() - _center.get_y(); + PN_stdfloat z = vertex.get_z() - _center.get_z(); PN_stdfloat radius = sqrt(x * x + y * y + z * z); me.add_stdfloat(radius); me.add_stdfloat(x); me.add_stdfloat(y); me.add_stdfloat(z); - for(int i=0; i < 6; i++) { + 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); + LMatrix4 to_2d_mat[6]; + for (int i = 0; i < 6; ++i) { + LMatrix4 to_3d_mat; + look_at(to_3d_mat, -get_plane(i).get_normal(), + LVector3(0.0f, 0.0f, 1.0f), CS_zup_right); + to_3d_mat.set_row(3, get_plane(i).get_point()); + + to_2d_mat[i].invert_from(to_3d_mat); + to_2d_mat[i].write_datagram(me); } - for(int i=0; i < 6; i++) { + for (int i = 0; i < 6; ++i) { me.add_uint16(4); + LPoint2 points[4]; for (size_t j = 0; j < 4; j++) { - _points[i][j]._p.write_datagram(me); - _points[i][j]._v.write_datagram(me); + points[j] = (get_point(plane_def[i][j]) * to_2d_mat[i]).get_xz(); + } + for (size_t j = 0; j < 4; j++) { + LPoint2 vec = points[(i + 1) % 4] - points[i]; + vec.normalize(); + points[j].write_datagram(me); + vec.write_datagram(me); } } } @@ -1525,25 +1175,28 @@ fillin(DatagramIterator& scan, BamReader* 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); + for (int i = 0; i < 8; ++i) { + LPoint3 vertex; + vertex.read_datagram(scan); } scan.get_stdfloat(); scan.get_stdfloat(); scan.get_stdfloat(); scan.get_stdfloat(); - for(int i=0; i < 6; i++) { + 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) { + LMatrix4 to_2d_mat; + to_2d_mat.read_datagram(scan); } - for(int i=0; i < 6; i++) { + for (int i = 0; i < 6; ++i) { size_t size = scan.get_uint16(); - nassertv(size == 4); - for (size_t j = 0; j < size; j++) { - _points[i][j]._p.read_datagram(scan); - _points[i][j]._v.read_datagram(scan); + for (size_t j = 0; j < size; ++j) { + LPoint2 p; + LVector2 v; + p.read_datagram(scan); + v.read_datagram(scan); } } } diff --git a/panda/src/collide/collisionBox.h b/panda/src/collide/collisionBox.h index 816f0a694b..95588a8acf 100644 --- a/panda/src/collide/collisionBox.h +++ b/panda/src/collide/collisionBox.h @@ -49,7 +49,6 @@ public: virtual void output(std::ostream &out) const; INLINE static void flush_level(); - void setup_box(); PUBLISHED: INLINE int get_num_points() const; @@ -102,7 +101,6 @@ private: LPoint3 _center; LPoint3 _min; LPoint3 _max; - LPoint3 _vertex[8]; // Each of the Eight Vertices of the Box LPlane _planes[6]; //Points to each of the six sides of the Box static const int plane_def[6][4]; @@ -110,49 +108,6 @@ private: static PStatCollector _volume_pcollector; static PStatCollector _test_pcollector; -private: - INLINE static bool is_right(const LVector2 &v1, const LVector2 &v2); - INLINE static PN_stdfloat dist_to_line(const LPoint2 &p, - const LPoint2 &f, const LVector2 &v); - static PN_stdfloat dist_to_line_segment(const LPoint2 &p, - const LPoint2 &f, const LPoint2 &t, - const LVector2 &v); - -public: - class PointDef { - public: - PointDef() = default; - INLINE PointDef(const LPoint2 &p, const LVector2 &v); - INLINE PointDef(PN_stdfloat x, PN_stdfloat y); - INLINE PointDef(const PointDef ©); - INLINE void operator = (const PointDef ©); - - LPoint2 _p; // the point in 2-d space - LVector2 _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 LPoint2 &p, const Points &points) const; - PN_stdfloat dist_to_polygon(const LPoint2 &p, const PointDef *points, size_t num_points) const; - - void setup_points(int plane); - INLINE LPoint2 to_2d(const LVecBase3 &point3d, int plane) const; - INLINE void calc_to_3d_mat(LMatrix4 &to_3d_mat, int plane) const; - INLINE static LPoint3 to_3d(const LVecBase2 &point2d, const LMatrix4 &to_3d_mat); - bool clip_polygon(Points &new_points, const PointDef *source_points, - size_t num_source_points, const LPlane &plane, - int plane_no) const; - bool apply_clip_plane(Points &new_points, const ClipPlaneAttrib *cpa, - const TransformState *net_transform, int plane_no) const; - -private: - PointDef _points[6][4]; // one set of points for each of the six planes that make up the box - LMatrix4 _to_2d_mat[6]; - public: static void register_with_read_factory(); virtual void write_datagram(BamWriter *manager, Datagram &me);