diff options
Diffstat (limited to 'servers/physics_3d/godot_shape_3d.cpp')
| -rw-r--r-- | servers/physics_3d/godot_shape_3d.cpp | 2204 |
1 files changed, 2204 insertions, 0 deletions
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp new file mode 100644 index 0000000000..5364a9833d --- /dev/null +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -0,0 +1,2204 @@ +/*************************************************************************/ +/* godot_shape_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_shape_3d.h" + +#include "core/io/image.h" +#include "core/math/convex_hull.h" +#include "core/math/geometry_3d.h" +#include "core/templates/sort_array.h" + +// GodotHeightMapShape3D 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 + +#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 +#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 + +void GodotShape3D::configure(const AABB &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue<GodotShapeOwner3D *, int> &E : owners) { + GodotShapeOwner3D *co = (GodotShapeOwner3D *)E.key; + co->_shape_changed(); + } +} + +Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const { + Vector3 res; + int amnt; + FeatureType type; + get_supports(p_normal, 1, &res, amnt, type); + return res; +} + +void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) { + Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) { + Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const { + return owners.has(p_owner); +} + +const Map<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { + return owners; +} + +GodotShape3D::~GodotShape3D() { + ERR_FAIL_COND(owners.size()); +} + +Plane GodotWorldBoundaryShape3D::get_plane() const { + return plane; +} + +void GodotWorldBoundaryShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // gibberish, a plane is infinity + r_min = -1e7; + r_max = 1e7; +} + +Vector3 GodotWorldBoundaryShape3D::get_support(const Vector3 &p_normal) const { + return p_normal * 1e15; +} + +bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + bool inters = plane.intersects_segment(p_begin, p_end, &r_result); + if (inters) { + r_normal = plane.normal; + } + return inters; +} + +bool GodotWorldBoundaryShape3D::intersect_point(const Vector3 &p_point) const { + return plane.distance_to(p_point) < 0; +} + +Vector3 GodotWorldBoundaryShape3D::get_closest_point_to(const Vector3 &p_point) const { + if (plane.is_point_over(p_point)) { + return plane.project(p_point); + } else { + return p_point; + } +} + +Vector3 GodotWorldBoundaryShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // not applicable. +} + +void GodotWorldBoundaryShape3D::_setup(const Plane &p_plane) { + plane = p_plane; + configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); +} + +void GodotWorldBoundaryShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotWorldBoundaryShape3D::get_data() const { + return plane; +} + +GodotWorldBoundaryShape3D::GodotWorldBoundaryShape3D() { +} + +// + +real_t GodotSeparationRayShape3D::get_length() const { + return length; +} + +bool GodotSeparationRayShape3D::get_slide_on_slope() const { + return slide_on_slope; +} + +void GodotSeparationRayShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // don't think this will be even used + r_min = 0; + r_max = 1; +} + +Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const { + if (p_normal.z > 0) { + return Vector3(0, 0, length); + } else { + return Vector3(0, 0, 0); + } +} + +void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, length); + } else { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, 0); + } +} + +bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + return false; //simply not possible +} + +bool GodotSeparationRayShape3D::intersect_point(const Vector3 &p_point) const { + return false; //simply not possible +} + +Vector3 GodotSeparationRayShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry3D::get_closest_point_to_segment(p_point, s); +} + +Vector3 GodotSeparationRayShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); +} + +void GodotSeparationRayShape3D::_setup(real_t p_length, bool p_slide_on_slope) { + length = p_length; + slide_on_slope = p_slide_on_slope; + configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); +} + +void GodotSeparationRayShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + _setup(d["length"], d["slide_on_slope"]); +} + +Variant GodotSeparationRayShape3D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +GodotSeparationRayShape3D::GodotSeparationRayShape3D() {} + +/********** SPHERE *************/ + +real_t GodotSphereShape3D::get_radius() const { + return radius; +} + +void GodotSphereShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + real_t d = p_normal.dot(p_transform.origin); + + // figure out scale at point + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; +} + +Vector3 GodotSphereShape3D::get_support(const Vector3 &p_normal) const { + return p_normal * radius; +} + +void GodotSphereShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + *r_supports = p_normal * radius; + r_amount = 1; + r_type = FEATURE_POINT; +} + +bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); +} + +bool GodotSphereShape3D::intersect_point(const Vector3 &p_point) const { + return p_point.length() < radius; +} + +Vector3 GodotSphereShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 p = p_point; + real_t l = p.length(); + if (l < radius) { + return p_point; + } + return (p / l) * radius; +} + +Vector3 GodotSphereShape3D::get_moment_of_inertia(real_t p_mass) const { + real_t s = 0.4 * p_mass * radius * radius; + return Vector3(s, s, s); +} + +void GodotSphereShape3D::_setup(real_t p_radius) { + radius = p_radius; + configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); +} + +void GodotSphereShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotSphereShape3D::get_data() const { + return radius; +} + +GodotSphereShape3D::GodotSphereShape3D() {} + +/********** BOX *************/ + +void GodotBoxShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + + real_t length = local_normal.abs().dot(half_extents); + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 GodotBoxShape3D::get_support(const Vector3 &p_normal) const { + Vector3 point( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y, + (p_normal.z < 0) ? -half_extents.z : half_extents.z); + + return point; +} + +void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + static const int next[3] = { 1, 2, 0 }; + static const int next2[3] = { 2, 0, 1 }; + + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + real_t dot = p_normal.dot(axis); + if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + //Vector3 axis_b; + + bool neg = dot < 0; + r_amount = 4; + r_type = FEATURE_FACE; + + Vector3 point; + point[i] = half_extents[i]; + + int i_n = next[i]; + int i_n2 = next2[i]; + + static const real_t sign[4][2] = { + { -1.0, 1.0 }, + { 1.0, 1.0 }, + { 1.0, -1.0 }, + { -1.0, -1.0 }, + }; + + for (int j = 0; j < 4; j++) { + point[i_n] = sign[j][0] * half_extents[i_n]; + point[i_n2] = sign[j][1] * half_extents[i_n2]; + r_supports[j] = neg ? -point : point; + } + + if (neg) { + SWAP(r_supports[1], r_supports[2]); + SWAP(r_supports[0], r_supports[3]); + } + + return; + } + + r_amount = 0; + } + + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + + if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + + int i_n = next[i]; + int i_n2 = next2[i]; + + Vector3 point = half_extents; + + if (p_normal[i_n] < 0) { + point[i_n] = -point[i_n]; + } + if (p_normal[i_n2] < 0) { + point[i_n2] = -point[i_n2]; + } + + r_supports[0] = point; + point[i] = -point[i]; + r_supports[1] = point; + return; + } + } + /* USE POINT */ + + Vector3 point( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y, + (p_normal.z < 0) ? -half_extents.z : half_extents.z); + + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = point; +} + +bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + AABB aabb(-half_extents, half_extents * 2.0); + + return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); +} + +bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const { + return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); +} + +Vector3 GodotBoxShape3D::get_closest_point_to(const Vector3 &p_point) const { + int outside = 0; + Vector3 min_point; + + for (int i = 0; i < 3; i++) { + if (Math::abs(p_point[i]) > half_extents[i]) { + outside++; + if (outside == 1) { + //use plane if only one side matches + Vector3 n; + n[i] = SIGN(p_point[i]); + + Plane p(n, half_extents[i]); + min_point = p.project(p_point); + } + } + } + + if (!outside) { + return p_point; //it's inside, don't do anything else + } + + if (outside == 1) { //if only above one plane, this plane clearly wins + return min_point; + } + + //check segments + real_t min_distance = 1e20; + Vector3 closest_vertex = half_extents * p_point.sign(); + Vector3 s[2] = { + closest_vertex, + closest_vertex + }; + + for (int i = 0; i < 3; i++) { + s[1] = closest_vertex; + s[1][i] = -s[1][i]; //edge + + Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s); + + real_t d = p_point.distance_to(closest_edge); + if (d < min_distance) { + min_point = closest_edge; + min_distance = d; + } + } + + return min_point; +} + +Vector3 GodotBoxShape3D::get_moment_of_inertia(real_t p_mass) const { + real_t lx = half_extents.x; + real_t ly = half_extents.y; + real_t lz = half_extents.z; + + return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly)); +} + +void GodotBoxShape3D::_setup(const Vector3 &p_half_extents) { + half_extents = p_half_extents.abs(); + + configure(AABB(-half_extents, half_extents * 2)); +} + +void GodotBoxShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotBoxShape3D::get_data() const { + return half_extents; +} + +GodotBoxShape3D::GodotBoxShape3D() {} + +/********** CAPSULE *************/ + +void GodotCapsuleShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); +} + +Vector3 GodotCapsuleShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + return n; +} + +void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + Vector3 n = p_normal; + + real_t d = n.y; + + if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5 - radius; + r_supports[1] = n; + r_supports[1].y -= height * 0.5 - radius; + + } else { + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (d > 0) ? h : -h; + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = n; + } +} + +bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + Vector3 norm = (p_end - p_begin).normalized(); + real_t min_d = 1e20; + + Vector3 res, n; + bool collision = false; + + Vector3 auxres, auxn; + bool collided; + + // test against cylinder and spheres :-| + + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1); + + if (collided) { + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn); + + if (collided) { + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn); + + if (collided) { + real_t d = norm.dot(auxres); + + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + if (collision) { + r_result = res; + r_normal = n; + } + return collision; +} + +bool GodotCapsuleShape3D::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5 - radius) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } else { + Vector3 p = p_point; + p.y = Math::abs(p.y) - height * 0.5 + radius; + return p.length() < radius; + } +} + +Vector3 GodotCapsuleShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, -height * 0.5 + radius, 0), + Vector3(0, height * 0.5 - radius, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; +} + +Vector3 GodotCapsuleShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotCapsuleShape3D::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2))); +} + +void GodotCapsuleShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant GodotCapsuleShape3D::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +GodotCapsuleShape3D::GodotCapsuleShape3D() {} + +/********** CYLINDER *************/ + +void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); + real_t axis_dot = cylinder_axis.dot(p_normal); + + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + real_t scaled_radius = radius * scale; + real_t scaled_height = height * scale; + + real_t length; + if (Math::abs(axis_dot) > 1.0) { + length = scaled_height * 0.5; + } else { + length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); + } + + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + real_t h = (n.y > 0) ? height : -height; + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = radius; + n.y = h * 0.5; + n.z = 0.0; + } else { + real_t d = radius / s; + n.x = n.x * d; + n.y = h * 0.5; + n.z = n.z * d; + } + + return n; +} + +void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + real_t d = p_normal.y; + if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { + real_t h = (d > 0) ? height : -height; + + Vector3 n = p_normal; + n.x = 0.0; + n.z = 0.0; + n.y = h * 0.5; + + r_amount = 3; + r_type = FEATURE_CIRCLE; + r_supports[0] = n; + r_supports[1] = n; + r_supports[1].x += radius; + r_supports[2] = n; + r_supports[2].z += radius; + } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + Vector3 n = p_normal; + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5; + r_supports[1] = n; + r_supports[1].y -= height * 0.5; + } else { + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = get_support(p_normal); + return; + + Vector3 n = p_normal; + real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); + if (Math::abs(h) > 1.0) { + // Top or bottom surface. + n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; + } else { + // Lateral surface. + n.y = height * 0.5 * h; + } + + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = 0.0; + n.z = 0.0; + } else { + real_t scaled_radius = radius / s; + n.x = n.x * scaled_radius; + n.z = n.z * scaled_radius; + } + + r_supports[0] = n; + } +} + +bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); +} + +bool GodotCylinderShape3D::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } + return false; +} + +Vector3 GodotCylinderShape3D::get_closest_point_to(const Vector3 &p_point) const { + if (Math::absf(p_point.y) > height * 0.5) { + // Project point to top disk. + real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; + Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); + Plane circle_plane(Vector3(0.0, dir, 0.0), circle_pos); + Vector3 proj_point = circle_plane.project(p_point); + + // Clip position. + Vector3 delta_point_1 = proj_point - circle_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; + } + + return proj_point; + } else { + Vector3 s[2] = { + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; + } +} + +Vector3 GodotCylinderShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotCylinderShape3D::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); +} + +void GodotCylinderShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant GodotCylinderShape3D::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +GodotCylinderShape3D::GodotCylinderShape3D() {} + +/********** CONVEX POLYGON *************/ + +void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) { + return; + } + + const Vector3 *vrts = &mesh.vertices[0]; + + for (int i = 0; i < vertex_count; i++) { + real_t d = p_normal.dot(p_transform.xform(vrts[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + + int vert_support_idx = -1; + real_t support_max = 0; + + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) { + return Vector3(); + } + + const Vector3 *vrts = &mesh.vertices[0]; + + for (int i = 0; i < vertex_count; i++) { + real_t d = n.dot(vrts[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vrts[vert_support_idx]; +} + +void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + int vc = mesh.vertices.size(); + + r_amount = 0; + ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices."); + + //find vertex first + real_t max = 0; + int vtx = 0; + + for (int i = 0; i < vc; i++) { + real_t d = p_normal.dot(vertices[i]); + + if (i == 0 || d > max) { + max = d; + vtx = i; + } + } + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + int ic = faces[i].indices.size(); + const int *ind = faces[i].indices.ptr(); + + bool valid = false; + for (int j = 0; j < ic; j++) { + if (ind[j] == vtx) { + valid = true; + break; + } + } + + if (!valid) { + continue; + } + + int m = MIN(p_max, ic); + for (int j = 0; j < m; j++) { + r_supports[j] = vertices[ind[j]]; + } + r_amount = m; + r_type = FEATURE_FACE; + return; + } + } + + for (int i = 0; i < ec; i++) { + real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); + dot = ABS(dot); + if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = vertices[edges[i].a]; + r_supports[1] = vertices[edges[i].b]; + return; + } + } + + r_supports[0] = vertices[vtx]; + r_amount = 1; + r_type = FEATURE_POINT; +} + +bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + + Vector3 n = p_end - p_begin; + real_t min = 1e20; + bool col = false; + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.normal.dot(n) > 0) { + continue; //opposing face + } + + int ic = faces[i].indices.size(); + const int *ind = faces[i].indices.ptr(); + + for (int j = 1; j < ic - 1; j++) { + Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); + Vector3 result; + if (f.intersects_segment(p_begin, p_end, &result)) { + real_t d = n.dot(result); + if (d < min) { + min = d; + r_result = result; + r_normal = faces[i].plane.normal; + col = true; + } + + break; + } + } + } + + return col; +} + +bool GodotConvexPolygonShape3D::intersect_point(const Vector3 &p_point) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.distance_to(p_point) >= 0) { + return false; + } + } + + return true; +} + +Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + bool all_inside = true; + for (int i = 0; i < fc; i++) { + if (!faces[i].plane.is_point_over(p_point)) { + continue; + } + + all_inside = false; + bool is_inside = true; + int ic = faces[i].indices.size(); + const int *indices = faces[i].indices.ptr(); + + for (int j = 0; j < ic; j++) { + Vector3 a = vertices[indices[j]]; + Vector3 b = vertices[indices[(j + 1) % ic]]; + Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); + if (Plane(n, a).is_point_over(p_point)) { + is_inside = false; + break; + } + } + + if (is_inside) { + return faces[i].plane.project(p_point); + } + } + + if (all_inside) { + return p_point; + } + + real_t min_distance = 1e20; + Vector3 min_point; + + //check edges + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + for (int i = 0; i < ec; i++) { + Vector3 s[2] = { + vertices[edges[i].a], + vertices[edges[i].b] + }; + + Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); + real_t d = closest.distance_to(p_point); + if (d < min_distance) { + min_distance = d; + min_point = closest; + } + } + + return min_point; +} + +Vector3 GodotConvexPolygonShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { + Error err = ConvexHullComputer::convex_hull(p_vertices, mesh); + if (err != OK) { + ERR_PRINT("Failed to build convex hull"); + } + + AABB _aabb; + + for (int i = 0; i < mesh.vertices.size(); i++) { + if (i == 0) { + _aabb.position = mesh.vertices[i]; + } else { + _aabb.expand_to(mesh.vertices[i]); + } + } + + configure(_aabb); +} + +void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotConvexPolygonShape3D::get_data() const { + return mesh.vertices; +} + +GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() { +} + +/********** FACE POLYGON *************/ + +void GodotFaceShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + for (int i = 0; i < 3; i++) { + Vector3 v = p_transform.xform(vertex[i]); + real_t d = p_normal.dot(v); + + if (i == 0 || d > r_max) { + r_max = d; + } + + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotFaceShape3D::get_support(const Vector3 &p_normal) const { + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < 3; i++) { + real_t d = p_normal.dot(vertex[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vertex[vert_support_idx]; +} + +void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + Vector3 n = p_normal; + + /** TEST FACE AS SUPPORT **/ + if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 3; + r_type = FEATURE_FACE; + for (int i = 0; i < 3; i++) { + r_supports[i] = vertex[i]; + } + return; + } + + /** FIND SUPPORT VERTEX **/ + + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < 3; i++) { + real_t d = n.dot(vertex[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + /** TEST EDGES AS SUPPORT **/ + + for (int i = 0; i < 3; i++) { + int nx = (i + 1) % 3; + if (i != vert_support_idx && nx != vert_support_idx) { + continue; + } + + // check if edge is valid as a support + real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); + dot = ABS(dot); + if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = vertex[i]; + r_supports[1] = vertex[nx]; + return; + } + } + + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = vertex[vert_support_idx]; +} + +bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); + if (c) { + r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; + if (r_normal.dot(p_end - p_begin) > 0) { + if (backface_collision && p_hit_back_faces) { + r_normal = -r_normal; + } else { + c = false; + } + } + } + + return c; +} + +bool GodotFaceShape3D::intersect_point(const Vector3 &p_point) const { + return false; //face is flat +} + +Vector3 GodotFaceShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + +Vector3 GodotFaceShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! +} + +GodotFaceShape3D::GodotFaceShape3D() { + configure(AABB()); +} + +Vector<Vector3> GodotConcavePolygonShape3D::get_faces() const { + Vector<Vector3> rfaces; + rfaces.resize(faces.size() * 3); + + for (int i = 0; i < faces.size(); i++) { + Face f = faces.get(i); + + for (int j = 0; j < 3; j++) { + rfaces.set(i * 3 + j, vertices.get(f.indices[j])); + } + } + + return rfaces; +} + +void GodotConcavePolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + int count = vertices.size(); + if (count == 0) { + r_min = 0; + r_max = 0; + return; + } + const Vector3 *vptr = vertices.ptr(); + + for (int i = 0; i < count; i++) { + real_t d = p_normal.dot(p_transform.xform(vptr[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const { + int count = vertices.size(); + if (count == 0) { + return Vector3(); + } + + const Vector3 *vptr = vertices.ptr(); + + Vector3 n = p_normal; + + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < count; i++) { + real_t d = n.dot(vptr[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vptr[vert_support_idx]; +} + +void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { + const BVH *bvh = &p_params->bvh[p_idx]; + + /* + if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) + return; //test against whole AABB, which isn't very costly + */ + + //printf("addr: %p\n",bvh); + if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { + return; + } + + if (bvh->face_index >= 0) { + const Face *f = &p_params->faces[bvh->face_index]; + GodotFaceShape3D *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + + Vector3 res; + Vector3 normal; + if (face->intersect_segment(p_params->from, p_params->to, res, normal, true)) { + real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); + if ((d > 0) && (d < p_params->min_d)) { + p_params->min_d = d; + p_params->result = res; + p_params->normal = normal; + p_params->collisions++; + } + } + } else { + if (bvh->left >= 0) { + _cull_segment(bvh->left, p_params); + } + if (bvh->right >= 0) { + _cull_segment(bvh->right, p_params); + } + } +} + +bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { + if (faces.size() == 0) { + return false; + } + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + GodotFaceShape3D face; + face.backface_collision = backface_collision && p_hit_back_faces; + + _SegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + + params.faces = fr; + params.vertices = vr; + params.bvh = br; + + params.face = &face; + + // cull + _cull_segment(0, ¶ms); + + if (params.collisions > 0) { + r_result = params.result; + r_normal = params.normal; + return true; + } else { + return false; + } +} + +bool GodotConcavePolygonShape3D::intersect_point(const Vector3 &p_point) const { + return false; //face is flat +} + +Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Vector3(); +} + +bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { + const BVH *bvh = &p_params->bvh[p_idx]; + + if (!p_params->aabb.intersects(bvh->aabb)) { + return false; + } + + if (bvh->face_index >= 0) { + const Face *f = &p_params->faces[bvh->face_index]; + GodotFaceShape3D *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + if (p_params->callback(p_params->userdata, face)) { + return true; + } + } else { + if (bvh->left >= 0) { + if (_cull(bvh->left, p_params)) { + return true; + } + } + + if (bvh->right >= 0) { + if (_cull(bvh->right, p_params)) { + return true; + } + } + } + + return false; +} + +void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { + // make matrix local to concave + if (faces.size() == 0) { + return; + } + + AABB local_aabb = p_local_aabb; + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + GodotFaceShape3D face; // use this to send in the callback + face.backface_collision = backface_collision; + face.invert_backface_collision = p_invert_backface_collision; + + _CullParams params; + params.aabb = local_aabb; + params.face = &face; + params.faces = fr; + params.vertices = vr; + params.bvh = br; + params.callback = p_callback; + params.userdata = p_userdata; + + // cull + _cull(0, ¶ms); +} + +Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +struct _Volume_BVH_Element { + AABB aabb; + Vector3 center; + int face_index; +}; + +struct _Volume_BVH_CompareX { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.x < b.center.x; + } +}; + +struct _Volume_BVH_CompareY { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.y < b.center.y; + } +}; + +struct _Volume_BVH_CompareZ { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.z < b.center.z; + } +}; + +struct _Volume_BVH { + AABB aabb; + _Volume_BVH *left; + _Volume_BVH *right; + + int face_index; +}; + +_Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) { + _Volume_BVH *bvh = memnew(_Volume_BVH); + + if (p_size == 1) { + //leaf + bvh->aabb = p_elements[0].aabb; + bvh->left = nullptr; + bvh->right = nullptr; + bvh->face_index = p_elements->face_index; + count++; + return bvh; + } else { + bvh->face_index = -1; + } + + AABB aabb; + for (int i = 0; i < p_size; i++) { + if (i == 0) { + aabb = p_elements[i].aabb; + } else { + aabb.merge_with(p_elements[i].aabb); + } + } + bvh->aabb = aabb; + switch (aabb.get_longest_axis_index()) { + case 0: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareX> sort_x; + sort_x.sort(p_elements, p_size); + + } break; + case 1: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareY> sort_y; + sort_y.sort(p_elements, p_size); + } break; + case 2: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareZ> sort_z; + sort_z.sort(p_elements, p_size); + } break; + } + + int split = p_size / 2; + bvh->left = _volume_build_bvh(p_elements, split, count); + bvh->right = _volume_build_bvh(&p_elements[split], p_size - split, count); + + //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); + count++; + return bvh; +} + +void GodotConcavePolygonShape3D::_fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { + int idx = p_idx; + + p_bvh_array[idx].aabb = p_bvh_tree->aabb; + p_bvh_array[idx].face_index = p_bvh_tree->face_index; + //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); + + if (p_bvh_tree->left) { + p_bvh_array[idx].left = ++p_idx; + _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); + + } else { + p_bvh_array[p_idx].left = -1; + } + + if (p_bvh_tree->right) { + p_bvh_array[idx].right = ++p_idx; + _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); + + } else { + p_bvh_array[p_idx].right = -1; + } + + memdelete(p_bvh_tree); +} + +void GodotConcavePolygonShape3D::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) { + int src_face_count = p_faces.size(); + if (src_face_count == 0) { + configure(AABB()); + return; + } + ERR_FAIL_COND(src_face_count % 3); + src_face_count /= 3; + + const Vector3 *facesr = p_faces.ptr(); + + Vector<_Volume_BVH_Element> bvh_array; + bvh_array.resize(src_face_count); + + _Volume_BVH_Element *bvh_arrayw = bvh_array.ptrw(); + + faces.resize(src_face_count); + Face *facesw = faces.ptrw(); + + vertices.resize(src_face_count * 3); + + Vector3 *verticesw = vertices.ptrw(); + + AABB _aabb; + + for (int i = 0; i < src_face_count; i++) { + Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); + + bvh_arrayw[i].aabb = face.get_aabb(); + bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center(); + bvh_arrayw[i].face_index = i; + facesw[i].indices[0] = i * 3 + 0; + facesw[i].indices[1] = i * 3 + 1; + facesw[i].indices[2] = i * 3 + 2; + facesw[i].normal = face.get_plane().normal; + verticesw[i * 3 + 0] = face.vertex[0]; + verticesw[i * 3 + 1] = face.vertex[1]; + verticesw[i * 3 + 2] = face.vertex[2]; + if (i == 0) { + _aabb = bvh_arrayw[i].aabb; + } else { + _aabb.merge_with(bvh_arrayw[i].aabb); + } + } + + int count = 0; + _Volume_BVH *bvh_tree = _volume_build_bvh(bvh_arrayw, src_face_count, count); + + bvh.resize(count + 1); + + BVH *bvh_arrayw2 = bvh.ptrw(); + + int idx = 0; + _fill_bvh(bvh_tree, bvh_arrayw2, idx); + + backface_collision = p_backface_collision; + + configure(_aabb); // this type of shape has no margin +} + +void GodotConcavePolygonShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("faces")); + + _setup(d["faces"], d["backface_collision"]); +} + +Variant GodotConcavePolygonShape3D::get_data() const { + Dictionary d; + d["faces"] = get_faces(); + d["backface_collision"] = backface_collision; + + return d; +} + +GodotConcavePolygonShape3D::GodotConcavePolygonShape3D() { +} + +/* HEIGHT MAP SHAPE */ + +Vector<real_t> GodotHeightMapShape3D::get_heights() const { + return heights; +} + +int GodotHeightMapShape3D::get_width() const { + return width; +} + +int GodotHeightMapShape3D::get_depth() const { + return depth; +} + +void GodotHeightMapShape3D::project_range(const Vector3 &p_normal, const Transform3D &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), r_min, r_max); +} + +Vector3 GodotHeightMapShape3D::get_support(const Vector3 &p_normal) const { + //not very useful, but not very used either + return get_aabb().get_support(p_normal); +} + +struct _HeightmapSegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + + Vector3 result; + Vector3 normal; + + const GodotHeightMapShape3D *heightmap = nullptr; + GodotFaceShape3D *face = nullptr; +}; + +struct _HeightmapGridCullState { + real_t length = 0.0; + real_t length_flat = 0.0; + + real_t dist = 0.0; + real_t prev_dist = 0.0; + + int x = 0; + int z = 0; +}; + +_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, true)) { + p_params.result = res; + p_params.normal = normal; + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { + // First triangle. + p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]); + p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]); + p_params.heightmap->_get_point(p_state.x, p_state.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_state.x + 1, p_state.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; +} + +_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { + const GodotHeightMapShape3D::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z); + + Vector3 enter_pos; + Vector3 exit_pos; + + if (p_state.length_flat > CMP_EPSILON) { + real_t flat_to_3d = p_state.length / p_state.length_flat; + real_t enter_param = p_state.prev_dist * flat_to_3d; + real_t exit_param = p_state.dist * flat_to_3d; + enter_pos = p_params.from + p_params.dir * enter_param; + exit_pos = p_params.from + p_params.dir * exit_param; + } else { + // Consider the ray vertical. + // (though we shouldn't reach this often because there is an early check up-front) + enter_pos = p_params.from; + exit_pos = p_params.to; + } + + // Transform positions to heightmap space. + enter_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE; + exit_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis. + if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) { + return false; + } + if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) { + return false; + } + + return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal); +} + +template <typename ProcessFunction> +bool GodotHeightMapShape3D::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const { + Vector3 delta = (p_end - p_begin); + real_t length = delta.length(); + + if (length < CMP_EPSILON) { + return false; + } + + Vector3 local_begin = p_begin + offset; + + GodotFaceShape3D face; + face.backface_collision = false; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = delta / length; + params.heightmap = this; + params.face = &face; + + _HeightmapGridCullState state; + + // Perform grid query from projected ray. + Vector2 ray_dir_flat(delta.x, delta.z); + state.length = length; + state.length_flat = ray_dir_flat.length(); + + if (state.length_flat < CMP_EPSILON) { + ray_dir_flat = Vector2(); + } else { + ray_dir_flat /= state.length_flat; + } + + const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0); + const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0); + + const real_t infinite = 1e20; + const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite; + const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.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 = (Math::ceil(local_begin.x) - local_begin.x) * delta_x; + } else { + cross_x = (local_begin.x - Math::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 = (Math::ceil(local_begin.z) - local_begin.z) * delta_z; + } else { + cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z; + } + } else { + cross_z = infinite; // Will never cross on Z. + } + + int x = Math::floor(local_begin.x); + int z = Math::floor(local_begin.z); + + // Workaround cases where the ray starts at an integer position. + if (Math::is_zero_approx(cross_x)) { + 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::is_zero_approx(cross_z)) { + cross_z += delta_z; + if (z_step == -1) { + z -= 1; + } + } + + // Start inside the grid. + int x_start = MAX(MIN(x, p_width - 2), 0); + int z_start = MAX(MIN(z, p_depth - 2), 0); + + // 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; + + while (true) { + state.prev_dist = state.dist; + state.x = x; + state.z = z; + + if (cross_x < cross_z) { + // X lane. + x += x_step; + // Assign before advancing the param, + // to be in sync with the initialization step. + state.dist = cross_x; + cross_x += delta_x; + } else { + // Z lane. + z += z_step; + state.dist = cross_z; + cross_z += delta_z; + } + + if (state.dist > state.length_flat) { + state.dist = state.length_flat; + if (p_process(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + break; + } + + if (p_process(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + // Stop when outside the grid. + if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) { + break; + } + } + + return false; +} + +bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const { + if (heights.is_empty()) { + return false; + } + + Vector3 local_begin = p_begin + local_origin; + Vector3 local_end = p_end + local_origin; + + // Quantize the ray begin/end. + int begin_x = Math::floor(local_begin.x); + int begin_z = Math::floor(local_begin.z); + int end_x = Math::floor(local_end.x); + int end_z = Math::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. + GodotFaceShape3D face; + face.backface_collision = p_hit_back_faces; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + + params.heightmap = this; + params.face = &face; + + _HeightmapGridCullState state; + state.x = MAX(MIN(begin_x, width - 2), 0); + state.z = MAX(MIN(begin_z, depth - 2), 0); + if (_heightmap_cell_cull_segment(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + } else if (bounds_grid.is_empty()) { + // Process all cells intersecting the flat projection of the ray. + return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); + } else { + Vector3 ray_diff = (p_end - p_begin); + real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z; + if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) { + // Don't use chunks, the ray is too short in the plane. + return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); + } else { + // The ray is long, run raycast on a higher-level grid. + Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE; + Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE; + Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE; + return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal); + } + } + + return false; +} + +bool GodotHeightMapShape3D::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Vector3(); +} + +void GodotHeightMapShape3D::_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 GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) 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]); + + GodotFaceShape3D face; + face.backface_collision = !p_invert_backface_collision; + face.invert_backface_collision = p_invert_backface_collision; + + 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[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } + + // 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[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } + } + } +} + +Vector3 GodotHeightMapShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotHeightMapShape3D::_build_accelerator() { + bounds_grid.clear(); + + bounds_grid_width = width / BOUNDS_CHUNK_SIZE; + bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE; + + if (width % BOUNDS_CHUNK_SIZE > 0) { + ++bounds_grid_width; // In case terrain size isn't dividable by chunk size. + } + + if (depth % BOUNDS_CHUNK_SIZE > 0) { + ++bounds_grid_depth; + } + + uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth); + + if (bound_grid_size < 2) { + // Grid is empty or just one chunk. + return; + } + + bounds_grid.resize(bound_grid_size); + + // Compute min and max height for all chunks. + for (int cz = 0; cz < bounds_grid_depth; ++cz) { + int z0 = cz * BOUNDS_CHUNK_SIZE; + + for (int cx = 0; cx < bounds_grid_width; ++cx) { + int x0 = cx * BOUNDS_CHUNK_SIZE; + + Range r; + + r.min = _get_height(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth); + int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width); + for (int z = z0; z < z_max; ++z) { + for (int x = x0; x < x_max; ++x) { + real_t height = _get_height(x, z); + if (height < r.min) { + r.min = height; + } else if (height > r.max) { + r.max = height; + } + } + } + + bounds_grid[cx + cz * bounds_grid_width] = r; + } + } +} + +void GodotHeightMapShape3D::_setup(const Vector<real_t> &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; + + // 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); + + // Initialize origin as the aabb center. + local_origin = aabb.position + 0.5 * aabb.size; + local_origin.y = 0.0; + + aabb.position -= local_origin; + + _build_accelerator(); + + configure(aabb); +} + +void GodotHeightMapShape3D::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("heights")); + + int width = d["width"]; + int depth = d["depth"]; + + ERR_FAIL_COND(width <= 0.0); + ERR_FAIL_COND(depth <= 0.0); + + Variant heights_variant = d["heights"]; + Vector<real_t> heights_buffer; +#ifdef REAL_T_IS_DOUBLE + if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) { +#else + if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { +#endif + // 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> 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()); + + real_t *w = heights_buffer.ptrw(); + real_t *rp = (real_t *)im_data.ptr(); + for (int i = 0; i < heights_buffer.size(); ++i) { + w[i] = rp[i]; + } + } else { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_MSG("Expected PackedFloat64Array or float Image."); +#else + ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); +#endif + } + + // 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) { + real_t 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 GodotHeightMapShape3D::get_data() const { + 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; +} + +GodotHeightMapShape3D::GodotHeightMapShape3D() { +} |