diff --git a/panda/src/collide/CMakeLists.txt b/panda/src/collide/CMakeLists.txt index 32ea22008f..296a40eeed 100644 --- a/panda/src/collide/CMakeLists.txt +++ b/panda/src/collide/CMakeLists.txt @@ -12,6 +12,7 @@ set(P3COLLIDE_HEADERS collisionHandlerPusher.I collisionHandlerPusher.h collisionHandlerFluidPusher.I collisionHandlerFluidPusher.h collisionHandlerQueue.h + collisionHeightfield.I collisionHeightfield.h collisionInvSphere.I collisionInvSphere.h collisionLine.I collisionLine.h collisionLevelStateBase.I collisionLevelStateBase.h @@ -46,6 +47,7 @@ set(P3COLLIDE_SOURCES collisionHandlerPusher.cxx collisionHandlerFluidPusher.cxx collisionHandlerQueue.cxx + collisionHeightfield.cxx collisionLevelStateBase.cxx collisionLevelState.cxx collisionInvSphere.cxx diff --git a/panda/src/collide/collisionHeightfield.I b/panda/src/collide/collisionHeightfield.I new file mode 100644 index 0000000000..8dd9f4b974 --- /dev/null +++ b/panda/src/collide/collisionHeightfield.I @@ -0,0 +1,102 @@ +/** + * 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." + * + * @file collisionHeightfield.I + * @author hecris + * @date 2019-07-01 + */ + +/** + * + */ +INLINE CollisionHeightfield:: +CollisionHeightfield() { +} + +/** + * + */ +INLINE CollisionHeightfield:: +CollisionHeightfield(const CollisionHeightfield ©) { +} + +/** + * + */ +INLINE CollisionHeightfield:: +~CollisionHeightfield() { + delete[] _nodes; + } + +/** + * + */ +INLINE void CollisionHeightfield:: +flush_level() { + _volume_pcollector.flush_level(); + _test_pcollector.flush_level(); +} + +/** + * + */ +INLINE PNMImage CollisionHeightfield:: +get_heightfield() { + return _heightfield; +} + +/** + * + */ +INLINE void CollisionHeightfield:: +set_heightfield(PNMImage heightfield) { + int r = _heightfield.get_x_size(); + int c = _heightfield.get_y_size(); + _heightfield = heightfield; + + if (_heightfield.get_x_size() == r && + _heightfield.get_y_size() == c) { + fill_quadtree_heights(); + } else { + fill_quadtree_areas(); + fill_quadtree_heights(); + } +} + +/** + * + */ +INLINE PN_stdfloat CollisionHeightfield:: +get_max_height() { + return _max_height; +} + +/** + * + */ +INLINE void CollisionHeightfield:: +set_max_height(PN_stdfloat max_height) { + _max_height = max_height; + fill_quadtree_heights(); +} + +/** + * + */ +INLINE int CollisionHeightfield:: +get_num_subdivisions() { + return _num_subdivisions; +} + +/** + * + */ +INLINE PN_stdfloat CollisionHeightfield:: +get_height(int x, int y) const { + return _heightfield.get_gray(x, y) * _max_height; +} diff --git a/panda/src/collide/collisionHeightfield.cxx b/panda/src/collide/collisionHeightfield.cxx new file mode 100644 index 0000000000..6568163d31 --- /dev/null +++ b/panda/src/collide/collisionHeightfield.cxx @@ -0,0 +1,819 @@ +/** + * 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." + * + * @file collisionHeightfield.cxx + * @author hecris + * @date 2019-07-01 + */ + +#include "collisionBox.h" +#include "collisionRay.h" +#include "collisionSphere.h" +#include "collisionHeightfield.h" +#include "collisionHandler.h" +#include "collisionEntry.h" +#include "cmath.h" +#include "config_collide.h" +#include "boundingBox.h" +#include "datagram.h" +#include "datagramIterator.h" +#include "bamReader.h" +#include "bamWriter.h" +#include "geom.h" +#include "geomTriangles.h" +#include "geomVertexWriter.h" +#include +#include + +using std::min; +using std::max; +using std::queue; +using std::vector; +using std::sort; + +PStatCollector CollisionHeightfield::_volume_pcollector( + "Collision Volumes:CollisionHeightfield"); +PStatCollector CollisionHeightfield::_test_pcollector( + "Collision Tests:CollisionHeightfield"); +TypeHandle CollisionHeightfield::_type_handle; + +/** + * + */ +CollisionHeightfield:: +CollisionHeightfield(PNMImage heightfield, + PN_stdfloat max_height, int num_subdivisions) { + _heightfield = heightfield; + _max_height = max_height; + _nodes_count = 0; + + set_num_subdivisions(num_subdivisions); +} + +/** + * Sets the number of quadtree subdivisions and modifies + * the quadtree accordingly. This should be called when a + * user wants to modify the number of quadtree subdivisions + * or from a constructor to initialize the quadtree. + * + * If the number of subdivisions is too high, it will + * automatically be decremented. + */ +void CollisionHeightfield:: +set_num_subdivisions(int num_subdivisions) { + // The number of subdivisions should not be negative + nassertv(num_subdivisions >= 0 && num_subdivisions <= 10); + // Determine the number of quadtree nodes needed + // for the corresponding number of subdivisions. + int nodes_count = 0; + for (int i = 0; i <= num_subdivisions; i++) { + nodes_count += 1 << (i * 2); + } + if (nodes_count == _nodes_count) { + // No changes to quadtree to be done + return; + } + // Calculate the index of the first quad tree leaf node + int leaf_first_index = nodes_count - pow(4, num_subdivisions); + // Calculate the area of a leaf node in the quadtree + PN_stdfloat heightfield_area = _heightfield.get_x_size() * + _heightfield.get_y_size(); + nassertv(heightfield_area > 0); + PN_stdfloat num_leafs = nodes_count - leaf_first_index; + // If the area is too small (less than 1), then we + // retry by decrementing the number of subdivisions. + if (heightfield_area / num_leafs < 1) { + set_num_subdivisions(num_subdivisions - 1); + return; + } + if (nodes_count < _nodes_count) { + // If the user is decreasing the number of subdivisions, then + // we need only to update the index where the quadtree leaves start. + _leaf_first_index = leaf_first_index; + } else { + // Otherwise we need to build a new quadtree + if (_nodes_count != 0) { + // There is an existing quadtree, delete it first + delete[] _nodes; + _nodes = nullptr; + } + QuadTreeNode *nodes = new QuadTreeNode[nodes_count]; + _nodes = nodes; + _nodes_count = nodes_count; + _leaf_first_index = leaf_first_index; + fill_quadtree_areas(); + fill_quadtree_heights(); + } + _num_subdivisions = num_subdivisions; +} + +/** + * + */ +void CollisionHeightfield:: +fill_quadtree_areas() { + nassertv(_heightfield.get_x_size() > 0 && + _heightfield.get_y_size() > 0); + + _nodes[0].area.min = {0, 0}; + _nodes[0].area.max = {(float)_heightfield.get_x_size(), + (float)_heightfield.get_y_size()}; + _nodes[0].index = 0; + QuadTreeNode parent; + for (int i = 1; i < _nodes_count; i += 4) { + parent = _nodes[(i-1) / 4]; + LVector2 sub_area = (parent.area.max - parent.area.min) / 2; + // SE Quadrant + _nodes[i].area.min = parent.area.min; + _nodes[i].area.max = parent.area.min + sub_area; + _nodes[i].index = i; + // SW Quadrant + _nodes[i + 1].area.min = {parent.area.min[0] + sub_area[0], + parent.area.min[1]}; + _nodes[i + 1].area.max = _nodes[i + 1].area.min + sub_area; + _nodes[i + 1].index = i + 1; + // NE Quadrant + _nodes[i + 2].area.min = {parent.area.min[0], + parent.area.min[1] + sub_area[1]}; + _nodes[i + 2].area.max = _nodes[i + 2].area.min + sub_area; + _nodes[i + 2].index = i + 2; + // NW Quadrant + _nodes[i + 3].area.min = parent.area.min + sub_area; + _nodes[i + 3].area.max = parent.area.max; + _nodes[i + 3].index = i + 3; + } +} + +/** + * Processes the heightfield image, setting the height values + * of each quadtree node. + * + * @relates set_max_height + * @relates set_heightfield + */ +void CollisionHeightfield:: +fill_quadtree_heights() { + QuadTreeNode node; + QuadTreeNode child; + for (int i = _nodes_count - 1; i >= 0; i--) { + node = _nodes[i]; + PN_stdfloat height_min = INT_MAX; + PN_stdfloat height_max = INT_MIN; + if (i >= _leaf_first_index) { + for (int x = node.area.min[0]; x < node.area.max[0]; x++) { + for (int y = node.area.min[1]; y < node.area.max[1]; y++) { + PN_stdfloat value = _heightfield.get_gray(x, + _heightfield.get_y_size() - 1 - y) * _max_height; + + height_min = min(value, height_min); + height_max = max(value, height_max); + } + } + } else { + for (int c = (i * 4) + 1, cmax = c + 4; c < cmax; c++) { + child = _nodes[c]; + height_min = min(child.height_min, height_min); + height_max = max(child.height_max, height_max); + } + } + _nodes[i].height_min = height_min; + _nodes[i].height_max = height_max; + } +} + +/** + * + */ +PT(CollisionEntry) CollisionHeightfield:: +test_intersection_from_ray(const CollisionEntry &entry) const { + const CollisionRay *ray; + DCAST_INTO_R(ray, entry.get_from(), nullptr); + const LMatrix4 &wrt_mat = entry.get_wrt_mat(); + + LPoint3 from_origin = ray->get_origin() * wrt_mat; + LPoint3 from_direction = ray->get_direction() * wrt_mat; + + IntersectionParams params; + params.from_origin = from_origin; + params.from_direction = from_direction; + vector intersections; + intersections = find_intersections(line_intersects_box, params); + + if (intersections.empty()) { + return nullptr; + } + // Sort intersections by their t1 values so we can return + // the first intersection found. + sort(intersections.begin(), intersections.end()); + + double t1, t2; + for (size_t i = 0; i < intersections.size(); i++) { + t1 = intersections[i].tmin; + t2 = intersections[i].tmax; + + if (t1 < 0.0 && t2 < 0.0) continue; + t1 = max(t1, 0.0); + + LPoint3 p1 = from_origin + from_direction * t1; + LPoint3 p2 = from_origin + from_direction * t2; + // We use Bresenhaum's Line Algorithm to directly get the heightfield + // coordinates that the line passes through. + int err = cabs(2 * (p2[1] - p1[1])); + int deltaerr = cabs(err - (p2[0] - p1[0])); + + int x = p1[0], y = p1[1]; + bool x_increasing = x < p2[0]; + bool y_increasing = y < p2[1]; + while ((x_increasing && x <= p2[0]) || (!x_increasing && x >= p2[0])) { + bool intersected = false; + Triangle int_tri; + vector triangles = get_triangles(x, y); + double min_t = DBL_MAX; + for (Triangle tri : triangles) { + double t; + if (line_intersects_triangle(t, from_origin, from_direction, tri)) { + if (t < 0) continue; + if (t < min_t) { + intersected = true; + min_t = t; + int_tri = tri; + } + } + } + // If the ray intersects this heightfield element, we can + // stop here and return the intersection. + if (intersected) { + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + new_entry->set_surface_point(from_origin + from_direction * min_t); + LPlane p = LPlane(int_tri.p1, int_tri.p2, int_tri.p3); + LVector3 normal = p.get_normal(); + if (p.dist_to_plane(from_origin) < 0.0f) normal *= -1; + new_entry->set_surface_normal(normal); + return new_entry; + } + // Otherwise, go to the next heightfield element + deltaerr += err; + if (deltaerr >= 0) { + if (y_increasing) y++; + else y--; + deltaerr -= 2 * (p2[0] - p1[0]); + } + if (x_increasing) x++; + else x--; + } + } + + return nullptr; +} + +/** + * + */ +PT(CollisionEntry) CollisionHeightfield:: +test_intersection_from_sphere(const CollisionEntry &entry) const { + const CollisionSphere *sphere; + DCAST_INTO_R(sphere, entry.get_from(), nullptr); + + CPT(TransformState) wrt_space = entry.get_wrt_space(); + const LMatrix4 &wrt_mat = wrt_space->get_mat(); + + LPoint3 center = sphere->get_center() * wrt_mat; + LVector3 radius_v = LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; + PN_stdfloat radius_2 = radius_v.length_squared(); + PN_stdfloat radius = csqrt(radius_2); + + IntersectionParams params; + params.center = center; + params.radius = radius; + + vector intersections; + intersections= find_intersections(sphere_intersects_box, params); + + if (intersections.empty()) { + return nullptr; + } + + LPoint3 point; + LPoint3 closest_point; + PN_stdfloat dist; + PN_stdfloat dist_min = FLT_MAX; + + bool intersected = false; + for (size_t i = 0; i < intersections.size(); i++) { + QuadTreeNode node = _nodes[intersections[i].node_index]; + // Iterate through the circle's area and find triangle intersections, + // find the one closest to the center of the sphere. + for (int dx = -radius; dx <= radius; dx++) { + for (int dy = -radius; dy <= radius; dy++) { + int x = dx + center[0]; + int y = dy + center[1]; + if (x < node.area.min[0] || y < node.area.min[1] || + x > node.area.max[0] || y > node.area.max[1]) { + // point not contained in rectangle + continue; + } + if (dx * dx + dy * dy > radius_2) continue; + + vector triangles = get_triangles(x, y); + for (Triangle tri : triangles) { + point = closest_point_on_triangle(center, tri); + dist = (point - center).length_squared(); + if (dist > radius_2) continue; + if (dist < dist_min) { + intersected = true; + dist_min = dist; + closest_point = point; + } + } + } + } + } + if (intersected) { + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + LVector3 v(center - closest_point); + PN_stdfloat v_length = v.length(); + if (IS_NEARLY_ZERO(v_length)) { + v.set(1, 0, 0); + } else { + v /= v_length; + } + new_entry->set_surface_point(closest_point); + new_entry->set_surface_normal(v); + new_entry->set_interior_point(center - radius * v); + return new_entry; + } else { + return nullptr; + } +} + +/** + * + */ +PT(CollisionEntry) CollisionHeightfield:: +test_intersection_from_box(const CollisionEntry &entry) const { + const CollisionBox *box; + DCAST_INTO_R(box, entry.get_from(), nullptr); + + const LMatrix4 &wrt_mat = entry.get_wrt_mat(); + LPoint3 box_min = box->get_min() * wrt_mat; + LPoint3 box_max = box->get_max() * wrt_mat; + + IntersectionParams params; + params.box_min = box_min; + params.box_max = box_max; + + vector intersections; + intersections = find_intersections(box_intersects_box, params); + + if (intersections.empty()) { + return nullptr; + } + + bool intersected = false; + Triangle intersected_tri; + for (size_t i = 0; i < intersections.size(); i++) { + QuadTreeNode node = _nodes[intersections[i].node_index]; + // Find the overlapping rectangle between the two boxes and + // test the heightfield elements in that area. + LVecBase2 overlap_min = LVecBase2(max(box_min[0], node.area.min[0]), + max(box_min[1], node.area.min[1])); + LVecBase2 overlap_max = LVecBase2(min(box_max[0], node.area.max[0]), + min(box_max[1], node.area.max[1])); + for (int x = overlap_min[0]; x < overlap_max[0]; x++) { + for (int y = overlap_min[1]; y < overlap_max[1]; y++) { + vector triangles = get_triangles(x, y); + for (Triangle tri : triangles) { + if (box_intersects_triangle(box_min, box_max, tri)) { + intersected = true; + intersected_tri = tri; + } + } + } + } + } + + if (intersected) { + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + LPlane p = LPlane(intersected_tri.p1, intersected_tri.p2, intersected_tri.p3); + LVector3 normal = p.get_normal(); + if (p.dist_to_plane(box->get_center() * wrt_mat) < 0.0f) { + normal *= -1; + p.flip(); + } + new_entry->set_surface_normal(normal); + PN_stdfloat min_dist = 0; + LPoint3 interior_point; + // Find the deepest vertex and set it as the interior point + for (int i = 0; i < 8; i++) { + LPoint3 vertex = box->get_point(i) * wrt_mat; + PN_stdfloat dist = p.dist_to_plane(vertex); + if (dist <= min_dist) { + min_dist = dist; + interior_point = vertex; + } + } + new_entry->set_interior_point(interior_point); + new_entry->set_surface_point( + closest_point_on_triangle(interior_point, intersected_tri)); + return new_entry; + } else { + return nullptr; + } +} + +/** + * + */ +bool CollisionHeightfield:: +line_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms) { + LPoint3 from = params.from_origin; + LPoint3 delta = params.from_direction; + + double tmin = -DBL_MAX; + double tmax = DBL_MAX; + for (int i = 0; i < 3; ++i) { + PN_stdfloat d = delta[i]; + if (!IS_NEARLY_ZERO(d)) { + double tmin2 = (box_min[i] - from[i]) / d; + double tmax2 = (box_max[i] - from[i]) / d; + if (tmin2 > tmax2) { + std::swap(tmin2, tmax2); + } + tmin = max(tmin, tmin2); + tmax = min(tmax, tmax2); + if (tmin > tmax) { + return false; + } + } else if (from[i] < box_min[i] || from[i] > box_max[i]) { + // The line is parallel + return false; + } + } + params.t1 = tmin; + params.t2 = tmax; + return true; +} + +/** + * + */ +bool CollisionHeightfield:: +line_intersects_triangle(double &t, const LPoint3 &from, + const LPoint3 &delta, + const Triangle &triangle) { + // Implementation of Möller-Trumbore algorithm + PN_stdfloat a,f,u,v; + LVector3 edge1, edge2, h, s, q; + edge1 = triangle.p2 - triangle.p1; + edge2 = triangle.p3 - triangle.p1; + h = delta.cross(edge2); + a = dot(edge1, h); + if (IS_NEARLY_ZERO(a)) { + // line parallel to triangle + return false; + } + f = 1.0 / a; + s = from - triangle.p1; + u = f * dot(s, h); + if (u < 0.0 || u > 1.0) { + return false; + } + q = s.cross(edge1); + v = f * dot(delta, q); + if (v < 0.0 || u + v > 1.0) { + return false; + } + t = f * dot(edge2, q); + return true; +} + +/** + * + */ +bool CollisionHeightfield:: +sphere_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms) { + LPoint3 center = params.center; + double radius = params.radius; + LPoint3 p = center.fmin(box_max).fmax(box_min); + return (center - p).length_squared() <= radius * radius; +} + +/** + * + */ +bool CollisionHeightfield:: +box_intersects_triangle(const LPoint3 &box_min, const LPoint3 &box_max, + const Triangle &triangle) { + // Using the Seperating Axis Theorem, we have a maximum of 13 SAT tests + // Refer to Christer Ericson's Collision Detection book for full explanation + LPoint3 v0 = triangle.p1, v1 = triangle.p2, v2 = triangle.p3; + + PN_stdfloat p0, p1, p2, r; + LVector3 c = (box_min + box_max) * 0.5f; + PN_stdfloat e0 = (box_max[0] - box_min[0]) * 0.5f; + PN_stdfloat e1 = (box_max[1] - box_min[1]) * 0.5f; + PN_stdfloat e2 = (box_max[2] - box_min[2]) * 0.5f; + + v0 = v0 - c; + v1 = v1 - c; + v2 = v2 - c; + + LVector3 f0 = v1 - v0, f1 = v2 - v1, f2 = v0 - v2; + + r = e1 * cabs(f0[2]) + e2 * cabs(f0[1]); + p0 = v0[1]*(-v1[2] + v0[2]) + v0[2]*(v1[1] - v0[1]); + p2 = v2[1]*(-v1[2] + v0[2]) + v2[2]*(v1[1] - v0[1]); + if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false; + + r = e1 * cabs(f1[2]) + e2 * cabs(f1[1]); + p0 = v0[1]*(-v2[2] + v1[2]) + v0[2]*(v2[1] - v1[1]); + p2 = v2[1]*(-v2[2] + v1[2]) + v2[2]*(v2[1] - v1[1]); + if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false; + + r = e1 * cabs(f2[2]) + e2 * cabs(f2[1]); + p0 = v0[1]*(-v0[2] + v2[2]) + v0[2]*(v0[1] - v2[1]); + p1 = v1[1]*(-v0[2] + v2[2]) + v1[2]*(v0[1] - v2[1]); + if (max(-1 * max(p0, p1), min(p0, p1)) > r) return false; + + r = e0 * cabs(f0[2]) + e2 * cabs(f0[0]); + p0 = v0[0]*(v1[2] - v0[2]) + v0[2]*(-v1[0] + v0[0]); + p2 = v2[0]*(v1[2] - v0[2]) + v2[2]*(-v1[0] + v0[0]); + if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false; + + r = e0 * cabs(f1[2]) + e2 * cabs(f1[0]); + p0 = v0[0]*(v2[2] - v1[2]) + v0[2]*(-v2[0] + v1[0]); + p2 = v2[0]*(v2[2] - v1[2]) + v2[2]*(-v2[0] + v1[0]); + if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false; + + r = e0 * cabs(f2[2]) + e2 * cabs(f2[0]); + p0 = v0[0]*(v0[2] - v2[2]) + v0[2]*(-v0[0] + v2[0]); + p1 = v1[0]*(v0[2] - v2[2]) + v1[2]*(-v0[0] + v2[0]); + if (max(-1 * max(p0, p1), min(p0, p1)) > r) return false; + + r = e0 * cabs(f0[1]) + e1 * cabs(f0[0]); + p0 = v0[0]*(-v1[1] + v0[1]) + v0[1]*(v1[0] - v0[0]); + p2 = v2[0]*(-v1[1] + v0[1]) + v2[1]*(v1[0] - v0[0]); + if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false; + + r = e0 * cabs(f1[1]) + e1 * cabs(f1[0]); + p0 = v0[0]*(-v2[1] + v1[1]) + v0[1]*(v2[0] - v1[0]); + p2 = v2[0]*(-v2[1] + v1[1]) + v2[1]*(v2[0] - v1[0]); + if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false; + + r = e0 * cabs(f2[1]) + e1 * cabs(f2[0]); + p0 = v0[0]*(-v0[1] + v2[1]) + v0[1]*(v0[0] - v2[0]); + p1 = v1[0]*(-v0[1] + v2[1]) + v1[1]*(v0[0] - v2[0]); + if (max(-1 * max(p0, p1), min(p0, p1)) > r) return false; + + if (max(max(v0[0], v1[0]), v2[0]) < -e0 || + min(min(v0[0], v1[0]), v2[0]) > e0) return false; + if (max(max(v0[1], v1[1]), v2[1]) < -e1 || + min(min(v0[1], v1[1]), v2[1]) > e1) return false; + if (max(max(v0[2], v1[2]), v2[2]) < -e2 || + min(min(v0[2], v1[2]), v2[2]) > e2) return false; + + LVector3 n = f0.cross(f1); + PN_stdfloat d = dot(n, triangle.p1); + + LPoint3 e = box_max - c; + + PN_stdfloat r2 = e[0] * cabs(n[0]) + e[1] * cabs(n[1]) + e[2] * cabs(n[2]); + PN_stdfloat s = dot(n, c) - d; + return cabs(s) <= r2; +} + +/** + * + */ +bool CollisionHeightfield:: +box_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms) { + + return (box_min[0] <= params.box_max[0] && box_max[0] >= params.box_min[0]) && + (box_min[1] <= params.box_max[1] && box_max[1] >= params.box_min[1]) && + (box_min[2] <= params.box_max[2] && box_max[2] >= params.box_min[2]); +} + +/** + * Returns the closest point on a triangle to another given point. + */ +LPoint3 CollisionHeightfield:: +closest_point_on_triangle(const LPoint3 &p, const Triangle &triangle) { + LVector3 ab = triangle.p2 - triangle.p1; + LVector3 ac = triangle.p3 - triangle.p1; + LVector3 ap = p - triangle.p1; + PN_stdfloat d1 = dot(ab, ap); + PN_stdfloat d2 = dot(ac, ap); + if (d1 <= 0.0f && d2 <= 0.0f) return triangle.p1; + + LVector3 bp = p - triangle.p2; + PN_stdfloat d3 = dot(ab, bp); + PN_stdfloat d4 = dot(ac, bp); + if (d3 >= 0.0f && d4 <= d3) return triangle.p2; + + PN_stdfloat vc = d1 * d4 - d3 * d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { + PN_stdfloat v = d1 / (d1 - d3); + return triangle.p1 + v * ab; + } + + LVector3 cp = p - triangle.p3; + PN_stdfloat d5 = dot(ab, cp); + PN_stdfloat d6 = dot(ac, cp); + if (d6 >= 0.0f && d5 <= d6) return triangle.p3; + + PN_stdfloat vb = d5 * d2 - d1 * d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { + PN_stdfloat w = d2 / (d2 - d6); + return triangle.p1 + w * ac; + } + + PN_stdfloat va = d3 * d6 - d5 * d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { + PN_stdfloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + return triangle.p2 + w * (triangle.p3 - triangle.p2); + } + + PN_stdfloat denom = 1.0f / (va + vb + vc); + PN_stdfloat v = vb * denom; + PN_stdfloat w = vc * denom; + return triangle.p1 + ab * v + ac * w; +} + +/** + * Given a pointer to a function that tests for intersection between a solid + * and a box, return the quad tree nodes that are intersected by that solid. + */ +vector CollisionHeightfield:: +find_intersections(BoxIntersection intersects_box, IntersectionParams params) const { + queue q; + QuadTreeNode node = _nodes[0]; + q.push(node); + + LPoint3 box_min, box_max; + vector intersections; + + while (!q.empty()) { + node = q.front(); + q.pop(); + box_min = {node.area.min[0], node.area.min[1], node.height_min}; + box_max = {node.area.max[0], node.area.max[1], node.height_max}; + if (!intersects_box(box_min, box_max, params)) { + continue; + } + + if (node.index >= _leaf_first_index) { + QuadTreeIntersection intersection = {node.index, params.t1, params.t2}; + intersections.push_back(intersection); + } else { + int child_first_index = 4 * node.index + 1; + q.push(_nodes[child_first_index]); + q.push(_nodes[child_first_index + 1]); + q.push(_nodes[child_first_index + 2]); + q.push(_nodes[child_first_index + 3]); + } + } + + return intersections; +} + +/** + * Return the triangles that are defined at the + * given 2D coordinate. + */ +vector CollisionHeightfield:: +get_triangles(int x, int y) const { + int rows = _heightfield.get_x_size(); + int cols = _heightfield.get_y_size(); + vector triangles; + if (x < 0 || y < 0 || x >= rows || y >= cols) + return triangles; + + Triangle t; + int heightfield_y = cols - 1 - y; + bool odd = (x + heightfield_y) & 1; + t.p1 = LPoint3(x, y, get_height(x, heightfield_y)); + for (int dx = -1; dx <= 1; dx += 2) { + for (int dy = -1; dy <= 1; dy += 2) { + int x2 = x + dx; + int y2 = y - dy; + int heightfield_y2 = heightfield_y + dy; + if (x2 < 0 || heightfield_y2 < 0 || x2 >= rows || heightfield_y2 >= cols) continue; + if (odd) { + t.p2 = LPoint3(x2, y, get_height(x2, heightfield_y)); + t.p3 = LPoint3(x, y2, get_height(x, heightfield_y2)); + } else { + t.p2 = LPoint3(x2, y2, get_height(x2, heightfield_y2)); + t.p3 = LPoint3(x2, y, get_height(x2, heightfield_y)); + triangles.push_back(t); + t.p3 = LPoint3(x, y2, get_height(x, heightfield_y2)); + triangles.push_back(t); + } + } + } + + return triangles; +} + +/** + * Generic CollisionSolid member functions + */ +void CollisionHeightfield:: +fill_viz_geom() { + PT(GeomVertexData) vdata = new GeomVertexData + ("collision", GeomVertexFormat::get_v3(), + Geom::UH_static); + + GeomVertexWriter vertex(vdata, InternalName::get_vertex()); + + PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static); + + int cols = _heightfield.get_x_size(); + int rows = _heightfield.get_y_size(); + + for (int y = 0; y < rows; y++) { + for (int x = 0; x < cols; x++) { + // a point (x, y) in the heightfield image + // maps to (x, y2, height_at(x, y)) in 3D space + int y2 = rows - y - 1; + vertex.add_data3(x, y2, get_height(x, y)); + } + } + + for (int i = 0; i < rows - 1; i++) { + for (int j = 0; j < cols - 1; j++) { + int pos = i * cols + j; + if (pos & 1) { // odd + tris->add_vertices(pos, pos + cols, pos + 1); + tris->add_vertices(pos + 1, pos + cols, pos + cols + 1); + } + else { // even + tris->add_vertices(pos, pos + cols, pos + cols + 1); + tris->add_vertices(pos, pos + cols + 1, pos + 1); + } + } + } + + PT(Geom) geom = new Geom(vdata); + geom->add_primitive(tris); + + _viz_geom->add_geom(geom, get_solid_viz_state()); + _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state()); +} + +CollisionSolid *CollisionHeightfield:: +make_copy() { + return new CollisionHeightfield(*this); +} + +LPoint3 CollisionHeightfield:: +get_collision_origin() const { + return LPoint3(0, 0, 0); +} + +PT(BoundingVolume) CollisionHeightfield:: +compute_internal_bounds() const { + QuadTreeNode node = _nodes[0]; + LPoint3 box_min = {node.area.min[0], node.area.min[1], + node.height_min}; + LPoint3 box_max = {node.area.max[0], node.area.max[1], + node.height_max}; + return new BoundingBox(box_min, box_max); +} + +PStatCollector &CollisionHeightfield:: +get_volume_pcollector() { + return _volume_pcollector; +} + +PStatCollector &CollisionHeightfield:: +get_test_pcollector() { + return _test_pcollector; +} + +TypedWritable *CollisionHeightfield:: +make_CollisionHeightfield(const FactoryParams ¶ms) { + CollisionHeightfield *me = new CollisionHeightfield; + DatagramIterator scan; + BamReader *manager; + + parse_params(params, scan, manager); + me->fillin(scan, manager); + return me; +} + +void CollisionHeightfield:: +fillin(DatagramIterator &scan, BamReader *manager) { +} + +void CollisionHeightfield:: +register_with_read_factory() { + BamReader::get_factory()->register_factory(get_class_type(), make_CollisionHeightfield); +} diff --git a/panda/src/collide/collisionHeightfield.h b/panda/src/collide/collisionHeightfield.h new file mode 100644 index 0000000000..bbeb6da34d --- /dev/null +++ b/panda/src/collide/collisionHeightfield.h @@ -0,0 +1,180 @@ +/** + * 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." + * + * @file collisionHeightfield.h + * @author hecris + * @date 2019-07-01 + */ + +#ifndef COLLISIONHEIGHTFIELD_H +#define COLLISIONHEIGHTFIELD_H + +#include "pandabase.h" +#include "collisionSolid.h" +#include "pnmImage.h" + +/* + * CollisionHeightfield efficiently deals with collisions on uneven + * terrain given a heightfield image. A quad tree is implemented to + * significantly reduce the amount of triangles tested. Each quad + * tree node represents a sub-rectangle of the heightfield image + * and thus a box in 3D space. + * */ +class EXPCL_PANDA_COLLIDE CollisionHeightfield : public CollisionSolid { +PUBLISHED: + CollisionHeightfield(PNMImage heightfield, + PN_stdfloat max_height, int num_subdivisions); + virtual LPoint3 get_collision_origin() const; + + INLINE PNMImage get_heightfield(); + INLINE void set_heightfield(PNMImage heightfield); + + INLINE PN_stdfloat get_max_height(); + INLINE void set_max_height(PN_stdfloat max_height); + + INLINE int get_num_subdivisions(); + void set_num_subdivisions(int num_subdivisions); + + INLINE PN_stdfloat get_height(int x, int y) const; + +protected: + struct Rect { + LVector2 min; + LVector2 max; + }; + + struct QuadTreeNode { + int index; + Rect area; + PN_stdfloat height_min; + PN_stdfloat height_max; + }; + + struct QuadTreeIntersection { + int node_index; + double tmin; + double tmax; + bool operator < (const QuadTreeIntersection& intersection) const { + return tmin < intersection.tmin; + } + }; + + struct IntersectionParams { + // From Line + double t1; + double t2; + LPoint3 from_origin; + LVector3 from_direction; + // From Sphere + LPoint3 center; + double radius; + // From Box + LPoint3 box_min; + LPoint3 box_max; + }; + + struct Triangle { + LPoint3 p1; + LPoint3 p2; + LPoint3 p3; + }; + +private: + PNMImage _heightfield; + PN_stdfloat _max_height; + int _num_subdivisions; + QuadTreeNode *_nodes; + int _nodes_count; + int _leaf_first_index; + void fill_quadtree_areas(); + void fill_quadtree_heights(); + std::vector get_triangles(int x, int y) const; + + // A pointer to a function that tests for intersection between a box and a + // solid defined by the given IntersectionParams. + typedef bool (*BoxIntersection)(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms); + + std::vector find_intersections(BoxIntersection intersects_box, + IntersectionParams params) const; + +protected: + static LPoint3 closest_point_on_triangle(const LPoint3 &p, const Triangle &triangle); + + static bool line_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms); + + static bool line_intersects_triangle(double &t, const LPoint3 &from, + const LPoint3 &delta, + const Triangle &triangle); + + static bool sphere_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms); + + static bool box_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max, + IntersectionParams ¶ms); + + static bool box_intersects_triangle(const LPoint3 &box_min, const LPoint3 &box_max, + const Triangle &triangle); + +protected: + virtual PT(CollisionEntry) + test_intersection_from_ray(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_sphere(const CollisionEntry &entry) const; + virtual PT(CollisionEntry) + test_intersection_from_box(const CollisionEntry &entry) const; + + virtual void fill_viz_geom(); + +public: + INLINE CollisionHeightfield(const CollisionHeightfield ©); + virtual CollisionSolid *make_copy(); + + virtual PStatCollector &get_volume_pcollector(); + virtual PStatCollector &get_test_pcollector(); + + INLINE static void flush_level(); + INLINE ~CollisionHeightfield(); + +protected: + virtual PT(BoundingVolume) compute_internal_bounds() const; + +private: + INLINE CollisionHeightfield(); + static PStatCollector _volume_pcollector; + static PStatCollector _test_pcollector; + +protected: + static TypedWritable *make_CollisionHeightfield(const FactoryParams ¶ms); + void fillin(DatagramIterator &scan, BamReader *manager); + +public: + static void register_with_read_factory(); + +public: + static TypeHandle get_class_type() { + return _type_handle; + } + static void init_type() { + CollisionSolid::init_type(); + register_type(_type_handle, "CollisionHeightfield", + 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 "collisionHeightfield.I" + +#endif diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index 4721d70381..4b1d17ce15 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -181,6 +181,7 @@ private: friend class CollisionParabola; friend class CollisionHandlerFluidPusher; friend class CollisionBox; + friend class CollisionHeightfield; }; INLINE std::ostream &operator << (std::ostream &out, const CollisionSolid &cs) { diff --git a/panda/src/collide/collisionTraverser.cxx b/panda/src/collide/collisionTraverser.cxx index 50f187ad77..87dad21203 100644 --- a/panda/src/collide/collisionTraverser.cxx +++ b/panda/src/collide/collisionTraverser.cxx @@ -19,6 +19,7 @@ #include "collisionRecorder.h" #include "collisionVisualizer.h" #include "collisionSphere.h" +#include "collisionHeightfield.h" #include "collisionBox.h" #include "collisionCapsule.h" #include "collisionPolygon.h" @@ -354,6 +355,7 @@ traverse(const NodePath &root) { CollisionPolygon::flush_level(); CollisionPlane::flush_level(); CollisionBox::flush_level(); + CollisionHeightfield::flush_level(); } #if defined(DO_COLLISION_RECORDING) || !defined(CPPPARSER) diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index b9ca4e368a..88e701abe3 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -25,6 +25,7 @@ #include "collisionHandlerFluidPusher.h" #include "collisionHandlerQueue.h" #include "collisionInvSphere.h" +#include "collisionHeightfield.h" #include "collisionLine.h" #include "collisionLevelStateBase.h" #include "collisionGeom.h" @@ -138,6 +139,7 @@ init_libcollide() { CollisionHandlerFluidPusher::init_type(); CollisionHandlerQueue::init_type(); CollisionInvSphere::init_type(); + CollisionHeightfield::init_type(); CollisionLine::init_type(); CollisionLevelStateBase::init_type(); CollisionGeom::init_type(); @@ -164,6 +166,7 @@ init_libcollide() { CollisionBox::register_with_read_factory(); CollisionCapsule::register_with_read_factory(); CollisionInvSphere::register_with_read_factory(); + CollisionHeightfield::register_with_read_factory(); CollisionLine::register_with_read_factory(); CollisionNode::register_with_read_factory(); CollisionParabola::register_with_read_factory(); diff --git a/panda/src/collide/p3collide_composite1.cxx b/panda/src/collide/p3collide_composite1.cxx index af8ecc07d8..a105cc83f5 100644 --- a/panda/src/collide/p3collide_composite1.cxx +++ b/panda/src/collide/p3collide_composite1.cxx @@ -13,3 +13,4 @@ #include "collisionHandlerFluidPusher.cxx" #include "collisionHandlerQueue.cxx" #include "collisionInvSphere.cxx" +#include "collisionHeightfield.cxx" diff --git a/tests/collide/collisions.py b/tests/collide/collisions.py index cca2a04215..a66b03e4d1 100644 --- a/tests/collide/collisions.py +++ b/tests/collide/collisions.py @@ -2,7 +2,7 @@ from panda3d.core import CollisionNode, NodePath from panda3d.core import CollisionTraverser, CollisionHandlerQueue from panda3d.core import CollisionSphere, CollisionInvSphere, CollisionBox, CollisionPolygon, CollisionCapsule from panda3d.core import CollisionLine, CollisionRay, CollisionSegment, CollisionParabola -from panda3d.core import CollisionPlane +from panda3d.core import CollisionPlane, CollisionHeightfield from panda3d.core import Point3, Vec3, Plane, LParabola diff --git a/tests/collide/test_into_heightfield.py b/tests/collide/test_into_heightfield.py new file mode 100644 index 0000000000..fe2ab07b57 --- /dev/null +++ b/tests/collide/test_into_heightfield.py @@ -0,0 +1,92 @@ +import pytest +from collisions import * +from panda3d.core import PNMImage + +def test_sphere_into_heightfield(): + # Setup PNMImage + img = PNMImage(512, 512, 1) + img.set_gray_val(1, 1, 255) + # Make CollisionHeightfield + max_height = 10 + num_subdivisions = 1 + heightfield = CollisionHeightfield(img, max_height, num_subdivisions) + # The coordinate (1, 1) on our heightfield image + # maps to the coordinate (1, 510, Z) in 3D space + sphere = CollisionSphere((1, 510, 11), 1) + entry, np_from, np_into = make_collision(sphere, heightfield) + assert entry.get_surface_point(np_from) == (1, 510, 10) + assert entry.get_surface_normal(np_from) == (0, 0, 1) + # Set the sphere higher so it is not colliding anymore + sphere.set_center((1, 510, 11.1)) + entry = make_collision(sphere, heightfield)[0] + assert entry is None + # Set the max_height to re-collide with the sphere + max_height = 10.1 + heightfield.set_max_height(max_height) + entry, np_from, np_into = make_collision(sphere, heightfield) + assert entry.get_surface_point(np_from) == (1, 510, 10.1) + + with pytest.raises(AssertionError) as err: + assert heightfield.set_num_subdivisions(-1) == err + assert heightfield.set_num_subdivisions(11) == err + + # Use a greater number of subdivisions, should still work + num_subdivisions = 10 + heightfield.set_num_subdivisions(num_subdivisions) + entry, np_from, np_into = make_collision(sphere, heightfield) + assert entry.get_surface_point(np_from) == (1, 510, 10.1) + # Using 10 subdivisions is overkill for such a small heightfield, + # CollisionHeightfield should've automatically decreased it + assert heightfield.get_num_subdivisions() < num_subdivisions + # Zero subdivisions should work too + num_subdivisions = 0 + heightfield.set_num_subdivisions(num_subdivisions) + entry, np_from, np_into = make_collision(sphere, heightfield) + assert entry.get_surface_point(np_from) == (1, 510, 10.1) + # Modify the heightfield, no longer colliding + img.set_gray_val(1, 1, 254) + heightfield.set_heightfield(img) + entry = make_collision(sphere, heightfield)[0] + assert entry is None + + +def test_ray_into_heightfield(): + # Setup heightfield + img = PNMImage(127, 127, 1) + img.fill_val(0) + max_height = 10 + num_subdivisions = 1 + heightfield = CollisionHeightfield(img, max_height, num_subdivisions) + # Make ray + ray = CollisionRay((100, 100, 100), (-1, -1, -1)) + entry = make_collision(ray, heightfield)[0] + assert entry is not None + + # Ray with only a z component in its direction + ray.set_direction((0, 0, -5)) + entry, np_from, np_into = make_collision(ray, heightfield) + assert entry.get_surface_point(np_from) == (100, 100, 0) + + # Set coordinate (54, 38) on heightfield to gray value of 255 + # Note that coordinate (54, 38) on the heightfield + # maps to (54, 88, Z) in 3D space + img.set_gray_val(54, 38, 255) + heightfield.set_heightfield(img) + + ray.set_origin((54, 88, 10)) + entry, np_from, np_into = make_collision(ray, heightfield) + assert entry.get_surface_point(np_from) == (54, 88, 10) + + +def test_box_into_heightfield(): + # Try using a large non-square heightfield image + img = PNMImage(5023, 5130, 1) + img.set_gray_val(1, 1, 255) + # Make CollisionHeightfield + max_height = 10 + num_subdivisions = 5 + heightfield = CollisionHeightfield(img, max_height, num_subdivisions) + # Make box + box = CollisionBox((1, 5128, 10), 1, 1, 1) + entry = make_collision(box, heightfield) + assert entry is not None