diff options
Diffstat (limited to 'servers/physics_3d/shape_3d_sw.cpp')
| -rw-r--r-- | servers/physics_3d/shape_3d_sw.cpp | 1655 |
1 files changed, 1655 insertions, 0 deletions
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp new file mode 100644 index 0000000000..61c32b779a --- /dev/null +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -0,0 +1,1655 @@ +/*************************************************************************/ +/* shape_3d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 "shape_3d_sw.h" + +#include "core/math/geometry.h" +#include "core/math/quick_hull.h" +#include "core/sort_array.h" + +#define _POINT_SNAP 0.001953125 +#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 +#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 + +void Shape3DSW::configure(const AABB &p_aabb) { + aabb = p_aabb; + configured = true; + for (Map<ShapeOwner3DSW *, int>::Element *E = owners.front(); E; E = E->next()) { + ShapeOwner3DSW *co = (ShapeOwner3DSW *)E->key(); + co->_shape_changed(); + } +} + +Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { + + Vector3 res; + int amnt; + get_supports(p_normal, 1, &res, amnt); + return res; +} + +void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) { + + Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) { + + Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const { + + return owners.has(p_owner); +} + +const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const { + return owners; +} + +Shape3DSW::Shape3DSW() { + + custom_bias = 0; + configured = false; +} + +Shape3DSW::~Shape3DSW() { + + ERR_FAIL_COND(owners.size()); +} + +Plane PlaneShape3DSW::get_plane() const { + + return plane; +} + +void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + + // gibberish, a plane is infinity + r_min = -1e7; + r_max = 1e7; +} + +Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const { + + return p_normal * 1e15; +} + +bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + bool inters = plane.intersects_segment(p_begin, p_end, &r_result); + if (inters) + r_normal = plane.normal; + return inters; +} + +bool PlaneShape3DSW::intersect_point(const Vector3 &p_point) const { + + return plane.distance_to(p_point) < 0; +} + +Vector3 PlaneShape3DSW::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 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + return Vector3(); //wtf +} + +void PlaneShape3DSW::_setup(const Plane &p_plane) { + + plane = p_plane; + configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); +} + +void PlaneShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant PlaneShape3DSW::get_data() const { + + return plane; +} + +PlaneShape3DSW::PlaneShape3DSW() { +} + +// + +real_t RayShape3DSW::get_length() const { + + return length; +} + +bool RayShape3DSW::get_slips_on_slope() const { + return slips_on_slope; +} + +void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &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 RayShape3DSW::get_support(const Vector3 &p_normal) const { + + if (p_normal.z > 0) + return Vector3(0, 0, length); + else + return Vector3(0, 0, 0); +} + +void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + + r_amount = 2; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + *r_supports = Vector3(0, 0, length); + } else { + r_amount = 1; + *r_supports = Vector3(0, 0, 0); + } +} + +bool RayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + return false; //simply not possible +} + +bool RayShape3DSW::intersect_point(const Vector3 &p_point) const { + + return false; //simply not possible +} + +Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry::get_closest_point_to_segment(p_point, s); +} + +Vector3 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + return Vector3(); +} + +void RayShape3DSW::_setup(real_t p_length, bool p_slips_on_slope) { + + length = p_length; + slips_on_slope = p_slips_on_slope; + configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); +} + +void RayShape3DSW::set_data(const Variant &p_data) { + + Dictionary d = p_data; + _setup(d["length"], d["slips_on_slope"]); +} + +Variant RayShape3DSW::get_data() const { + + Dictionary d; + d["length"] = length; + d["slips_on_slope"] = slips_on_slope; + return d; +} + +RayShape3DSW::RayShape3DSW() { + + length = 1; + slips_on_slope = false; +} + +/********** SPHERE *************/ + +real_t SphereShape3DSW::get_radius() const { + + return radius; +} + +void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform &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 SphereShape3DSW::get_support(const Vector3 &p_normal) const { + + return p_normal * radius; +} + +void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + *r_supports = p_normal * radius; + r_amount = 1; +} + +bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); +} + +bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { + + return p_point.length() < radius; +} + +Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 p = p_point; + float l = p.length(); + if (l < radius) + return p_point; + return (p / l) * radius; +} + +Vector3 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + real_t s = 0.4 * p_mass * radius * radius; + return Vector3(s, s, s); +} + +void SphereShape3DSW::_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 SphereShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant SphereShape3DSW::get_data() const { + + return radius; +} + +SphereShape3DSW::SphereShape3DSW() { + + radius = 0; +} + +/********** BOX *************/ + +void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform &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 BoxShape3DSW::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 BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) 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; + + 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; + + 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_supports[0] = point; +} + +bool BoxShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + AABB aabb(-half_extents, half_extents * 2.0); + + return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); +} + +bool BoxShape3DSW::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 BoxShape3DSW::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] = SGN(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 + float 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 = Geometry::get_closest_point_to_segment(p_point, s); + + float d = p_point.distance_to(closest_edge); + if (d < min_distance) { + min_point = closest_edge; + min_distance = d; + } + } + + return min_point; +} + +Vector3 BoxShape3DSW::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 BoxShape3DSW::_setup(const Vector3 &p_half_extents) { + + half_extents = p_half_extents.abs(); + + configure(AABB(-half_extents, half_extents * 2)); +} + +void BoxShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant BoxShape3DSW::get_data() const { + + return half_extents; +} + +BoxShape3DSW::BoxShape3DSW() { +} + +/********** CAPSULE *************/ + +void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + real_t h = (n.z > 0) ? height : -height; + + n *= radius; + n.z += h * 0.5; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); +} + +Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { + + Vector3 n = p_normal; + + real_t h = (n.z > 0) ? height : -height; + + n *= radius; + n.z += h * 0.5; + return n; +} + +void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + Vector3 n = p_normal; + + real_t d = n.z; + + if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + + // make it flat + n.z = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_supports[0] = n; + r_supports[0].z += height * 0.5; + r_supports[1] = n; + r_supports[1].z -= height * 0.5; + + } else { + + real_t h = (d > 0) ? height : -height; + + n *= radius; + n.z += h * 0.5; + r_amount = 1; + *r_supports = n; + } +} + +bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) 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 = Geometry::segment_intersects_cylinder(p_begin, p_end, height, 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 = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), 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 = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), 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 CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { + + if (Math::abs(p_point.z) < height * 0.5) { + return Vector3(p_point.x, p_point.y, 0).length() < radius; + } else { + Vector3 p = p_point; + p.z = Math::abs(p.z) - height * 0.5; + return p.length() < radius; + } +} + +Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 s[2] = { + Vector3(0, 0, -height * 0.5), + Vector3(0, 0, height * 0.5), + }; + + Vector3 p = Geometry::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 CapsuleShape3DSW::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.y * extents.y + extents.y * extents.y)); +} + +void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) { + + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); +} + +void CapsuleShape3DSW::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 CapsuleShape3DSW::get_data() const { + + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +CapsuleShape3DSW::CapsuleShape3DSW() { + + height = radius = 0; +} + +/********** CONVEX POLYGON *************/ + +void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &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 ConvexPolygonShape3DSW::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 ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + int vc = mesh.vertices.size(); + + //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; + 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_supports[0] = vertices[edges[i].a]; + r_supports[1] = vertices[edges[i].b]; + return; + } + } + + r_supports[0] = vertices[vtx]; + r_amount = 1; +} + +bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + const Geometry::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 ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { + + const Geometry::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 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + const Geometry::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(a, n).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; + } + + float min_distance = 1e20; + Vector3 min_point; + + //check edges + const Geometry::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 = Geometry::get_closest_point_to_segment(p_point, s); + float d = closest.distance_to(p_point); + if (d < min_distance) { + min_distance = d; + min_point = closest; + } + } + + return min_point; +} + +Vector3 ConvexPolygonShape3DSW::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.y * extents.y + extents.y * extents.y)); +} + +void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) { + + Error err = QuickHull::build(p_vertices, mesh); + if (err != OK) + ERR_PRINT("Failed to build QuickHull"); + + 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 ConvexPolygonShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant ConvexPolygonShape3DSW::get_data() const { + + return mesh.vertices; +} + +ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() { +} + +/********** FACE POLYGON *************/ + +void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &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 FaceShape3DSW::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 FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + Vector3 n = p_normal; + + /** TEST FACE AS SUPPORT **/ + if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + + r_amount = 3; + 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_supports[0] = vertex[i]; + r_supports[1] = vertex[nx]; + return; + } + } + + r_amount = 1; + r_supports[0] = vertex[vert_support_idx]; +} + +bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + bool c = Geometry::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) { + r_normal = -r_normal; + } + } + + return c; +} + +bool FaceShape3DSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + +Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! +} + +FaceShape3DSW::FaceShape3DSW() { + + configure(AABB()); +} + +Vector<Vector3> ConcavePolygonShape3DSW::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 ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &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 ConcavePolygonShape3DSW::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 ConcavePolygonShape3DSW::_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) { + + Vector3 res; + Vector3 vertices[3] = { + p_params->vertices[p_params->faces[bvh->face_index].indices[0]], + p_params->vertices[p_params->faces[bvh->face_index].indices[1]], + p_params->vertices[p_params->faces[bvh->face_index].indices[2]] + }; + + if (Geometry::segment_intersects_triangle( + p_params->from, + p_params->to, + vertices[0], + vertices[1], + vertices[2], + &res)) { + + real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); + //TODO, seems segmen/triangle intersection is broken :( + if (d > 0 && d < p_params->min_d) { + + p_params->min_d = d; + p_params->result = res; + p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).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 ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + if (faces.size() == 0) + return false; + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + _SegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.collisions = 0; + params.dir = (p_end - p_begin).normalized(); + + params.faces = fr; + params.vertices = vr; + params.bvh = br; + + params.min_d = 1e20; + // cull + _cull_segment(0, ¶ms); + + if (params.collisions > 0) { + + r_result = params.result; + r_normal = params.normal; + return true; + } else { + + return false; + } +} + +bool ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + +void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { + + const BVH *bvh = &p_params->bvh[p_idx]; + + if (!p_params->aabb.intersects(bvh->aabb)) + return; + + if (bvh->face_index >= 0) { + + const Face *f = &p_params->faces[bvh->face_index]; + FaceShape3DSW *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]]; + p_params->callback(p_params->userdata, face); + + } else { + + if (bvh->left >= 0) { + + _cull(bvh->left, p_params); + } + + if (bvh->right >= 0) { + + _cull(bvh->right, p_params); + } + } +} + +void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) 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(); + + FaceShape3DSW face; // use this to send in the callback + + _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 ConcavePolygonShape3DSW::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.y * extents.y + extents.y * extents.y)); +} + +struct _VolumeSW_BVH_Element { + + AABB aabb; + Vector3 center; + int face_index; +}; + +struct _VolumeSW_BVH_CompareX { + + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { + + return a.center.x < b.center.x; + } +}; + +struct _VolumeSW_BVH_CompareY { + + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { + + return a.center.y < b.center.y; + } +}; + +struct _VolumeSW_BVH_CompareZ { + + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { + + return a.center.z < b.center.z; + } +}; + +struct _VolumeSW_BVH { + + AABB aabb; + _VolumeSW_BVH *left; + _VolumeSW_BVH *right; + + int face_index; +}; + +_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) { + + _VolumeSW_BVH *bvh = memnew(_VolumeSW_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<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x; + sort_x.sort(p_elements, p_size); + + } break; + case 1: { + + SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y; + sort_y.sort(p_elements, p_size); + } break; + case 2: { + + SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z; + sort_z.sort(p_elements, p_size); + } break; + } + + int split = p_size / 2; + bvh->left = _volume_sw_build_bvh(p_elements, split, count); + bvh->right = _volume_sw_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 ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_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 ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { + + 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<_VolumeSW_BVH_Element> bvh_array; + bvh_array.resize(src_face_count); + + _VolumeSW_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.position + bvh_arrayw[i].aabb.size * 0.5; + 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; + _VolumeSW_BVH *bvh_tree = _volume_sw_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); + + configure(_aabb); // this type of shape has no margin +} + +void ConcavePolygonShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant ConcavePolygonShape3DSW::get_data() const { + + return get_faces(); +} + +ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { +} + +/* HEIGHT MAP SHAPE */ + +Vector<real_t> HeightMapShape3DSW::get_heights() const { + + return heights; +} +int HeightMapShape3DSW::get_width() const { + + return width; +} +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); +} + +Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { + + //not very useful, but not very used either + return get_aabb().get_support(p_normal); +} + +bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { + + return false; +} + +bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + +void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { +} + +Vector3 HeightMapShape3DSW::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.y * extents.y + extents.y * extents.y)); +} + +void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { + + heights = p_heights; + width = p_width; + depth = p_depth; + cell_size = p_cell_size; + + const real_t *r = heights.ptr(); + + AABB aabb; + + for (int i = 0; i < depth; i++) { + + for (int j = 0; j < width; j++) { + + real_t h = r[i * width + j]; + + Vector3 pos(j * cell_size, h, i * cell_size); + if (i == 0 || j == 0) + aabb.position = pos; + else + aabb.expand_to(pos); + } + } + + 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<real_t> 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); +} + +Variant HeightMapShape3DSW::get_data() const { + + ERR_FAIL_V(Variant()); +} + +HeightMapShape3DSW::HeightMapShape3DSW() { + + width = 0; + depth = 0; + cell_size = 0; +} |