You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
571 lines
13 KiB
571 lines
13 KiB
extends Reference
|
|
|
|
class_name Delaunator
|
|
|
|
const EPSILON = pow(2, -52)
|
|
const EDGE_STACK = []
|
|
|
|
var coords = [] # PoolRealArray.
|
|
var halfedges = [] # PoolIntArray.
|
|
var hull = [] # PoolIntArray.
|
|
var triangles = [] # PoolIntArray.
|
|
var triangles_len = 0
|
|
var _cx
|
|
var _cy
|
|
var _dists = [] # PoolRealArray.
|
|
var _halfedges = [] # This array should be a PoolIntArray but we need to use the .slice() function on it.
|
|
var _hash_size
|
|
var _hull_hash = [] # PoolIntArray.
|
|
var _hull_next = [] # PoolIntArray.
|
|
var _hull_prev = [] # PoolIntArray.
|
|
var _hull_start
|
|
var _hull_tri = [] # PoolIntArray.
|
|
var _ids = [] # PoolIntArray.
|
|
var _triangles = [] # This array should be a PoolIntArray but we need to use the .slice() function on it.
|
|
|
|
func _init(points):
|
|
if points.size() < 3:
|
|
push_error(ProjectSettings.get_setting("application/config/name") + " needs at least 3 points.")
|
|
return
|
|
|
|
EDGE_STACK.resize(512)
|
|
|
|
var n = points.size()
|
|
|
|
coords.resize(n * 2)
|
|
|
|
for i in n:
|
|
var point = points[i]
|
|
coords[2 * i] = point.x
|
|
coords[2 * i + 1] = point.z
|
|
|
|
_constructor()
|
|
|
|
func _constructor():
|
|
var n = coords.size() >> 1
|
|
|
|
# Arrays that will store the triangulation graph.
|
|
var max_triangles = max(2 * n - 5, 0)
|
|
_triangles.resize(max_triangles * 3)
|
|
_halfedges.resize(max_triangles * 3)
|
|
|
|
# Temporary arrays for tracking the edges of the advancing convex hull.
|
|
_hash_size = ceil(sqrt(n))
|
|
_hull_prev.resize(n) # Edge to prev edge.
|
|
_hull_next.resize(n) # Edge to next edge.
|
|
_hull_tri.resize(n) # Edge to adjacent triangle.
|
|
|
|
_hull_hash.resize(_hash_size)
|
|
for i in _hash_size:
|
|
_hull_hash[i] = -1 # angular edge hash
|
|
|
|
# Temporary arrays for sorting points.
|
|
_ids.resize(n)
|
|
_dists.resize(n)
|
|
|
|
update()
|
|
|
|
|
|
func update():
|
|
var n = coords.size() >> 1
|
|
|
|
# Populate an array of point indices; calculate input data bbox.
|
|
var min_x = INF
|
|
var min_y = INF
|
|
var max_x = -INF
|
|
var max_y = -INF
|
|
|
|
for i in n:
|
|
var x = coords[2 * i]
|
|
var y = coords[2 * i + 1]
|
|
if x < min_x: min_x = x
|
|
if y < min_y: min_y = y
|
|
if x > max_x: max_x = x
|
|
if y > max_y: max_y = y
|
|
_ids[i] = i
|
|
|
|
var cx = (min_x + max_x) / 2
|
|
var cy = (min_y + max_y) / 2
|
|
|
|
var min_dist = INF
|
|
var i0 = 0
|
|
var i1 = 0
|
|
var i2 = 0
|
|
|
|
# Pick a seed point close to the center.
|
|
for i in n:
|
|
var d = dist(cx, cy, coords[2 * i], coords[2 * i + 1])
|
|
if (d < min_dist):
|
|
i0 = i
|
|
min_dist = d
|
|
var i0x = coords[2 * i0]
|
|
var i0y = coords[2 * i0 + 1]
|
|
|
|
min_dist = INF
|
|
|
|
# Find the point closest to the seed.
|
|
for i in n:
|
|
if i == i0: continue
|
|
var d = dist(i0x, i0y, coords[2 * i], coords[2 * i + 1])
|
|
if (d < min_dist and d > 0):
|
|
i1 = i
|
|
min_dist = d
|
|
var i1x = coords[2 * i1]
|
|
var i1y = coords[2 * i1 + 1]
|
|
|
|
var min_radius = INF
|
|
|
|
# Find the third point which forms the smallest circumcircle with the first two.
|
|
for i in n:
|
|
if i == i0 or i == i1: continue
|
|
var r = circumradius(i0x, i0y, i1x, i1y, coords[2 * i], coords[2 * i + 1])
|
|
if r < min_radius:
|
|
i2 = i
|
|
min_radius = r
|
|
var i2x = coords[2 * i2]
|
|
var i2y = coords[2 * i2 + 1]
|
|
|
|
if min_radius == INF:
|
|
# Order collinear points by dx (or dy if all x are identical)
|
|
# and return the list as a hull.
|
|
for i in n:
|
|
var _dist_temp
|
|
|
|
if coords[2 * i] - coords[0]:
|
|
_dist_temp = coords[2 * i] - coords[0]
|
|
elif coords[2 * i + 1] - coords[1]:
|
|
_dist_temp = coords[2 * i + 1] - coords[1]
|
|
else:
|
|
_dist_temp = 0
|
|
|
|
_dists[i] = _dist_temp
|
|
|
|
quicksort(_ids, _dists, 0, n - 1)
|
|
var hull = []
|
|
hull.resize(n)
|
|
var j = 0
|
|
var d0 = -INF
|
|
|
|
for i in n:
|
|
var id = _ids[i]
|
|
if _dists[id] > d0:
|
|
hull[j] = id
|
|
j += 1
|
|
d0 = _dists[id]
|
|
hull = hull.slice(0, j - 1)
|
|
triangles = []
|
|
halfedges = []
|
|
|
|
return
|
|
|
|
# Swap the order of the seed points for counter-clockwise orientation.
|
|
if orient(i0x, i0y, i1x, i1y, i2x, i2y):
|
|
var i = i1
|
|
var x = i1x
|
|
var y = i1y
|
|
i1 = i2
|
|
i1x = i2x
|
|
i1y = i2y
|
|
i2 = i
|
|
i2x = x
|
|
i2y = y
|
|
|
|
var center = circumcenter(i0x, i0y, i1x, i1y, i2x, i2y)
|
|
_cx = center[0]
|
|
_cy = center[1]
|
|
|
|
for i in n:
|
|
_dists[i] = dist(coords[2 * i], coords[2 * i + 1], center[0], center[1])
|
|
|
|
# Sort the points by distance from the seed triangle circumcenter.
|
|
quicksort(_ids, _dists, 0, n - 1)
|
|
|
|
# Set up the seed triangle as the starting hull.
|
|
_hull_start = i0
|
|
var hull_size = 3
|
|
|
|
_hull_next[i0] = i1
|
|
_hull_prev[i2] = i1
|
|
_hull_next[i1] = i2
|
|
_hull_prev[i0] = i2
|
|
_hull_next[i2] = i0
|
|
_hull_prev[i1] = i0
|
|
|
|
_hull_tri[i0] = 0
|
|
_hull_tri[i1] = 1
|
|
_hull_tri[i2] = 2
|
|
|
|
for i in _hull_hash.size():
|
|
_hull_hash[i] = -1
|
|
_hull_hash[_hash_key(i0x, i0y)] = i0
|
|
_hull_hash[_hash_key(i1x, i1y)] = i1
|
|
_hull_hash[_hash_key(i2x, i2y)] = i2
|
|
|
|
# triangles_len = 0
|
|
_add_triangle(i0, i1, i2, -1, -1, -1)
|
|
|
|
var xp = 0
|
|
var yp = 0
|
|
|
|
for k in _ids.size():
|
|
var i = _ids[k]
|
|
var x = coords[2 * i]
|
|
var y = coords[2 * i + 1]
|
|
|
|
# Skip near-duplicate points.
|
|
if k > 0 and abs(x - xp) <= EPSILON and abs(y - yp) <= EPSILON: continue
|
|
|
|
xp = x
|
|
yp = y
|
|
|
|
# Skip seed triangle points.
|
|
if i == i0 or i == i1 or i == i2: continue
|
|
|
|
# Find a visible edge on the convex hull using edge hash.
|
|
var start = 0
|
|
var key = _hash_key(x, y)
|
|
|
|
for j in _hash_size:
|
|
start = _hull_hash[fmod((key + j), _hash_size)]
|
|
if (start != -1 and start != _hull_next[start]): break
|
|
|
|
start = _hull_prev[start]
|
|
var e = start
|
|
|
|
while true:
|
|
var q = _hull_next[e]
|
|
if orient(x, y, coords[2 * e], coords[2 * e + 1], coords[2 * q], coords[2 * q + 1]): break
|
|
e = q
|
|
|
|
if (e == start):
|
|
e = -1
|
|
break
|
|
|
|
if (e == -1): continue # Likely a near-duplicate point; Skip it.
|
|
|
|
# Add the first triangle from the point.
|
|
var t = _add_triangle(e, i, _hull_next[e], -1, -1, _hull_tri[e])
|
|
# Recursively flip triangles from the point until they satisfy the Delaunay condition.
|
|
_hull_tri[i] = _legalize(t + 2)
|
|
_hull_tri[e] = t # Keep track of boundary triangles on the hull.
|
|
hull_size += 1
|
|
|
|
# Walk forward through the hull, adding more triangles and flipping recursively.
|
|
n = _hull_next[e]
|
|
|
|
while true:
|
|
var q = _hull_next[n]
|
|
if not orient(x, y, coords[2 * n], coords[2 * n + 1], coords[2 * q], coords[2 * q + 1]): break
|
|
t = _add_triangle(n, i, q, _hull_tri[i], -1, _hull_tri[n])
|
|
_hull_tri[i] = _legalize(t + 2)
|
|
_hull_next[n] = n # Mark as removed.
|
|
hull_size -= 1
|
|
n = q
|
|
|
|
# Walk backward from the other side, adding more triangles and flipping.
|
|
if (e == start):
|
|
while true:
|
|
var q = _hull_prev[e]
|
|
if not orient(x, y, coords[2 * q], coords[2 * q + 1], coords[2 * e], coords[2 * e + 1]): break
|
|
t = _add_triangle(q, i, e, -1, _hull_tri[e], _hull_tri[q])
|
|
_legalize(t + 2)
|
|
_hull_tri[q] = t
|
|
_hull_next[e] = e # Mark as removed.
|
|
hull_size -= 1
|
|
e = q
|
|
|
|
# Update the hull indices.
|
|
_hull_start = e
|
|
_hull_prev[i] = e
|
|
_hull_next[e] = i
|
|
_hull_prev[n] = i
|
|
_hull_next[i] = n
|
|
|
|
# Save the two new edges in the hash table.
|
|
_hull_hash[_hash_key(x, y)] = i
|
|
_hull_hash[_hash_key(coords[2 * e], coords[2 * e + 1])] = e
|
|
|
|
hull.resize(hull_size)
|
|
var e = _hull_start
|
|
for i in hull_size:
|
|
hull[i] = e
|
|
e = _hull_next[e]
|
|
|
|
# Trim typed triangle mesh arrays.
|
|
triangles = _triangles.slice(0, triangles_len - 1)
|
|
halfedges = _halfedges.slice(0, triangles_len - 1)
|
|
|
|
|
|
func _hash_key(x, y):
|
|
return fmod(floor(pseudo_angle(x - _cx, y - _cy) * _hash_size), _hash_size)
|
|
|
|
|
|
func _legalize(a):
|
|
var i = 0
|
|
var ar = 0
|
|
|
|
# Recursion eliminated with a fixed-size stack.
|
|
while true:
|
|
var b = _halfedges[a]
|
|
|
|
# If the pair of triangles doesn't satisfy the Delaunay condition
|
|
# (p1 is inside the circumcircle of [p0, pl, pr]), flip them,
|
|
# then do the same check/flip recursively for the new pair of triangles
|
|
#
|
|
# pl pl
|
|
# /||\ / \
|
|
# al/ || \bl al/ \a
|
|
# / || \ / \
|
|
# / a||b \ flip /___ar___\
|
|
# p0\ || /p1 => p0\---bl---/p1
|
|
# \ || / \ /
|
|
# ar\ || /br b\ /br
|
|
# \||/ \ /
|
|
# pr pr
|
|
|
|
var a0 = a - a % 3
|
|
ar = a0 + (a + 2) % 3
|
|
|
|
if b == -1: # Convex hull edge.
|
|
if i == 0: break
|
|
i -= 1
|
|
a = EDGE_STACK[i]
|
|
continue
|
|
|
|
var b0 = b - b % 3
|
|
var al = a0 + (a + 1) % 3
|
|
var bl = b0 + (b + 2) % 3
|
|
|
|
var p0 = _triangles[ar]
|
|
var pr = _triangles[a]
|
|
var pl = _triangles[al]
|
|
var p1 = _triangles[bl]
|
|
|
|
var illegal = in_circle(
|
|
coords[2 * p0], coords[2 * p0 + 1],
|
|
coords[2 * pr], coords[2 * pr + 1],
|
|
coords[2 * pl], coords[2 * pl + 1],
|
|
coords[2 * p1], coords[2 * p1 + 1]
|
|
)
|
|
|
|
if illegal:
|
|
_triangles[a] = p1
|
|
_triangles[b] = p0
|
|
|
|
var hbl = _halfedges[bl]
|
|
|
|
# Edge swapped on the other side of the hull (rare); Fix the halfedge reference.
|
|
if (hbl == -1):
|
|
var e = _hull_start
|
|
while true:
|
|
if _hull_tri[e] == bl:
|
|
_hull_tri[e] = a
|
|
break
|
|
|
|
e = _hull_prev[e]
|
|
if e == _hull_start: break
|
|
|
|
_link(a, hbl)
|
|
_link(b, _halfedges[ar])
|
|
_link(ar, bl)
|
|
|
|
var br = b0 + (b + 1) % 3
|
|
|
|
# Don't worry about hitting the cap: it can only happen on extremely degenerate input.
|
|
if i < EDGE_STACK.size():
|
|
EDGE_STACK[i] = br
|
|
i += 1
|
|
else:
|
|
if i == 0: break
|
|
i -= 1
|
|
a = EDGE_STACK[i]
|
|
|
|
return ar
|
|
|
|
|
|
func _link(a, b):
|
|
_halfedges[a] = b
|
|
if (b != -1):
|
|
_halfedges[b] = a
|
|
|
|
|
|
# Add a new triangle given vertex indices and adjacent half-edge ids.
|
|
func _add_triangle(i0, i1, i2, a, b, c):
|
|
var t = triangles_len
|
|
|
|
_triangles[t] = i0
|
|
_triangles[t + 1] = i1
|
|
_triangles[t + 2] = i2
|
|
|
|
_link(t, a)
|
|
_link(t + 1, b)
|
|
_link(t + 2, c)
|
|
|
|
triangles_len += 3
|
|
|
|
return t
|
|
|
|
|
|
# Monotonically increases with real angle, but doesn't need expensive trigonometry.
|
|
func pseudo_angle(dx, dy):
|
|
var p = dx / (abs(dx) + abs(dy))
|
|
|
|
if (dy > 0):
|
|
return (3 - p) / 4 # [0..1]
|
|
else:
|
|
return (1 + p) / 4 # [0..1]
|
|
|
|
|
|
func dist(ax, ay, bx, by):
|
|
var dx = ax - bx
|
|
var dy = ay - by
|
|
return dx * dx + dy * dy
|
|
|
|
|
|
# Return 2d orientation sign if we're confident in it through J. Shewchuk's error bound check.
|
|
func orient_if_sure(px, py, rx, ry, qx, qy):
|
|
var l = (ry - py) * (qx - px)
|
|
var r = (rx - px) * (qy - py)
|
|
|
|
if (abs(l - r) >= 0.00000000000000033306690738754716 * abs(l + r)):
|
|
return l - r
|
|
else:
|
|
return 0
|
|
|
|
|
|
# A more robust orientation test that's stable in a given triangle (to fix robustness issues).
|
|
func orient(rx, ry, qx, qy, px, py):
|
|
var _sign
|
|
|
|
if orient_if_sure(px, py, rx, ry, qx, qy):
|
|
_sign = orient_if_sure(px, py, rx, ry, qx, qy)
|
|
elif orient_if_sure(rx, ry, qx, qy, px, py):
|
|
_sign = orient_if_sure(rx, ry, qx, qy, px, py)
|
|
elif orient_if_sure(qx, qy, px, py, rx, ry):
|
|
_sign = orient_if_sure(qx, qy, px, py, rx, ry)
|
|
|
|
return false if _sign == null else _sign < 0
|
|
|
|
|
|
func in_circle(ax, ay, bx, by, cx, cy, px, py):
|
|
var dx = ax - px
|
|
var dy = ay - py
|
|
var ex = bx - px
|
|
var ey = by - py
|
|
var fx = cx - px
|
|
var fy = cy - py
|
|
|
|
var ap = dx * dx + dy * dy
|
|
var bp = ex * ex + ey * ey
|
|
var cp = fx * fx + fy * fy
|
|
|
|
return dx * (ey * cp - bp * fy) -\
|
|
dy * (ex * cp - bp * fx) +\
|
|
ap * (ex * fy - ey * fx) < 0
|
|
|
|
|
|
func circumradius(ax, ay, bx, by, cx, cy):
|
|
var dx = bx - ax
|
|
var dy = by - ay
|
|
var ex = cx - ax
|
|
var ey = cy - ay
|
|
|
|
var bl = dx * dx + dy * dy
|
|
var cl = ex * ex + ey * ey
|
|
|
|
# When you divide by 0 in Godot you get an error.
|
|
# It should return INF (positive or negative).
|
|
var d
|
|
if (dx * ey - dy * ex) == 0:
|
|
d = INF
|
|
elif (dx * ey - dy * ex) == -0:
|
|
d = -INF
|
|
else:
|
|
d = 0.5 / (dx * ey - dy * ex)
|
|
|
|
var x = (ey * bl - dy * cl) * d
|
|
var y = (dx * cl - ex * bl) * d
|
|
|
|
return x * x + y * y
|
|
|
|
|
|
func circumcenter(ax, ay, bx, by, cx, cy):
|
|
var dx = bx - ax
|
|
var dy = by - ay
|
|
var ex = cx - ax
|
|
var ey = cy - ay
|
|
|
|
var bl = dx * dx + dy * dy
|
|
var cl = ex * ex + ey * ey
|
|
|
|
# When you divide by 0 in Godot you get an error.
|
|
# It should return INF (positive or negative).
|
|
var d
|
|
if (dx * ey - dy * ex) == 0:
|
|
d = INF
|
|
elif (dx * ey - dy * ex) == -0:
|
|
d = -INF
|
|
else:
|
|
d = 0.5 / (dx * ey - dy * ex)
|
|
|
|
var x = ax + (ey * bl - dy * cl) * d
|
|
var y = ay + (dx * cl - ex * bl) * d
|
|
|
|
return [x, y]
|
|
|
|
|
|
func quicksort(ids, dists, left, right):
|
|
if right - left <= 20:
|
|
for i in range(left + 1, right + 1):
|
|
var temp = ids[i]
|
|
var temp_dist = dists[temp]
|
|
var j = i - 1
|
|
while j >= left and dists[ids[j]] > temp_dist:
|
|
ids[j + 1] = ids[j]
|
|
j -= 1
|
|
ids[j + 1] = temp
|
|
else:
|
|
var median = (left + right) >> 1
|
|
var i = left + 1
|
|
var j = right
|
|
swap(ids, median, i)
|
|
|
|
if (dists[ids[left]] > dists[ids[right]]):
|
|
swap(ids, left, right)
|
|
|
|
if (dists[ids[i]] > dists[ids[right]]):
|
|
swap(ids, i, right)
|
|
|
|
if (dists[ids[left]] > dists[ids[i]]):
|
|
swap(ids, left, i)
|
|
|
|
var temp = ids[i]
|
|
var temp_dist = dists[temp]
|
|
|
|
while true:
|
|
while true:
|
|
i += 1
|
|
if dists[ids[i]] >= temp_dist: break
|
|
|
|
while true:
|
|
j -= 1
|
|
if dists[ids[j]] <= temp_dist: break
|
|
|
|
if j < i: break
|
|
swap(ids, i, j)
|
|
|
|
ids[left + 1] = ids[j]
|
|
ids[j] = temp
|
|
|
|
if right - i + 1 >= j - left:
|
|
quicksort(ids, dists, i, right)
|
|
quicksort(ids, dists, left, j - 1)
|
|
else:
|
|
quicksort(ids, dists, left, j - 1)
|
|
quicksort(ids, dists, i, right)
|
|
|
|
|
|
func swap(arr, i, j):
|
|
var tmp = arr[i]
|
|
arr[i] = arr[j]
|
|
arr[j] = tmp
|
|
|