diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index 6684978318..03baf65930 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -89,6 +89,8 @@ Files: ./servers/physics_3d/gjk_epa.cpp ./servers/physics_3d/joints/slider_joint_3d_sw.h ./servers/physics_3d/soft_body_3d_sw.cpp ./servers/physics_3d/soft_body_3d_sw.h + ./servers/physics_3d/shape_3d_sw.cpp + ./servers/physics_3d/shape_3d_sw.h Comment: Bullet Continuous Collision Detection and Physics Library Copyright: 2003-2008, Erwin Coumans 2007-2021, Juan Linietsky, Ariel Manzur. diff --git a/doc/classes/HeightMapShape3D.xml b/doc/classes/HeightMapShape3D.xml index 6d230bdab8..f6f2a27891 100644 --- a/doc/classes/HeightMapShape3D.xml +++ b/doc/classes/HeightMapShape3D.xml @@ -1,7 +1,7 @@ - Height map shape for 3D physics (Bullet only). + Height map shape for 3D physics. Height map shape resource, which can be added to a [PhysicsBody3D] or [Area3D]. diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp index 471b154813..40e785d699 100644 --- a/modules/bullet/shape_bullet.cpp +++ b/modules/bullet/shape_bullet.cpp @@ -142,7 +142,7 @@ btScaledBvhTriangleMeshShape *ShapeBullet::create_shape_concave(btBvhTriangleMes } } -btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { +btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { const btScalar ignoredHeightScale(1); const int YAxis = 1; // 0=X, 1=Y, 2=Z const bool flipQuadEdges = false; @@ -480,17 +480,10 @@ void HeightMapShapeBullet::set_data(const Variant &p_data) { ERR_FAIL_COND_MSG(l_width < 2, "Map width must be at least 2."); ERR_FAIL_COND_MSG(l_depth < 2, "Map depth must be at least 2."); - // TODO This code will need adjustments if real_t is set to `double`, - // because that precision is unnecessary for a heightmap and Bullet doesn't support it... - - Vector l_heights; + Vector l_heights; Variant l_heights_v = d["heights"]; -#ifdef REAL_T_IS_DOUBLE - if (l_heights_v.get_type() == Variant::PACKED_FLOAT64_ARRAY) { -#else if (l_heights_v.get_type() == Variant::PACKED_FLOAT32_ARRAY) { -#endif // Ready-to-use heights can be passed l_heights = l_heights_v; @@ -511,9 +504,9 @@ void HeightMapShapeBullet::set_data(const Variant &p_data) { l_heights.resize(l_image->get_width() * l_image->get_height()); - real_t *w = l_heights.ptrw(); + float *w = l_heights.ptrw(); const uint8_t *r = im_data.ptr(); - real_t *rp = (real_t *)r; + float *rp = (float *)r; // At this point, `rp` could be used directly for Bullet, but I don't know how safe it would be. for (int i = 0; i < l_heights.size(); ++i) { @@ -521,11 +514,7 @@ void HeightMapShapeBullet::set_data(const Variant &p_data) { } } else { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_MSG("Expected PackedFloat64Array or float Image."); -#else ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); -#endif } ERR_FAIL_COND(l_width <= 0); @@ -534,11 +523,11 @@ void HeightMapShapeBullet::set_data(const Variant &p_data) { // Compute min and max heights if not specified. if (!d.has("min_height") && !d.has("max_height")) { - const real_t *r = l_heights.ptr(); + const float *r = l_heights.ptr(); int heights_size = l_heights.size(); for (int i = 0; i < heights_size; ++i) { - real_t h = r[i]; + float h = r[i]; if (h < l_min_height) { l_min_height = h; @@ -559,7 +548,7 @@ PhysicsServer3D::ShapeType HeightMapShapeBullet::get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; } -void HeightMapShapeBullet::setup(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { +void HeightMapShapeBullet::setup(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { // TODO cell size must be tweaked using localScaling, which is a shared property for all Bullet shapes // If this array is resized outside of here, it should be preserved due to CoW diff --git a/modules/bullet/shape_bullet.h b/modules/bullet/shape_bullet.h index bfd95747eb..5080d13d99 100644 --- a/modules/bullet/shape_bullet.h +++ b/modules/bullet/shape_bullet.h @@ -89,7 +89,7 @@ public: /// IMPORTANT: Remember to delete the shape interface by calling: delete my_shape->getMeshInterface(); static class btConvexPointCloudShape *create_shape_convex(btAlignedObjectArray &p_vertices, const btVector3 &p_local_scaling = btVector3(1, 1, 1)); static class btScaledBvhTriangleMeshShape *create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling = btVector3(1, 1, 1)); - static class btHeightfieldTerrainShape *create_shape_height_field(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); + static class btHeightfieldTerrainShape *create_shape_height_field(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); static class btRayShape *create_shape_ray(real_t p_length, bool p_slips_on_slope); }; @@ -212,7 +212,7 @@ private: class HeightMapShapeBullet : public ShapeBullet { public: - Vector heights; + Vector heights; int width = 0; int depth = 0; real_t min_height = 0.0; @@ -226,7 +226,7 @@ public: virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: - void setup(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); + void setup(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); }; class RayShapeBullet : public ShapeBullet { diff --git a/scene/resources/height_map_shape_3d.cpp b/scene/resources/height_map_shape_3d.cpp index 5593bb766f..6bb0faf098 100644 --- a/scene/resources/height_map_shape_3d.cpp +++ b/scene/resources/height_map_shape_3d.cpp @@ -41,7 +41,7 @@ Vector HeightMapShape3D::get_debug_mesh_lines() const { Vector2 size(map_width - 1, map_depth - 1); Vector2 start = size * -0.5; - const real_t *r = map_data.ptr(); + const float *r = map_data.ptr(); // reserve some memory for our points.. points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2)); @@ -100,7 +100,7 @@ void HeightMapShape3D::set_map_width(int p_new) { int new_size = map_width * map_depth; map_data.resize(map_width * map_depth); - real_t *w = map_data.ptrw(); + float *w = map_data.ptrw(); while (was_size < new_size) { w[was_size++] = 0.0; } @@ -124,7 +124,7 @@ void HeightMapShape3D::set_map_depth(int p_new) { int new_size = map_width * map_depth; map_data.resize(new_size); - real_t *w = map_data.ptrw(); + float *w = map_data.ptrw(); while (was_size < new_size) { w[was_size++] = 0.0; } @@ -146,8 +146,8 @@ void HeightMapShape3D::set_map_data(PackedFloat32Array p_new) { } // copy - real_t *w = map_data.ptrw(); - const real_t *r = p_new.ptr(); + float *w = map_data.ptrw(); + const float *r = p_new.ptr(); for (int i = 0; i < size; i++) { float val = r[i]; w[i] = val; @@ -189,7 +189,7 @@ void HeightMapShape3D::_bind_methods() { HeightMapShape3D::HeightMapShape3D() : Shape3D(PhysicsServer3D::get_singleton()->shape_create(PhysicsServer3D::SHAPE_HEIGHTMAP)) { map_data.resize(map_width * map_depth); - real_t *w = map_data.ptrw(); + float *w = map_data.ptrw(); w[0] = 0.0; w[1] = 0.0; w[2] = 0.0; diff --git a/scene/resources/height_map_shape_3d.h b/scene/resources/height_map_shape_3d.h index 6fc88cff90..1219791c56 100644 --- a/scene/resources/height_map_shape_3d.h +++ b/scene/resources/height_map_shape_3d.h @@ -39,8 +39,8 @@ class HeightMapShape3D : public Shape3D { int map_width = 2; int map_depth = 2; PackedFloat32Array map_data; - float min_height = 0.0; - float max_height = 0.0; + real_t min_height = 0.0; + real_t max_height = 0.0; protected: static void _bind_methods(); diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index bf0946a0e2..48fa9c2813 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -30,10 +30,28 @@ #include "shape_3d_sw.h" +#include "core/io/image.h" #include "core/math/geometry_3d.h" #include "core/math/quick_hull.h" #include "core/templates/sort_array.h" +// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 @@ -1614,7 +1632,7 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { /* HEIGHT MAP SHAPE */ -Vector HeightMapShape3DSW::get_heights() const { +Vector HeightMapShape3DSW::get_heights() const { return heights; } @@ -1626,10 +1644,6 @@ int HeightMapShape3DSW::get_depth() const { return depth; } -real_t HeightMapShape3DSW::get_cell_size() const { - return cell_size; -} - void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { //not very useful, but not very used either p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); @@ -1640,7 +1654,198 @@ Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { return get_aabb().get_support(p_normal); } +struct _HeightmapSegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + + Vector3 result; + Vector3 normal; + + const HeightMapShape3DSW *heightmap = nullptr; + FaceShape3DSW *face = nullptr; +}; + +_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { + Vector3 res; + Vector3 normal; + if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) { + p_params.result = res; + p_params.normal = normal; + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, int p_x, int p_z) { + // First triangle. + p_params.heightmap->_get_point(p_x, p_z, p_params.face->vertex[0]); + p_params.heightmap->_get_point(p_x + 1, p_z, p_params.face->vertex[1]); + p_params.heightmap->_get_point(p_x, p_z + 1, p_params.face->vertex[2]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + // Second triangle. + p_params.face->vertex[0] = p_params.face->vertex[1]; + p_params.heightmap->_get_point(p_x + 1, p_z + 1, p_params.face->vertex[1]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + return false; +} + bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { + if (heights.is_empty()) { + return false; + } + + Vector3 local_begin = p_begin + local_origin; + Vector3 local_end = p_end + local_origin; + + FaceShape3DSW face; + face.backface_collision = false; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + params.heightmap = this; + params.face = &face; + + // Quantize the ray begin/end. + int begin_x = floor(local_begin.x); + int begin_z = floor(local_begin.z); + int end_x = floor(local_end.x); + int end_z = floor(local_end.z); + + if ((begin_x == end_x) && (begin_z == end_z)) { + // Simple case for rays that don't traverse the grid horizontally. + // Just perform a test on the given cell. + int x = CLAMP(begin_x, 0, width - 2); + int z = CLAMP(begin_z, 0, depth - 2); + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + } else { + // Perform grid query from projected ray. + Vector2 ray_dir_proj(local_end.x - local_begin.x, local_end.z - local_begin.z); + real_t ray_dist_proj = ray_dir_proj.length(); + + if (ray_dist_proj < CMP_EPSILON) { + ray_dir_proj = Vector2(); + } else { + ray_dir_proj /= ray_dist_proj; + } + + const int x_step = (ray_dir_proj.x > CMP_EPSILON) ? 1 : ((ray_dir_proj.x < -CMP_EPSILON) ? -1 : 0); + const int z_step = (ray_dir_proj.y > CMP_EPSILON) ? 1 : ((ray_dir_proj.y < -CMP_EPSILON) ? -1 : 0); + + const real_t infinite = 1e20; + const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_proj.x) : infinite; + const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_proj.y) : infinite; + + real_t cross_x; // At which value of `param` we will cross a x-axis lane? + real_t cross_z; // At which value of `param` we will cross a z-axis lane? + + // X initialization. + if (x_step != 0) { + if (x_step == 1) { + cross_x = (ceil(local_begin.x) - local_begin.x) * delta_x; + } else { + cross_x = (local_begin.x - floor(local_begin.x)) * delta_x; + } + } else { + cross_x = infinite; // Will never cross on X. + } + + // Z initialization. + if (z_step != 0) { + if (z_step == 1) { + cross_z = (ceil(local_begin.z) - local_begin.z) * delta_z; + } else { + cross_z = (local_begin.z - floor(local_begin.z)) * delta_z; + } + } else { + cross_z = infinite; // Will never cross on Z. + } + + int x = floor(local_begin.x); + int z = floor(local_begin.z); + + // Workaround cases where the ray starts at an integer position. + if (Math::abs(cross_x) < CMP_EPSILON) { + cross_x += delta_x; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction. + if (x_step == -1) { + x -= 1; + } + } + + if (Math::abs(cross_z) < CMP_EPSILON) { + cross_z += delta_z; + if (z_step == -1) { + z -= 1; + } + } + + // Start inside the grid. + int x_start = CLAMP(x, 0, width - 2); + int z_start = CLAMP(z, 0, depth - 2); + + // Adjust initial cross values. + cross_x += delta_x * x_step * (x_start - x); + cross_z += delta_z * z_step * (z_start - z); + + x = x_start; + z = z_start; + + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + real_t dist = 0.0; + while (true) { + if (cross_x < cross_z) { + // X lane. + x += x_step; + // Assign before advancing the param, + // to be in sync with the initialization step. + dist = cross_x; + cross_x += delta_x; + } else { + // Z lane. + z += z_step; + dist = cross_z; + cross_z += delta_z; + } + + // Stop when outside the grid. + if ((x < 0) || (z < 0) || (x >= width - 1) || (z >= depth - 1)) { + break; + } + + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + if (dist > ray_dist_proj) { + break; + } + } + } + return false; } @@ -1652,7 +1857,66 @@ Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { return Vector3(); } +void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { + const AABB &aabb = get_aabb(); + + Vector3 pos_local = aabb.position + local_origin; + + Vector3 clamped_point(p_point); + clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); + clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); + clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); + + r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); + r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); + r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); +} + void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { + if (heights.is_empty()) { + return; + } + + AABB local_aabb = p_local_aabb; + local_aabb.position += local_origin; + + // Quantize the aabb, and adjust the start/end ranges. + int aabb_min[3]; + int aabb_max[3]; + _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]); + _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]); + + // Expand the min/max quantized values. + // This is to catch the case where the input aabb falls between grid points. + for (int i = 0; i < 3; ++i) { + aabb_min[i]--; + aabb_max[i]++; + } + + int start_x = MAX(0, aabb_min[0]); + int end_x = MIN(width - 1, aabb_max[0]); + int start_z = MAX(0, aabb_min[2]); + int end_z = MIN(depth - 1, aabb_max[2]); + + FaceShape3DSW face; + face.backface_collision = true; + + for (int z = start_z; z < end_z; z++) { + for (int x = start_x; x < end_x; x++) { + // First triangle. + _get_point(x, z, face.vertex[0]); + _get_point(x + 1, z, face.vertex[1]); + _get_point(x, z + 1, face.vertex[2]); + face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal; + p_callback(p_userdata, &face); + + // Second triangle. + face.vertex[0] = face.vertex[1]; + _get_point(x + 1, z + 1, face.vertex[1]); + face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal; + p_callback(p_userdata, &face); + } + } } Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { @@ -1665,58 +1929,102 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } -void HeightMapShape3DSW::_setup(Vector p_heights, int p_width, int p_depth, real_t p_cell_size) { +void HeightMapShape3DSW::_setup(const Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { heights = p_heights; width = p_width; depth = p_depth; - cell_size = p_cell_size; - - const real_t *r = heights.ptr(); + // Initialize aabb. AABB aabb; + aabb.position = Vector3(0.0, p_min_height, 0.0); + aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); - for (int i = 0; i < depth; i++) { - for (int j = 0; j < width; j++) { - real_t h = r[i * width + j]; + // Initialize origin as the aabb center. + local_origin = aabb.position + 0.5 * aabb.size; + local_origin.y = 0.0; - Vector3 pos(j * cell_size, h, i * cell_size); - if (i == 0 || j == 0) { - aabb.position = pos; - } else { - aabb.expand_to(pos); - } - } - } + aabb.position -= local_origin; configure(aabb); } void HeightMapShape3DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + Dictionary d = p_data; ERR_FAIL_COND(!d.has("width")); ERR_FAIL_COND(!d.has("depth")); - ERR_FAIL_COND(!d.has("cell_size")); ERR_FAIL_COND(!d.has("heights")); int width = d["width"]; int depth = d["depth"]; - real_t cell_size = d["cell_size"]; - Vector heights = d["heights"]; - ERR_FAIL_COND(width <= 0); - ERR_FAIL_COND(depth <= 0); - ERR_FAIL_COND(cell_size <= CMP_EPSILON); - ERR_FAIL_COND(heights.size() != (width * depth)); - _setup(heights, width, depth, cell_size); + ERR_FAIL_COND(width <= 0.0); + ERR_FAIL_COND(depth <= 0.0); + + Variant heights_variant = d["heights"]; + Vector heights_buffer; + if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { + // Ready-to-use heights can be passed. + heights_buffer = heights_variant; + } else if (heights_variant.get_type() == Variant::OBJECT) { + // If an image is passed, we have to convert it. + // This would be expensive to do with a script, so it's nice to have it here. + Ref image = heights_variant; + ERR_FAIL_COND(image.is_null()); + ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF); + + PackedByteArray im_data = image->get_data(); + heights_buffer.resize(image->get_width() * image->get_height()); + + float *w = heights_buffer.ptrw(); + float *rp = (float *)im_data.ptr(); + for (int i = 0; i < heights_buffer.size(); ++i) { + w[i] = rp[i]; + } + } else { + ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); + } + + // Compute min and max heights or use precomputed values. + real_t min_height = 0.0; + real_t max_height = 0.0; + if (d.has("min_height") && d.has("max_height")) { + min_height = d["min_height"]; + max_height = d["max_height"]; + } else { + int heights_size = heights.size(); + for (int i = 0; i < heights_size; ++i) { + float h = heights[i]; + if (h < min_height) { + min_height = h; + } else if (h > max_height) { + max_height = h; + } + } + } + + ERR_FAIL_COND(min_height > max_height); + + ERR_FAIL_COND(heights_buffer.size() != (width * depth)); + + // If specified, min and max height will be used as precomputed values. + _setup(heights_buffer, width, depth, min_height, max_height); } Variant HeightMapShape3DSW::get_data() const { - ERR_FAIL_V(Variant()); + Dictionary d; + d["width"] = width; + d["depth"] = depth; + + const AABB &aabb = get_aabb(); + d["min_height"] = aabb.position.y; + d["max_height"] = aabb.position.y + aabb.size.y; + + d["heights"] = heights; + + return d; } HeightMapShape3DSW::HeightMapShape3DSW() { - width = 0; - depth = 0; - cell_size = 0; } diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 988e76c699..4d2b6ffbed 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -81,7 +81,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const = 0; - _FORCE_INLINE_ AABB get_aabb() const { return aabb; } + _FORCE_INLINE_ const AABB &get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } virtual bool is_concave() const { return false; } @@ -389,21 +389,29 @@ public: }; struct HeightMapShape3DSW : public ConcaveShape3DSW { - Vector heights; - int width; - int depth; - real_t cell_size; + Vector heights; + int width = 0; + int depth = 0; + Vector3 local_origin; - //void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; - //void _cull(int p_idx,_CullParams *p_params) const; + _FORCE_INLINE_ float _get_height(int p_x, int p_z) const { + return heights[(p_z * width) + p_x]; + } - void _setup(Vector p_heights, int p_width, int p_depth, real_t p_cell_size); + _FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const { + r_point.x = p_x - 0.5 * (width - 1.0); + r_point.y = _get_height(p_x, p_z); + r_point.z = p_z - 0.5 * (depth - 1.0); + } + + void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; + + void _setup(const Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); public: - Vector get_heights() const; + Vector get_heights() const; int get_width() const; int get_depth() const; - real_t get_cell_size() const; virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; }