diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index cfd983bb0a..d438869157 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -55,6 +55,14 @@ Comment: Godot Engine logo Copyright: 2017, Andrea Calabró License: CC-BY-4.0 +Files: ./core/math/convex_hull.cpp + ./core/math/convex_hull.h +Comment: Bullet Continuous Collision Detection and Physics Library +Copyright: 2011, Ole Kniemeyer, MAXON, www.maxon.net + 2007-2021, Juan Linietsky, Ariel Manzur. + 2014-2021, Godot Engine contributors. +License: Expat and Zlib + Files: ./modules/fbx/fbx_parser/ Comment: Open Asset Import Library (FBX parser) Copyright: 2006-2020, assimp team diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp new file mode 100644 index 0000000000..682a7ea39e --- /dev/null +++ b/core/math/convex_hull.cpp @@ -0,0 +1,2290 @@ +/*************************************************************************/ +/* convex_hull.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. */ +/*************************************************************************/ + +/* + * Based on Godot's patched VHACD-version of Bullet's btConvexHullComputer. + * See /thirdparty/vhacd/btConvexHullComputer.cpp at 64403ddcab9f1dca2408f0a412a22d899708bbb1 + * In turn, based on /src/LinearMath/btConvexHullComputer.cpp in + * at 73b217fb07e7e3ce126caeb28ab3c9ddd0718467 + * + * Changes: + * - int32_t is consistently used instead of int in some cases + * - integrated patch db0d6c92927f5a1358b887f2645c11f3014f0e8a from Bullet (CWE-190 integer overflow in btConvexHullComputer) + * - adapted to Godot's code style + * - replaced Bullet's types (e.g. vectors) with Godot's + * - replaced custom Pool implementation with PagedAllocator + */ + +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +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. +*/ + +#include "convex_hull.h" + +#include "core/error/error_macros.h" +#include "core/math/aabb.h" +#include "core/math/math_defs.h" +#include "core/os/memory.h" +#include "core/templates/paged_allocator.h" + +#include + +//#define DEBUG_CONVEX_HULL +//#define SHOW_ITERATIONS + +// -- GODOT start -- +// Assembly optimizations are not used at the moment. +//#define USE_X86_64_ASM +// -- GODOT end -- + +#ifdef DEBUG_ENABLED +#define CHULL_ASSERT(m_cond) \ + do { \ + if (unlikely(!(m_cond))) { \ + ERR_PRINT("Assertion \"" _STR(m_cond) "\" failed."); \ + } \ + } while (0) +#else +#define CHULL_ASSERT(m_cond) \ + do { \ + } while (0) +#endif + +#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) +#include +#endif + +// Convex hull implementation based on Preparata and Hong +// Ole Kniemeyer, MAXON Computer GmbH +class ConvexHullInternal { +public: + class Point64 { + public: + int64_t x; + int64_t y; + int64_t z; + + Point64(int64_t p_x, int64_t p_y, int64_t p_z) { + x = p_x; + y = p_y; + z = p_z; + } + + bool is_zero() { + return (x == 0) && (y == 0) && (z == 0); + } + + int64_t dot(const Point64 &b) const { + return x * b.x + y * b.y + z * b.z; + } + }; + + class Point32 { + public: + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + int32_t index = -1; + + Point32() { + } + + Point32(int32_t p_x, int32_t p_y, int32_t p_z) { + x = p_x; + y = p_y; + z = p_z; + } + + bool operator==(const Point32 &b) const { + return (x == b.x) && (y == b.y) && (z == b.z); + } + + bool operator!=(const Point32 &b) const { + return (x != b.x) || (y != b.y) || (z != b.z); + } + + bool is_zero() { + return (x == 0) && (y == 0) && (z == 0); + } + + Point64 cross(const Point32 &b) const { + return Point64((int64_t)y * b.z - (int64_t)z * b.y, (int64_t)z * b.x - (int64_t)x * b.z, (int64_t)x * b.y - (int64_t)y * b.x); + } + + Point64 cross(const Point64 &b) const { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + int64_t dot(const Point32 &b) const { + return (int64_t)x * b.x + (int64_t)y * b.y + (int64_t)z * b.z; + } + + int64_t dot(const Point64 &b) const { + return x * b.x + y * b.y + z * b.z; + } + + Point32 operator+(const Point32 &b) const { + return Point32(x + b.x, y + b.y, z + b.z); + } + + Point32 operator-(const Point32 &b) const { + return Point32(x - b.x, y - b.y, z - b.z); + } + }; + + class Int128 { + public: + uint64_t low = 0; + uint64_t high = 0; + + Int128() { + } + + Int128(uint64_t p_low, uint64_t p_high) { + low = p_low; + high = p_high; + } + + Int128(uint64_t p_low) { + low = p_low; + high = 0; + } + + Int128(int64_t p_value) { + low = p_value; + if (p_value >= 0) { + high = 0; + } else { + high = (uint64_t)-1LL; + } + } + + static Int128 mul(int64_t a, int64_t b); + + static Int128 mul(uint64_t a, uint64_t b); + + Int128 operator-() const { + return Int128((uint64_t) - (int64_t)low, ~high + (low == 0)); + } + + Int128 operator+(const Int128 &b) const { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); + return result; +#else + uint64_t lo = low + b.low; + return Int128(lo, high + b.high + (lo < low)); +#endif + } + + Int128 operator-(const Int128 &b) const { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__("subq %[bl], %[rl]\n\t" + "sbbq %[bh], %[rh]\n\t" + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); + return result; +#else + return *this + -b; +#endif + } + + Int128 &operator+=(const Int128 &b) { +#ifdef USE_X86_64_ASM + __asm__("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r"(low), [rh] "=r"(high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); +#else + uint64_t lo = low + b.low; + if (lo < low) { + ++high; + } + low = lo; + high += b.high; +#endif + return *this; + } + + Int128 &operator++() { + if (++low == 0) { + ++high; + } + return *this; + } + + Int128 operator*(int64_t b) const; + + real_t to_scalar() const { + return ((int64_t)high >= 0) ? real_t(high) * (real_t(0x100000000LL) * real_t(0x100000000LL)) + real_t(low) : -(-*this).to_scalar(); + } + + int32_t get_sign() const { + return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : + 0; + } + + bool operator<(const Int128 &b) const { + return (high < b.high) || ((high == b.high) && (low < b.low)); + } + + int32_t ucmp(const Int128 &b) const { + if (high < b.high) { + return -1; + } + if (high > b.high) { + return 1; + } + if (low < b.low) { + return -1; + } + if (low > b.low) { + return 1; + } + return 0; + } + }; + + class Rational64 { + private: + uint64_t numerator; + uint64_t denominator; + int32_t sign; + + public: + Rational64(int64_t p_numerator, int64_t p_denominator) { + if (p_numerator > 0) { + sign = 1; + numerator = (uint64_t)p_numerator; + } else if (p_numerator < 0) { + sign = -1; + numerator = (uint64_t)-p_numerator; + } else { + sign = 0; + numerator = 0; + } + if (p_denominator > 0) { + denominator = (uint64_t)p_denominator; + } else if (p_denominator < 0) { + sign = -sign; + denominator = (uint64_t)-p_denominator; + } else { + denominator = 0; + } + } + + bool is_negative_infinity() const { + return (sign < 0) && (denominator == 0); + } + + bool is_nan() const { + return (sign == 0) && (denominator == 0); + } + + int32_t compare(const Rational64 &b) const; + + real_t to_scalar() const { + return sign * ((denominator == 0) ? FLT_MAX : (real_t)numerator / denominator); + } + }; + + class Rational128 { + private: + Int128 numerator; + Int128 denominator; + int32_t sign; + bool is_int_64; + + public: + Rational128(int64_t p_value) { + if (p_value > 0) { + sign = 1; + this->numerator = p_value; + } else if (p_value < 0) { + sign = -1; + this->numerator = -p_value; + } else { + sign = 0; + this->numerator = (uint64_t)0; + } + this->denominator = (uint64_t)1; + is_int_64 = true; + } + + Rational128(const Int128 &p_numerator, const Int128 &p_denominator) { + sign = p_numerator.get_sign(); + if (sign >= 0) { + this->numerator = p_numerator; + } else { + this->numerator = -p_numerator; + } + int32_t dsign = p_denominator.get_sign(); + if (dsign >= 0) { + this->denominator = p_denominator; + } else { + sign = -sign; + this->denominator = -p_denominator; + } + is_int_64 = false; + } + + int32_t compare(const Rational128 &b) const; + + int32_t compare(int64_t b) const; + + real_t to_scalar() const { + return sign * ((denominator.get_sign() == 0) ? FLT_MAX : numerator.to_scalar() / denominator.to_scalar()); + } + }; + + class PointR128 { + public: + Int128 x; + Int128 y; + Int128 z; + Int128 denominator; + + PointR128() { + } + + PointR128(Int128 p_x, Int128 p_y, Int128 p_z, Int128 p_denominator) { + x = p_x; + y = p_y; + z = p_z; + denominator = p_denominator; + } + + real_t xvalue() const { + return x.to_scalar() / denominator.to_scalar(); + } + + real_t yvalue() const { + return y.to_scalar() / denominator.to_scalar(); + } + + real_t zvalue() const { + return z.to_scalar() / denominator.to_scalar(); + } + }; + + class Edge; + class Face; + + class Vertex { + public: + Vertex *next = nullptr; + Vertex *prev = nullptr; + Edge *edges = nullptr; + Face *first_nearby_face = nullptr; + Face *last_nearby_face = nullptr; + PointR128 point128; + Point32 point; + int32_t copy = -1; + + Vertex() { + } + +#ifdef DEBUG_CONVEX_HULL + void print() { + printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); + } + + void print_graph(); +#endif + + Point32 operator-(const Vertex &b) const { + return point - b.point; + } + + Rational128 dot(const Point64 &b) const { + return (point.index >= 0) ? Rational128(point.dot(b)) : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); + } + + real_t xvalue() const { + return (point.index >= 0) ? real_t(point.x) : point128.xvalue(); + } + + real_t yvalue() const { + return (point.index >= 0) ? real_t(point.y) : point128.yvalue(); + } + + real_t zvalue() const { + return (point.index >= 0) ? real_t(point.z) : point128.zvalue(); + } + + void receive_nearby_faces(Vertex *p_src) { + if (last_nearby_face) { + last_nearby_face->next_with_same_nearby_vertex = p_src->first_nearby_face; + } else { + first_nearby_face = p_src->first_nearby_face; + } + if (p_src->last_nearby_face) { + last_nearby_face = p_src->last_nearby_face; + } + for (Face *f = p_src->first_nearby_face; f; f = f->next_with_same_nearby_vertex) { + CHULL_ASSERT(f->nearby_vertex == p_src); + f->nearby_vertex = this; + } + p_src->first_nearby_face = nullptr; + p_src->last_nearby_face = nullptr; + } + }; + + class Edge { + public: + Edge *next = nullptr; + Edge *prev = nullptr; + Edge *reverse = nullptr; + Vertex *target = nullptr; + Face *face = nullptr; + int32_t copy = -1; + + void link(Edge *n) { + CHULL_ASSERT(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } + +#ifdef DEBUG_CONVEX_HULL + void print() { + printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev, + reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); + } +#endif + }; + + class Face { + public: + Face *next = nullptr; + Vertex *nearby_vertex = nullptr; + Face *next_with_same_nearby_vertex = nullptr; + Point32 origin; + Point32 dir0; + Point32 dir1; + + Face() { + } + + void init(Vertex *p_a, Vertex *p_b, Vertex *p_c) { + nearby_vertex = p_a; + origin = p_a->point; + dir0 = *p_b - *p_a; + dir1 = *p_c - *p_a; + if (p_a->last_nearby_face) { + p_a->last_nearby_face->next_with_same_nearby_vertex = this; + } else { + p_a->first_nearby_face = this; + } + p_a->last_nearby_face = this; + } + + Point64 get_normal() { + return dir0.cross(dir1); + } + }; + + template + class DMul { + private: + static uint32_t high(uint64_t p_value) { + return (uint32_t)(p_value >> 32); + } + + static uint32_t low(uint64_t p_value) { + return (uint32_t)p_value; + } + + static uint64_t mul(uint32_t a, uint32_t b) { + return (uint64_t)a * (uint64_t)b; + } + + static void shl_half(uint64_t &p_value) { + p_value <<= 32; + } + + static uint64_t high(Int128 p_value) { + return p_value.high; + } + + static uint64_t low(Int128 p_value) { + return p_value.low; + } + + static Int128 mul(uint64_t a, uint64_t b) { + return Int128::mul(a, b); + } + + static void shl_half(Int128 &p_value) { + p_value.high = p_value.low; + p_value.low = 0; + } + + public: + static void mul(UWord p_a, UWord p_b, UWord &r_low, UWord &r_high) { + UWord p00 = mul(low(p_a), low(p_b)); + UWord p01 = mul(low(p_a), high(p_b)); + UWord p10 = mul(high(p_a), low(p_b)); + UWord p11 = mul(high(p_a), high(p_b)); + UWord p0110 = UWord(low(p01)) + UWord(low(p10)); + p11 += high(p01); + p11 += high(p10); + p11 += high(p0110); + shl_half(p0110); + p00 += p0110; + if (p00 < p0110) { + ++p11; + } + r_low = p00; + r_high = p11; + } + }; + +private: + class IntermediateHull { + public: + Vertex *min_xy = nullptr; + Vertex *max_xy = nullptr; + Vertex *min_yx = nullptr; + Vertex *max_yx = nullptr; + + IntermediateHull() { + } + + void print(); + }; + + enum Orientation { NONE, + CLOCKWISE, + COUNTER_CLOCKWISE }; + + Vector3 scaling; + Vector3 center; + PagedAllocator vertex_pool; + PagedAllocator edge_pool; + PagedAllocator face_pool; + LocalVector original_vertices; + int32_t merge_stamp = 0; + int32_t min_axis = 0; + int32_t med_axis = 0; + int32_t max_axis = 0; + int32_t used_edge_pairs = 0; + int32_t max_used_edge_pairs = 0; + + static Orientation get_orientation(const Edge *p_prev, const Edge *p_next, const Point32 &p_s, const Point32 &p_t); + Edge *find_max_angle(bool p_ccw, const Vertex *p_start, const Point32 &p_s, const Point64 &p_rxs, const Point64 &p_ssxrxs, Rational64 &p_min_cot); + void find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, Vertex *p_stop0, Vertex *p_stop1); + + Edge *new_edge_pair(Vertex *p_from, Vertex *p_to); + + void remove_edge_pair(Edge *p_edge) { + Edge *n = p_edge->next; + Edge *r = p_edge->reverse; + + CHULL_ASSERT(p_edge->target && r->target); + + if (n != p_edge) { + n->prev = p_edge->prev; + p_edge->prev->next = n; + r->target->edges = n; + } else { + r->target->edges = nullptr; + } + + n = r->next; + + if (n != r) { + n->prev = r->prev; + r->prev->next = n; + p_edge->target->edges = n; + } else { + p_edge->target->edges = nullptr; + } + + edge_pool.free(p_edge); + edge_pool.free(r); + used_edge_pairs--; + } + + void compute_internal(int32_t p_start, int32_t p_end, IntermediateHull &r_result); + + bool merge_projection(IntermediateHull &p_h0, IntermediateHull &p_h1, Vertex *&r_c0, Vertex *&r_c1); + + void merge(IntermediateHull &p_h0, IntermediateHull &p_h1); + + Vector3 to_gd_vector(const Point32 &p_v); + + Vector3 get_gd_normal(Face *p_face); + + bool shift_face(Face *p_face, real_t p_amount, LocalVector p_stack); + +public: + ~ConvexHullInternal() { + vertex_pool.reset(true); + edge_pool.reset(true); + face_pool.reset(true); + } + + Vertex *vertex_list; + + void compute(const Vector3 *p_coords, int32_t p_count); + + Vector3 get_coordinates(const Vertex *p_v); + + real_t shrink(real_t amount, real_t p_clamp_amount); +}; + +ConvexHullInternal::Int128 ConvexHullInternal::Int128::operator*(int64_t b) const { + bool negative = (int64_t)high < 0; + Int128 a = negative ? -*this : *this; + if (b < 0) { + negative = !negative; + b = -b; + } + Int128 result = mul(a.low, (uint64_t)b); + result.high += a.high * (uint64_t)b; + return negative ? -result : result; +} + +ConvexHullInternal::Int128 ConvexHullInternal::Int128::mul(int64_t a, int64_t b) { + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__("imulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); + return result; + +#else + bool negative = a < 0; + if (negative) { + a = -a; + } + if (b < 0) { + negative = !negative; + b = -b; + } + DMul::mul((uint64_t)a, (uint64_t)b, result.low, result.high); + return negative ? -result : result; +#endif +} + +ConvexHullInternal::Int128 ConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) { + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__("mulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); + +#else + DMul::mul(a, b, result.low, result.high); +#endif + + return result; +} + +int32_t ConvexHullInternal::Rational64::compare(const Rational64 &b) const { + if (sign != b.sign) { + return sign - b.sign; + } else if (sign == 0) { + return 0; + } + + // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; + +#ifdef USE_X86_64_ASM + + int32_t result; + int64_t tmp; + int64_t dummy; + __asm__("mulq %[bn]\n\t" + "movq %%rax, %[tmp]\n\t" + "movq %%rdx, %%rbx\n\t" + "movq %[tn], %%rax\n\t" + "mulq %[bd]\n\t" + "subq %[tmp], %%rax\n\t" + "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" + "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise + "orq %%rdx, %%rax\n\t" + "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero + "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference) + "shll $16, %%ebx\n\t" // ebx has same sign as difference + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) + : "%rdx", "cc"); + return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero) + // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) + : + 0; + +#else + + return sign * Int128::mul(numerator, b.denominator).ucmp(Int128::mul(denominator, b.numerator)); + +#endif +} + +int32_t ConvexHullInternal::Rational128::compare(const Rational128 &b) const { + if (sign != b.sign) { + return sign - b.sign; + } else if (sign == 0) { + return 0; + } + if (is_int_64) { + return -b.compare(sign * (int64_t)numerator.low); + } + + Int128 nbd_low, nbd_high, dbn_low, dbn_high; + DMul::mul(numerator, b.denominator, nbd_low, nbd_high); + DMul::mul(denominator, b.numerator, dbn_low, dbn_high); + + int32_t cmp = nbd_high.ucmp(dbn_high); + if (cmp) { + return cmp * sign; + } + return nbd_low.ucmp(dbn_low) * sign; +} + +int32_t ConvexHullInternal::Rational128::compare(int64_t b) const { + if (is_int_64) { + int64_t a = sign * (int64_t)numerator.low; + return (a > b) ? 1 : (a < b) ? -1 : + 0; + } + if (b > 0) { + if (sign <= 0) { + return -1; + } + } else if (b < 0) { + if (sign >= 0) { + return 1; + } + b = -b; + } else { + return sign; + } + + return numerator.ucmp(denominator * b) * sign; +} + +ConvexHullInternal::Edge *ConvexHullInternal::new_edge_pair(Vertex *p_from, Vertex *p_to) { + CHULL_ASSERT(p_from && p_to); + Edge *e = edge_pool.alloc(); + Edge *r = edge_pool.alloc(); + e->reverse = r; + r->reverse = e; + e->copy = merge_stamp; + r->copy = merge_stamp; + e->target = p_to; + r->target = p_from; + e->face = nullptr; + r->face = nullptr; + used_edge_pairs++; + if (used_edge_pairs > max_used_edge_pairs) { + max_used_edge_pairs = used_edge_pairs; + } + return e; +} + +bool ConvexHullInternal::merge_projection(IntermediateHull &r_h0, IntermediateHull &r_h1, Vertex *&r_c0, Vertex *&r_c1) { + Vertex *v0 = r_h0.max_yx; + Vertex *v1 = r_h1.min_yx; + if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) { + CHULL_ASSERT(v0->point.z < v1->point.z); + Vertex *v1p = v1->prev; + if (v1p == v1) { + r_c0 = v0; + if (v1->edges) { + CHULL_ASSERT(v1->edges->next == v1->edges); + v1 = v1->edges->target; + CHULL_ASSERT(v1->edges->next == v1->edges); + } + r_c1 = v1; + return false; + } + Vertex *v1n = v1->next; + v1p->next = v1n; + v1n->prev = v1p; + if (v1 == r_h1.min_xy) { + if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) { + r_h1.min_xy = v1n; + } else { + r_h1.min_xy = v1p; + } + } + if (v1 == r_h1.max_xy) { + if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) { + r_h1.max_xy = v1n; + } else { + r_h1.max_xy = v1p; + } + } + } + + v0 = r_h0.max_xy; + v1 = r_h1.max_xy; + Vertex *v00 = nullptr; + Vertex *v10 = nullptr; + int32_t sign = 1; + + for (int32_t side = 0; side <= 1; side++) { + int32_t dx = (v1->point.x - v0->point.x) * sign; + if (dx > 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex *w0 = side ? v0->next : v0->prev; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) { + v0 = w0; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex *w1 = side ? v1->next : v1->prev; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + int32_t dxn = (w1->point.x - v0->point.x) * sign; + if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) { + v1 = w1; + dx = dxn; + continue; + } + } + + break; + } + } else if (dx < 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex *w1 = side ? v1->prev : v1->next; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) { + v1 = w1; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex *w0 = side ? v0->prev : v0->next; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + int32_t dxn = (v1->point.x - w0->point.x) * sign; + if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) { + v0 = w0; + dx = dxn; + continue; + } + } + + break; + } + } else { + int32_t x = v0->point.x; + int32_t y0 = v0->point.y; + Vertex *w0 = v0; + Vertex *t; + while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) { + w0 = t; + y0 = t->point.y; + } + v0 = w0; + + int32_t y1 = v1->point.y; + Vertex *w1 = v1; + while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) { + w1 = t; + y1 = t->point.y; + } + v1 = w1; + } + + if (side == 0) { + v00 = v0; + v10 = v1; + + v0 = r_h0.min_xy; + v1 = r_h1.min_xy; + sign = -1; + } + } + + v0->prev = v1; + v1->next = v0; + + v00->next = v10; + v10->prev = v00; + + if (r_h1.min_xy->point.x < r_h0.min_xy->point.x) { + r_h0.min_xy = r_h1.min_xy; + } + if (r_h1.max_xy->point.x >= r_h0.max_xy->point.x) { + r_h0.max_xy = r_h1.max_xy; + } + + r_h0.max_yx = r_h1.max_yx; + + r_c0 = v00; + r_c1 = v10; + + return true; +} + +void ConvexHullInternal::compute_internal(int32_t p_start, int32_t p_end, IntermediateHull &r_result) { + int32_t n = p_end - p_start; + switch (n) { + case 0: + r_result.min_xy = nullptr; + r_result.max_xy = nullptr; + r_result.min_yx = nullptr; + r_result.max_yx = nullptr; + return; + case 2: { + Vertex *v = original_vertices[p_start]; + Vertex *w = original_vertices[p_start + 1]; + if (v->point != w->point) { + int32_t dx = v->point.x - w->point.x; + int32_t dy = v->point.y - w->point.y; + + if ((dx == 0) && (dy == 0)) { + if (v->point.z > w->point.z) { + Vertex *t = w; + w = v; + v = t; + } + CHULL_ASSERT(v->point.z < w->point.z); + v->next = v; + v->prev = v; + r_result.min_xy = v; + r_result.max_xy = v; + r_result.min_yx = v; + r_result.max_yx = v; + } else { + v->next = w; + v->prev = w; + w->next = v; + w->prev = v; + + if ((dx < 0) || ((dx == 0) && (dy < 0))) { + r_result.min_xy = v; + r_result.max_xy = w; + } else { + r_result.min_xy = w; + r_result.max_xy = v; + } + + if ((dy < 0) || ((dy == 0) && (dx < 0))) { + r_result.min_yx = v; + r_result.max_yx = w; + } else { + r_result.min_yx = w; + r_result.max_yx = v; + } + } + + Edge *e = new_edge_pair(v, w); + e->link(e); + v->edges = e; + + e = e->reverse; + e->link(e); + w->edges = e; + + return; + } + [[fallthrough]]; + } + case 1: { + Vertex *v = original_vertices[p_start]; + v->edges = nullptr; + v->next = v; + v->prev = v; + + r_result.min_xy = v; + r_result.max_xy = v; + r_result.min_yx = v; + r_result.max_yx = v; + + return; + } + } + + int32_t split0 = p_start + n / 2; + Point32 p = original_vertices[split0 - 1]->point; + int32_t split1 = split0; + while ((split1 < p_end) && (original_vertices[split1]->point == p)) { + split1++; + } + compute_internal(p_start, split0, r_result); + IntermediateHull hull1; + compute_internal(split1, p_end, hull1); +#ifdef DEBUG_CONVEX_HULL + printf("\n\nMerge\n"); + r_result.print(); + hull1.print(); +#endif + merge(r_result, hull1); +#ifdef DEBUG_CONVEX_HULL + printf("\n Result\n"); + r_result.print(); +#endif +} + +#ifdef DEBUG_CONVEX_HULL +void ConvexHullInternal::IntermediateHull::print() { + printf(" Hull\n"); + for (Vertex *v = min_xy; v;) { + printf(" "); + v->print(); + if (v == max_xy) { + printf(" max_xy"); + } + if (v == min_yx) { + printf(" min_yx"); + } + if (v == max_yx) { + printf(" max_yx"); + } + if (v->next->prev != v) { + printf(" Inconsistency"); + } + printf("\n"); + v = v->next; + if (v == min_xy) { + break; + } + } + if (min_xy) { + min_xy->copy = (min_xy->copy == -1) ? -2 : -1; + min_xy->print_graph(); + } +} + +void ConvexHullInternal::Vertex::print_graph() { + print(); + printf("\nEdges\n"); + Edge *e = edges; + if (e) { + do { + e->print(); + printf("\n"); + e = e->next; + } while (e != edges); + do { + Vertex *v = e->target; + if (v->copy != copy) { + v->copy = copy; + v->print_graph(); + } + e = e->next; + } while (e != edges); + } +} +#endif + +ConvexHullInternal::Orientation ConvexHullInternal::get_orientation(const Edge *p_prev, const Edge *p_next, const Point32 &p_s, const Point32 &p_t) { + CHULL_ASSERT(p_prev->reverse->target == p_next->reverse->target); + if (p_prev->next == p_next) { + if (p_prev->prev == p_next) { + Point64 n = p_t.cross(p_s); + Point64 m = (*p_prev->target - *p_next->reverse->target).cross(*p_next->target - *p_next->reverse->target); + CHULL_ASSERT(!m.is_zero()); + int64_t dot = n.dot(m); + CHULL_ASSERT(dot != 0); + return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; + } + return COUNTER_CLOCKWISE; + } else if (p_prev->prev == p_next) { + return CLOCKWISE; + } else { + return NONE; + } +} + +ConvexHullInternal::Edge *ConvexHullInternal::find_max_angle(bool p_ccw, const Vertex *p_start, const Point32 &p_s, const Point64 &p_rxs, const Point64 &p_sxrxs, Rational64 &p_min_cot) { + Edge *min_edge = nullptr; + +#ifdef DEBUG_CONVEX_HULL + printf("find max edge for %d\n", p_start->point.index); +#endif + Edge *e = p_start->edges; + if (e) { + do { + if (e->copy > merge_stamp) { + Point32 t = *e->target - *p_start; + Rational64 cot(t.dot(p_sxrxs), t.dot(p_rxs)); +#ifdef DEBUG_CONVEX_HULL + printf(" Angle is %f (%d) for ", Math::atan(cot.to_scalar()), (int32_t)cot.is_nan()); + e->print(); +#endif + if (cot.is_nan()) { + CHULL_ASSERT(p_ccw ? (t.dot(p_s) < 0) : (t.dot(p_s) > 0)); + } else { + int32_t cmp; + if (min_edge == nullptr) { + p_min_cot = cot; + min_edge = e; + } else if ((cmp = cot.compare(p_min_cot)) < 0) { + p_min_cot = cot; + min_edge = e; + } else if ((cmp == 0) && (p_ccw == (get_orientation(min_edge, e, p_s, t) == COUNTER_CLOCKWISE))) { + min_edge = e; + } + } +#ifdef DEBUG_CONVEX_HULL + printf("\n"); +#endif + } + e = e->next; + } while (e != p_start->edges); + } + return min_edge; +} + +void ConvexHullInternal::find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, Vertex *p_stop0, Vertex *p_stop1) { + Edge *start0 = p_e0; + Edge *start1 = p_e1; + Point32 et0 = start0 ? start0->target->point : p_c0->point; + Point32 et1 = start1 ? start1->target->point : p_c1->point; + Point32 s = p_c1->point - p_c0->point; + Point64 normal = ((start0 ? start0 : start1)->target->point - p_c0->point).cross(s); + int64_t dist = p_c0->point.dot(normal); + CHULL_ASSERT(!start1 || (start1->target->point.dot(normal) == dist)); + Point64 perp = s.cross(normal); + CHULL_ASSERT(!perp.is_zero()); + +#ifdef DEBUG_CONVEX_HULL + printf(" Advancing %d %d (%p %p, %d %d)\n", p_c0->point.index, p_c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1); +#endif + + int64_t max_dot0 = et0.dot(perp); + if (p_e0) { + while (p_e0->target != p_stop0) { + Edge *e = p_e0->reverse->prev; + if (e->target->point.dot(normal) < dist) { + break; + } + CHULL_ASSERT(e->target->point.dot(normal) == dist); + if (e->copy == merge_stamp) { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= max_dot0) { + break; + } + max_dot0 = dot; + p_e0 = e; + et0 = e->target->point; + } + } + + int64_t max_dot1 = et1.dot(perp); + if (p_e1) { + while (p_e1->target != p_stop1) { + Edge *e = p_e1->reverse->next; + if (e->target->point.dot(normal) < dist) { + break; + } + CHULL_ASSERT(e->target->point.dot(normal) == dist); + if (e->copy == merge_stamp) { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= max_dot1) { + break; + } + max_dot1 = dot; + p_e1 = e; + et1 = e->target->point; + } + } + +#ifdef DEBUG_CONVEX_HULL + printf(" Starting at %d %d\n", et0.index, et1.index); +#endif + + int64_t dx = max_dot1 - max_dot0; + if (dx > 0) { + while (true) { + int64_t dy = (et1 - et0).dot(s); + + if (p_e0 && (p_e0->target != p_stop0)) { + Edge *f0 = p_e0->next->reverse; + if (f0->copy > merge_stamp) { + int64_t dx0 = (f0->target->point - et0).dot(perp); + int64_t dy0 = (f0->target->point - et0).dot(s); + if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) { + et0 = f0->target->point; + dx = (et1 - et0).dot(perp); + p_e0 = (p_e0 == start0) ? nullptr : f0; + continue; + } + } + } + + if (p_e1 && (p_e1->target != p_stop1)) { + Edge *f1 = p_e1->reverse->next; + if (f1->copy > merge_stamp) { + Point32 d1 = f1->target->point - et1; + if (d1.dot(normal) == 0) { + int64_t dx1 = d1.dot(perp); + int64_t dy1 = d1.dot(s); + int64_t dxn = (f1->target->point - et0).dot(perp); + if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) { + p_e1 = f1; + et1 = p_e1->target->point; + dx = dxn; + continue; + } + } else { + CHULL_ASSERT((p_e1 == start1) && (d1.dot(normal) < 0)); + } + } + } + + break; + } + } else if (dx < 0) { + while (true) { + int64_t dy = (et1 - et0).dot(s); + + if (p_e1 && (p_e1->target != p_stop1)) { + Edge *f1 = p_e1->prev->reverse; + if (f1->copy > merge_stamp) { + int64_t dx1 = (f1->target->point - et1).dot(perp); + int64_t dy1 = (f1->target->point - et1).dot(s); + if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) { + et1 = f1->target->point; + dx = (et1 - et0).dot(perp); + p_e1 = (p_e1 == start1) ? nullptr : f1; + continue; + } + } + } + + if (p_e0 && (p_e0->target != p_stop0)) { + Edge *f0 = p_e0->reverse->prev; + if (f0->copy > merge_stamp) { + Point32 d0 = f0->target->point - et0; + if (d0.dot(normal) == 0) { + int64_t dx0 = d0.dot(perp); + int64_t dy0 = d0.dot(s); + int64_t dxn = (et1 - f0->target->point).dot(perp); + if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) { + p_e0 = f0; + et0 = p_e0->target->point; + dx = dxn; + continue; + } + } else { + CHULL_ASSERT((p_e0 == start0) && (d0.dot(normal) < 0)); + } + } + } + + break; + } + } +#ifdef DEBUG_CONVEX_HULL + printf(" Advanced edges to %d %d\n", et0.index, et1.index); +#endif +} + +void ConvexHullInternal::merge(IntermediateHull &p_h0, IntermediateHull &p_h1) { + if (!p_h1.max_xy) { + return; + } + if (!p_h0.max_xy) { + p_h0 = p_h1; + return; + } + + merge_stamp--; + + Vertex *c0 = nullptr; + Edge *to_prev0 = nullptr; + Edge *first_new0 = nullptr; + Edge *pending_head0 = nullptr; + Edge *pending_tail0 = nullptr; + Vertex *c1 = nullptr; + Edge *to_prev1 = nullptr; + Edge *first_new1 = nullptr; + Edge *pending_head1 = nullptr; + Edge *pending_tail1 = nullptr; + Point32 prev_point; + + if (merge_projection(p_h0, p_h1, c0, c1)) { + Point32 s = *c1 - *c0; + Point64 normal = Point32(0, 0, -1).cross(s); + Point64 t = s.cross(normal); + CHULL_ASSERT(!t.is_zero()); + + Edge *e = c0->edges; + Edge *start0 = nullptr; + if (e) { + do { + int64_t dot = (*e->target - *c0).dot(normal); + CHULL_ASSERT(dot <= 0); + if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) { + if (!start0 || (get_orientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) { + start0 = e; + } + } + e = e->next; + } while (e != c0->edges); + } + + e = c1->edges; + Edge *start1 = nullptr; + if (e) { + do { + int64_t dot = (*e->target - *c1).dot(normal); + CHULL_ASSERT(dot <= 0); + if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) { + if (!start1 || (get_orientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) { + start1 = e; + } + } + e = e->next; + } while (e != c1->edges); + } + + if (start0 || start1) { + find_edge_for_coplanar_faces(c0, c1, start0, start1, nullptr, nullptr); + if (start0) { + c0 = start0->target; + } + if (start1) { + c1 = start1->target; + } + } + + prev_point = c1->point; + prev_point.z++; + } else { + prev_point = c1->point; + prev_point.x++; + } + + Vertex *first0 = c0; + Vertex *first1 = c1; + bool first_run = true; + + while (true) { + Point32 s = *c1 - *c0; + Point32 r = prev_point - c0->point; + Point64 rxs = r.cross(s); + Point64 sxrxs = s.cross(rxs); + +#ifdef DEBUG_CONVEX_HULL + printf("\n Checking %d %d\n", c0->point.index, c1->point.index); +#endif + Rational64 min_cot0(0, 0); + Edge *min0 = find_max_angle(false, c0, s, rxs, sxrxs, min_cot0); + Rational64 min_cot1(0, 0); + Edge *min1 = find_max_angle(true, c1, s, rxs, sxrxs, min_cot1); + if (!min0 && !min1) { + Edge *e = new_edge_pair(c0, c1); + e->link(e); + c0->edges = e; + + e = e->reverse; + e->link(e); + c1->edges = e; + return; + } else { + int32_t cmp = !min0 ? 1 : !min1 ? -1 : + min_cot0.compare(min_cot1); +#ifdef DEBUG_CONVEX_HULL + printf(" -> Result %d\n", cmp); +#endif + if (first_run || ((cmp >= 0) ? !min_cot1.is_negative_infinity() : !min_cot0.is_negative_infinity())) { + Edge *e = new_edge_pair(c0, c1); + if (pending_tail0) { + pending_tail0->prev = e; + } else { + pending_head0 = e; + } + e->next = pending_tail0; + pending_tail0 = e; + + e = e->reverse; + if (pending_tail1) { + pending_tail1->next = e; + } else { + pending_head1 = e; + } + e->prev = pending_tail1; + pending_tail1 = e; + } + + Edge *e0 = min0; + Edge *e1 = min1; + +#ifdef DEBUG_CONVEX_HULL + printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); +#endif + + if (cmp == 0) { + find_edge_for_coplanar_faces(c0, c1, e0, e1, nullptr, nullptr); + } + + if ((cmp >= 0) && e1) { + if (to_prev1) { + for (Edge *e = to_prev1->next, *n = nullptr; e != min1; e = n) { + n = e->next; + remove_edge_pair(e); + } + } + + if (pending_tail1) { + if (to_prev1) { + to_prev1->link(pending_head1); + } else { + min1->prev->link(pending_head1); + first_new1 = pending_head1; + } + pending_tail1->link(min1); + pending_head1 = nullptr; + pending_tail1 = nullptr; + } else if (!to_prev1) { + first_new1 = min1; + } + + prev_point = c1->point; + c1 = e1->target; + to_prev1 = e1->reverse; + } + + if ((cmp <= 0) && e0) { + if (to_prev0) { + for (Edge *e = to_prev0->prev, *n = nullptr; e != min0; e = n) { + n = e->prev; + remove_edge_pair(e); + } + } + + if (pending_tail0) { + if (to_prev0) { + pending_head0->link(to_prev0); + } else { + pending_head0->link(min0->next); + first_new0 = pending_head0; + } + min0->link(pending_tail0); + pending_head0 = nullptr; + pending_tail0 = nullptr; + } else if (!to_prev0) { + first_new0 = min0; + } + + prev_point = c0->point; + c0 = e0->target; + to_prev0 = e0->reverse; + } + } + + if ((c0 == first0) && (c1 == first1)) { + if (to_prev0 == nullptr) { + pending_head0->link(pending_tail0); + c0->edges = pending_tail0; + } else { + for (Edge *e = to_prev0->prev, *n = nullptr; e != first_new0; e = n) { + n = e->prev; + remove_edge_pair(e); + } + if (pending_tail0) { + pending_head0->link(to_prev0); + first_new0->link(pending_tail0); + } + } + + if (to_prev1 == nullptr) { + pending_tail1->link(pending_head1); + c1->edges = pending_tail1; + } else { + for (Edge *e = to_prev1->next, *n = nullptr; e != first_new1; e = n) { + n = e->next; + remove_edge_pair(e); + } + if (pending_tail1) { + to_prev1->link(pending_head1); + pending_tail1->link(first_new1); + } + } + + return; + } + + first_run = false; + } +} + +struct PointComparator { + _FORCE_INLINE_ bool operator()(const ConvexHullInternal::Point32 &p, const ConvexHullInternal::Point32 &q) const { + return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); + } +}; + +void ConvexHullInternal::compute(const Vector3 *p_coords, int32_t p_count) { + AABB aabb; + for (int32_t i = 0; i < p_count; i++) { + Vector3 p = p_coords[i]; + if (i == 0) { + aabb.position = p; + } else { + aabb.expand_to(p); + } + } + + Vector3 s = aabb.size; + max_axis = s.max_axis(); + min_axis = s.min_axis(); + if (min_axis == max_axis) { + min_axis = (max_axis + 1) % 3; + } + med_axis = 3 - max_axis - min_axis; + + s /= real_t(10216); + if (((med_axis + 1) % 3) != max_axis) { + s *= -1; + } + scaling = s; + + if (s[0] != 0) { + s[0] = real_t(1) / s[0]; + } + if (s[1] != 0) { + s[1] = real_t(1) / s[1]; + } + if (s[2] != 0) { + s[2] = real_t(1) / s[2]; + } + + center = aabb.position; + + LocalVector points; + points.resize(p_count); + for (int32_t i = 0; i < p_count; i++) { + Vector3 p = p_coords[i]; + p = (p - center) * s; + points[i].x = (int32_t)p[med_axis]; + points[i].y = (int32_t)p[max_axis]; + points[i].z = (int32_t)p[min_axis]; + points[i].index = i; + } + + points.sort_custom(); + + vertex_pool.reset(true); + original_vertices.resize(p_count); + for (int32_t i = 0; i < p_count; i++) { + Vertex *v = vertex_pool.alloc(); + v->edges = nullptr; + v->point = points[i]; + v->copy = -1; + original_vertices[i] = v; + } + + points.clear(); + + edge_pool.reset(true); + + used_edge_pairs = 0; + max_used_edge_pairs = 0; + + merge_stamp = -3; + + IntermediateHull hull; + compute_internal(0, p_count, hull); + vertex_list = hull.min_xy; +#ifdef DEBUG_CONVEX_HULL + printf("max. edges %d (3v = %d)", max_used_edge_pairs, 3 * p_count); +#endif +} + +Vector3 ConvexHullInternal::to_gd_vector(const Point32 &p_v) { + Vector3 p; + p[med_axis] = real_t(p_v.x); + p[max_axis] = real_t(p_v.y); + p[min_axis] = real_t(p_v.z); + return p * scaling; +} + +Vector3 ConvexHullInternal::get_gd_normal(Face *p_face) { + return to_gd_vector(p_face->dir0).cross(to_gd_vector(p_face->dir1)).normalized(); +} + +Vector3 ConvexHullInternal::get_coordinates(const Vertex *p_v) { + Vector3 p; + p[med_axis] = p_v->xvalue(); + p[max_axis] = p_v->yvalue(); + p[min_axis] = p_v->zvalue(); + return p * scaling + center; +} + +real_t ConvexHullInternal::shrink(real_t p_amount, real_t p_clamp_amount) { + if (!vertex_list) { + return 0; + } + int32_t stamp = --merge_stamp; + LocalVector stack; + vertex_list->copy = stamp; + stack.push_back(vertex_list); + LocalVector faces; + + Point32 ref = vertex_list->point; + Int128 hull_center_x(0, 0); + Int128 hull_center_y(0, 0); + Int128 hull_center_z(0, 0); + Int128 volume(0, 0); + + while (stack.size() > 0) { + Vertex *v = stack[stack.size() - 1]; + stack.remove(stack.size() - 1); + Edge *e = v->edges; + if (e) { + do { + if (e->target->copy != stamp) { + e->target->copy = stamp; + stack.push_back(e->target); + } + if (e->copy != stamp) { + Face *face = face_pool.alloc(); + face->init(e->target, e->reverse->prev->target, v); + faces.push_back(face); + Edge *f = e; + + Vertex *a = nullptr; + Vertex *b = nullptr; + do { + if (a && b) { + int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); + CHULL_ASSERT(vol >= 0); + Point32 c = v->point + a->point + b->point + ref; + hull_center_x += vol * c.x; + hull_center_y += vol * c.y; + hull_center_z += vol * c.z; + volume += vol; + } + + CHULL_ASSERT(f->copy != stamp); + f->copy = stamp; + f->face = face; + + a = b; + b = f->target; + + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != v->edges); + } + } + + if (volume.get_sign() <= 0) { + return 0; + } + + Vector3 hull_center; + hull_center[med_axis] = hull_center_x.to_scalar(); + hull_center[max_axis] = hull_center_y.to_scalar(); + hull_center[min_axis] = hull_center_z.to_scalar(); + hull_center /= 4 * volume.to_scalar(); + hull_center *= scaling; + + int32_t face_count = faces.size(); + + if (p_clamp_amount > 0) { + real_t min_dist = FLT_MAX; + for (int32_t i = 0; i < face_count; i++) { + Vector3 normal = get_gd_normal(faces[i]); + real_t dist = normal.dot(to_gd_vector(faces[i]->origin) - hull_center); + if (dist < min_dist) { + min_dist = dist; + } + } + + if (min_dist <= 0) { + return 0; + } + + p_amount = MIN(p_amount, min_dist * p_clamp_amount); + } + + uint32_t seed = 243703; + for (int32_t i = 0; i < face_count; i++, seed = 1664525 * seed + 1013904223) { + SWAP(faces[i], faces[seed % face_count]); + } + + for (int32_t i = 0; i < face_count; i++) { + if (!shift_face(faces[i], p_amount, stack)) { + return -p_amount; + } + } + + return p_amount; +} + +bool ConvexHullInternal::shift_face(Face *p_face, real_t p_amount, LocalVector p_stack) { + Vector3 orig_shift = get_gd_normal(p_face) * -p_amount; + if (scaling[0] != 0) { + orig_shift[0] /= scaling[0]; + } + if (scaling[1] != 0) { + orig_shift[1] /= scaling[1]; + } + if (scaling[2] != 0) { + orig_shift[2] /= scaling[2]; + } + Point32 shift((int32_t)orig_shift[med_axis], (int32_t)orig_shift[max_axis], (int32_t)orig_shift[min_axis]); + if (shift.is_zero()) { + return true; + } + Point64 normal = p_face->get_normal(); +#ifdef DEBUG_CONVEX_HULL + printf("\nShrinking p_face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", + p_face->origin.x, p_face->origin.y, p_face->origin.z, p_face->dir0.x, p_face->dir0.y, p_face->dir0.z, p_face->dir1.x, p_face->dir1.y, p_face->dir1.z, shift.x, shift.y, shift.z); +#endif + int64_t orig_dot = p_face->origin.dot(normal); + Point32 shifted_origin = p_face->origin + shift; + int64_t shifted_dot = shifted_origin.dot(normal); + CHULL_ASSERT(shifted_dot <= orig_dot); + if (shifted_dot >= orig_dot) { + return false; + } + + Edge *intersection = nullptr; + + Edge *start_edge = p_face->nearby_vertex->edges; +#ifdef DEBUG_CONVEX_HULL + printf("Start edge is "); + start_edge->print(); + printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shifted_dot); +#endif + Rational128 opt_dot = p_face->nearby_vertex->dot(normal); + int32_t cmp = opt_dot.compare(shifted_dot); +#ifdef SHOW_ITERATIONS + int32_t n = 0; +#endif + if (cmp >= 0) { + Edge *e = start_edge; + do { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + CHULL_ASSERT(dot.compare(orig_dot) <= 0); +#ifdef DEBUG_CONVEX_HULL + printf("Moving downwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float)dot.to_scalar(), (float)opt_dot.to_scalar(), shifted_dot); +#endif + if (dot.compare(opt_dot) < 0) { + int32_t c = dot.compare(shifted_dot); + opt_dot = dot; + e = e->reverse; + start_edge = e; + if (c < 0) { + intersection = e; + break; + } + cmp = c; + } + e = e->prev; + } while (e != start_edge); + + if (!intersection) { + return false; + } + } else { + Edge *e = start_edge; + do { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + CHULL_ASSERT(dot.compare(orig_dot) <= 0); +#ifdef DEBUG_CONVEX_HULL + printf("Moving upwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float)dot.to_scalar(), (float)opt_dot.to_scalar(), shifted_dot); +#endif + if (dot.compare(opt_dot) > 0) { + cmp = dot.compare(shifted_dot); + if (cmp >= 0) { + intersection = e; + break; + } + opt_dot = dot; + e = e->reverse; + start_edge = e; + } + e = e->prev; + } while (e != start_edge); + + if (!intersection) { + return true; + } + } + +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to find initial intersection\n", n); +#endif + + if (cmp == 0) { + Edge *e = intersection->reverse->next; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (e->target->dot(normal).compare(shifted_dot) <= 0) { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->next; + if (e == intersection->reverse) { + return true; + } +#ifdef DEBUG_CONVEX_HULL + printf("Checking for outwards edge, current edge is "); + e->print(); + printf("\n"); +#endif + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to check for complete containment\n", n); +#endif + } + + Edge *first_intersection = nullptr; + Edge *face_edge = nullptr; + Edge *first_face_edge = nullptr; + +#ifdef SHOW_ITERATIONS + int32_t m = 0; +#endif + while (true) { +#ifdef SHOW_ITERATIONS + m++; +#endif +#ifdef DEBUG_CONVEX_HULL + printf("Intersecting edge is "); + intersection->print(); + printf("\n"); +#endif + if (cmp == 0) { + Edge *e = intersection->reverse->next; + start_edge = e; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) { +#ifdef SHOW_ITERATIONS + n++; +#endif + if (e->target->dot(normal).compare(shifted_dot) >= 0) { + break; + } + intersection = e->reverse; + e = e->next; + if (e == start_edge) { + return true; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to advance intersection\n", n); +#endif + } + +#ifdef DEBUG_CONVEX_HULL + printf("Advanced intersecting edge to "); + intersection->print(); + printf(", cmp = %d\n", cmp); +#endif + + if (!first_intersection) { + first_intersection = intersection; + } else if (intersection == first_intersection) { + break; + } + + int32_t prev_cmp = cmp; + Edge *prev_intersection = intersection; + Edge *prev_face_edge = face_edge; + + Edge *e = intersection->reverse; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->reverse->prev; + CHULL_ASSERT(e != intersection->reverse); + cmp = e->target->dot(normal).compare(shifted_dot); +#ifdef DEBUG_CONVEX_HULL + printf("Testing edge "); + e->print(); + printf(" -> cmp = %d\n", cmp); +#endif + if (cmp >= 0) { + intersection = e; + break; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to find other intersection of p_face\n", n); +#endif + + if (cmp > 0) { + Vertex *removed = intersection->target; + e = intersection->reverse; + if (e->prev == e) { + removed->edges = nullptr; + } else { + removed->edges = e->prev; + e->prev->link(e->next); + e->link(e); + } +#ifdef DEBUG_CONVEX_HULL + printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + + Point64 n0 = intersection->face->get_normal(); + Point64 n1 = intersection->reverse->face->get_normal(); + int64_t m00 = p_face->dir0.dot(n0); + int64_t m01 = p_face->dir1.dot(n0); + int64_t m10 = p_face->dir0.dot(n1); + int64_t m11 = p_face->dir1.dot(n1); + int64_t r0 = (intersection->face->origin - shifted_origin).dot(n0); + int64_t r1 = (intersection->reverse->face->origin - shifted_origin).dot(n1); + Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); + CHULL_ASSERT(det.get_sign() != 0); + Vertex *v = vertex_pool.alloc(); + v->point.index = -1; + v->copy = -1; + v->point128 = PointR128(Int128::mul(p_face->dir0.x * r0, m11) - Int128::mul(p_face->dir0.x * r1, m01) + Int128::mul(p_face->dir1.x * r1, m00) - Int128::mul(p_face->dir1.x * r0, m10) + det * shifted_origin.x, + Int128::mul(p_face->dir0.y * r0, m11) - Int128::mul(p_face->dir0.y * r1, m01) + Int128::mul(p_face->dir1.y * r1, m00) - Int128::mul(p_face->dir1.y * r0, m10) + det * shifted_origin.y, + Int128::mul(p_face->dir0.z * r0, m11) - Int128::mul(p_face->dir0.z * r1, m01) + Int128::mul(p_face->dir1.z * r1, m00) - Int128::mul(p_face->dir1.z * r0, m10) + det * shifted_origin.z, + det); + v->point.x = (int32_t)v->point128.xvalue(); + v->point.y = (int32_t)v->point128.yvalue(); + v->point.z = (int32_t)v->point128.zvalue(); + intersection->target = v; + v->edges = e; + + p_stack.push_back(v); + p_stack.push_back(removed); + p_stack.push_back(nullptr); + } + + if (cmp || prev_cmp || (prev_intersection->reverse->next->target != intersection->target)) { + face_edge = new_edge_pair(prev_intersection->target, intersection->target); + if (prev_cmp == 0) { + face_edge->link(prev_intersection->reverse->next); + } + if ((prev_cmp == 0) || prev_face_edge) { + prev_intersection->reverse->link(face_edge); + } + if (cmp == 0) { + intersection->reverse->prev->link(face_edge->reverse); + } + face_edge->reverse->link(intersection->reverse); + } else { + face_edge = prev_intersection->reverse->next; + } + + if (prev_face_edge) { + if (prev_cmp > 0) { + face_edge->link(prev_face_edge->reverse); + } else if (face_edge != prev_face_edge->reverse) { + p_stack.push_back(prev_face_edge->target); + while (face_edge->next != prev_face_edge->reverse) { + Vertex *removed = face_edge->next->target; + remove_edge_pair(face_edge->next); + p_stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + p_stack.push_back(nullptr); + } + } + face_edge->face = p_face; + face_edge->reverse->face = intersection->face; + + if (!first_face_edge) { + first_face_edge = face_edge; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to process all intersections\n", m); +#endif + + if (cmp > 0) { + first_face_edge->reverse->target = face_edge->target; + first_intersection->reverse->link(first_face_edge); + first_face_edge->link(face_edge->reverse); + } else if (first_face_edge != face_edge->reverse) { + p_stack.push_back(face_edge->target); + while (first_face_edge->next != face_edge->reverse) { + Vertex *removed = first_face_edge->next->target; + remove_edge_pair(first_face_edge->next); + p_stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + p_stack.push_back(nullptr); + } + + CHULL_ASSERT(p_stack.size() > 0); + vertex_list = p_stack[0]; + +#ifdef DEBUG_CONVEX_HULL + printf("Removing part\n"); +#endif +#ifdef SHOW_ITERATIONS + n = 0; +#endif + uint32_t pos = 0; + while (pos < p_stack.size()) { + uint32_t end = p_stack.size(); + while (pos < end) { + Vertex *kept = p_stack[pos++]; +#ifdef DEBUG_CONVEX_HULL + kept->print(); +#endif + bool deeper = false; + Vertex *removed; + while ((removed = p_stack[pos++]) != nullptr) { +#ifdef SHOW_ITERATIONS + n++; +#endif + kept->receive_nearby_faces(removed); + while (removed->edges) { + if (!deeper) { + deeper = true; + p_stack.push_back(kept); + } + p_stack.push_back(removed->edges->target); + remove_edge_pair(removed->edges); + } + } + if (deeper) { + p_stack.push_back(nullptr); + } + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to remove part\n", n); +#endif + + p_stack.resize(0); + p_face->origin = shifted_origin; + + return true; +} + +static int32_t get_vertex_copy(ConvexHullInternal::Vertex *p_vertex, LocalVector &p_vertices) { + int32_t index = p_vertex->copy; + if (index < 0) { + index = p_vertices.size(); + p_vertex->copy = index; + p_vertices.push_back(p_vertex); +#ifdef DEBUG_CONVEX_HULL + printf("Vertex %d gets index *%d\n", p_vertex->point.index, index); +#endif + } + return index; +} + +real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp) { + if (p_count <= 0) { + vertices.clear(); + edges.clear(); + faces.clear(); + return 0; + } + + ConvexHullInternal hull; + hull.compute(p_coords, p_count); + + real_t shift = 0; + if ((p_shrink > 0) && ((shift = hull.shrink(p_shrink, p_shrink_clamp)) < 0)) { + vertices.clear(); + edges.clear(); + faces.clear(); + return shift; + } + + vertices.resize(0); + edges.resize(0); + faces.resize(0); + + LocalVector old_vertices; + get_vertex_copy(hull.vertex_list, old_vertices); + int32_t copied = 0; + while (copied < (int32_t)old_vertices.size()) { + ConvexHullInternal::Vertex *v = old_vertices[copied]; + vertices.push_back(hull.get_coordinates(v)); + ConvexHullInternal::Edge *first_edge = v->edges; + if (first_edge) { + int32_t first_copy = -1; + int32_t prev_copy = -1; + ConvexHullInternal::Edge *e = first_edge; + do { + if (e->copy < 0) { + int32_t s = edges.size(); + edges.push_back(Edge()); + edges.push_back(Edge()); + Edge *c = &edges[s]; + Edge *r = &edges[s + 1]; + e->copy = s; + e->reverse->copy = s + 1; + c->reverse = 1; + r->reverse = -1; + c->target_vertex = get_vertex_copy(e->target, old_vertices); + r->target_vertex = copied; +#ifdef DEBUG_CONVEX_HULL + printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->get_target_vertex()); +#endif + } + if (prev_copy >= 0) { + edges[e->copy].next = prev_copy - e->copy; + } else { + first_copy = e->copy; + } + prev_copy = e->copy; + e = e->next; + } while (e != first_edge); + edges[first_copy].next = prev_copy - first_copy; + } + copied++; + } + + for (int32_t i = 0; i < copied; i++) { + ConvexHullInternal::Vertex *v = old_vertices[i]; + ConvexHullInternal::Edge *first_edge = v->edges; + if (first_edge) { + ConvexHullInternal::Edge *e = first_edge; + do { + if (e->copy >= 0) { +#ifdef DEBUG_CONVEX_HULL + printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].get_target_vertex()); +#endif + faces.push_back(e->copy); + ConvexHullInternal::Edge *f = e; + do { +#ifdef DEBUG_CONVEX_HULL + printf(" Face *%d\n", edges[f->copy].get_target_vertex()); +#endif + f->copy = -1; + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != first_edge); + } + } + + return shift; +} + +Error ConvexHullComputer::convex_hull(const Vector &p_points, Geometry3D::MeshData &r_mesh) { + r_mesh = Geometry3D::MeshData(); // clear + + if (p_points.size() == 0) { + return FAILED; // matches QuickHull + } + + ConvexHullComputer ch; + ch.compute(p_points.ptr(), p_points.size(), -1.0, -1.0); + + r_mesh.vertices = ch.vertices; + + r_mesh.edges.resize(ch.edges.size()); + for (uint32_t i = 0; i < ch.edges.size(); i++) { + r_mesh.edges.write[i].a = (&ch.edges[i])->get_source_vertex(); + r_mesh.edges.write[i].b = (&ch.edges[i])->get_target_vertex(); + } + + r_mesh.faces.resize(ch.faces.size()); + for (uint32_t i = 0; i < ch.faces.size(); i++) { + const Edge *e_start = &ch.edges[ch.faces[i]]; + const Edge *e = e_start; + Geometry3D::MeshData::Face &face = r_mesh.faces.write[i]; + + do { + face.indices.push_back(e->get_target_vertex()); + + e = e->get_next_edge_of_face(); + } while (e != e_start); + + // compute normal + if (face.indices.size() >= 3) { + face.plane = Plane(r_mesh.vertices[face.indices[0]], r_mesh.vertices[face.indices[2]], r_mesh.vertices[face.indices[1]]); + } else { + WARN_PRINT("Too few vertices per face."); + } + } + + return OK; +} diff --git a/core/math/convex_hull.h b/core/math/convex_hull.h new file mode 100644 index 0000000000..ba7be9c5e8 --- /dev/null +++ b/core/math/convex_hull.h @@ -0,0 +1,112 @@ +/*************************************************************************/ +/* convex_hull.h */ +/*************************************************************************/ +/* 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. */ +/*************************************************************************/ + +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net +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. +*/ + +#ifndef CONVEX_HULL_H +#define CONVEX_HULL_H + +#include "core/math/geometry_3d.h" +#include "core/math/vector3.h" +#include "core/templates/local_vector.h" +#include "core/templates/vector.h" + +/// Convex hull implementation based on Preparata and Hong +/// See http://code.google.com/p/bullet/issues/detail?id=275 +/// Ole Kniemeyer, MAXON Computer GmbH +class ConvexHullComputer { +public: + class Edge { + private: + int32_t next = 0; + int32_t reverse = 0; + int32_t target_vertex = 0; + + friend class ConvexHullComputer; + + public: + int32_t get_source_vertex() const { + return (this + reverse)->target_vertex; + } + + int32_t get_target_vertex() const { + return target_vertex; + } + + const Edge *get_next_edge_of_vertex() const // clockwise list of all edges of a vertex + { + return this + next; + } + + const Edge *get_next_edge_of_face() const // counter-clockwise list of all edges of a face + { + return (this + reverse)->get_next_edge_of_vertex(); + } + + const Edge *get_reverse_edge() const { + return this + reverse; + } + }; + + // Vertices of the output hull + Vector vertices; + + // Edges of the output hull + LocalVector edges; + + // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons + LocalVector faces; + + /* + Compute convex hull of "count" vertices stored in "coords". + If "shrink" is positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length units + towards the center along its normal). + If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius" + is the minimum distance of a face to the center of the convex hull. + The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large + that the resulting convex hull is empty. + The output convex hull can be found in the member variables "vertices", "edges", "faces". + */ + real_t compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp); + + static Error convex_hull(const Vector &p_points, Geometry3D::MeshData &r_mesh); +}; + +#endif // CONVEX_HULL_H diff --git a/core/templates/paged_allocator.h b/core/templates/paged_allocator.h index 7002034710..481289309f 100644 --- a/core/templates/paged_allocator.h +++ b/core/templates/paged_allocator.h @@ -35,6 +35,8 @@ #include "core/os/spin_lock.h" #include "core/typedefs.h" +#include + template class PagedAllocator { T **page_pool = nullptr; @@ -89,8 +91,10 @@ public: allocs_available++; } - void reset() { - ERR_FAIL_COND(allocs_available < pages_allocated * page_size); + void reset(bool p_allow_unfreed = false) { + if (!p_allow_unfreed || !std::is_trivially_destructible::value) { + ERR_FAIL_COND(allocs_available < pages_allocated * page_size); + } if (pages_allocated) { for (uint32_t i = 0; i < pages_allocated; i++) { memfree(page_pool[i]); diff --git a/editor/node_3d_editor_gizmos.cpp b/editor/node_3d_editor_gizmos.cpp index afafd7d195..cc261ea868 100644 --- a/editor/node_3d_editor_gizmos.cpp +++ b/editor/node_3d_editor_gizmos.cpp @@ -30,9 +30,9 @@ #include "node_3d_editor_gizmos.h" +#include "core/math/convex_hull.h" #include "core/math/geometry_2d.h" #include "core/math/geometry_3d.h" -#include "core/math/quick_hull.h" #include "scene/3d/audio_stream_player_3d.h" #include "scene/3d/baked_lightmap.h" #include "scene/3d/collision_polygon_3d.h" @@ -4161,7 +4161,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { if (points.size() > 3) { Vector varr = Variant(points); Geometry3D::MeshData md; - Error err = QuickHull::build(varr, md); + Error err = ConvexHullComputer::convex_hull(varr, md); if (err == OK) { Vector points2; points2.resize(md.edges.size() * 2); diff --git a/modules/gdnavigation/navigation_mesh_generator.cpp b/modules/gdnavigation/navigation_mesh_generator.cpp index a7d4e79148..7d30ce0f44 100644 --- a/modules/gdnavigation/navigation_mesh_generator.cpp +++ b/modules/gdnavigation/navigation_mesh_generator.cpp @@ -32,7 +32,7 @@ #include "navigation_mesh_generator.h" -#include "core/math/quick_hull.h" +#include "core/math/convex_hull.h" #include "core/os/thread.h" #include "scene/3d/collision_shape_3d.h" #include "scene/3d/mesh_instance_3d.h" @@ -220,7 +220,7 @@ void NavigationMeshGenerator::_parse_geometry(Transform p_accumulated_transform, Vector varr = Variant(convex_polygon->get_points()); Geometry3D::MeshData md; - Error err = QuickHull::build(varr, md); + Error err = ConvexHullComputer::convex_hull(varr, md); if (err == OK) { PackedVector3Array faces; diff --git a/scene/resources/convex_polygon_shape_3d.cpp b/scene/resources/convex_polygon_shape_3d.cpp index 9e030bc077..6b895da606 100644 --- a/scene/resources/convex_polygon_shape_3d.cpp +++ b/scene/resources/convex_polygon_shape_3d.cpp @@ -29,7 +29,7 @@ /*************************************************************************/ #include "convex_polygon_shape_3d.h" -#include "core/math/quick_hull.h" +#include "core/math/convex_hull.h" #include "servers/physics_server_3d.h" Vector ConvexPolygonShape3D::get_debug_mesh_lines() const { @@ -38,7 +38,7 @@ Vector ConvexPolygonShape3D::get_debug_mesh_lines() const { if (points.size() > 3) { Vector varr = Variant(points); Geometry3D::MeshData md; - Error err = QuickHull::build(varr, md); + Error err = ConvexHullComputer::convex_hull(varr, md); if (err == OK) { Vector lines; lines.resize(md.edges.size() * 2); diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index ccd37ca742..ca7248993f 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -31,8 +31,8 @@ #include "shape_3d_sw.h" #include "core/io/image.h" +#include "core/math/convex_hull.h" #include "core/math/geometry_3d.h" -#include "core/math/quick_hull.h" #include "core/templates/sort_array.h" // HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape. @@ -1089,9 +1089,9 @@ Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { } void ConvexPolygonShape3DSW::_setup(const Vector &p_vertices) { - Error err = QuickHull::build(p_vertices, mesh); + Error err = ConvexHullComputer::convex_hull(p_vertices, mesh); if (err != OK) { - ERR_PRINT("Failed to build QuickHull"); + ERR_PRINT("Failed to build convex hull"); } AABB _aabb; diff --git a/tests/test_physics_3d.cpp b/tests/test_physics_3d.cpp index ac8078a0a8..727c44b3ac 100644 --- a/tests/test_physics_3d.cpp +++ b/tests/test_physics_3d.cpp @@ -30,8 +30,8 @@ #include "test_physics_3d.h" +#include "core/math/convex_hull.h" #include "core/math/math_funcs.h" -#include "core/math/quick_hull.h" #include "core/os/main_loop.h" #include "core/os/os.h" #include "core/string/print_string.h" @@ -169,7 +169,7 @@ protected: RID convex_mesh = vs->mesh_create(); Geometry3D::MeshData convex_data = Geometry3D::build_convex_mesh(convex_planes); - QuickHull::build(convex_data.vertices, convex_data); + ConvexHullComputer::convex_hull(convex_data.vertices, convex_data); vs->mesh_add_surface_from_mesh_data(convex_mesh, convex_data); type_mesh_map[PhysicsServer3D::SHAPE_CONVEX_POLYGON] = convex_mesh; diff --git a/tests/test_render.cpp b/tests/test_render.cpp index 72b2840098..9737fd03f3 100644 --- a/tests/test_render.cpp +++ b/tests/test_render.cpp @@ -30,8 +30,8 @@ #include "test_render.h" +#include "core/math/convex_hull.h" #include "core/math/math_funcs.h" -#include "core/math/quick_hull.h" #include "core/os/keyboard.h" #include "core/os/main_loop.h" #include "core/os/os.h" @@ -118,7 +118,7 @@ public: vts.push_back(Vector3(-1, -1, -1)); Geometry3D::MeshData md; - Error err = QuickHull::build(vts, md); + Error err = ConvexHullComputer::convex_hull(vts, md); print_line("ERR: " + itos(err)); test_cube = vs->mesh_create(); vs->mesh_add_surface_from_mesh_data(test_cube, md);