934 lines
28 KiB
GDScript
934 lines
28 KiB
GDScript
# MIT License
|
|
#
|
|
# Copyright (c) 2023 Mark McKay
|
|
# https://github.com/blackears/cyclopsLevelBuilder
|
|
#
|
|
# 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.
|
|
|
|
@tool
|
|
class_name MathUtil
|
|
|
|
enum Axis { X, Y, Z }
|
|
|
|
static func square(value:float)->float:
|
|
return value * value
|
|
|
|
static func snap_to_grid(pos:Vector3, cell_size:float)->Vector3:
|
|
# return floor(pos / cell_size) * cell_size
|
|
return floor((pos + Vector3(cell_size, cell_size, cell_size) / 2) / cell_size) * cell_size
|
|
|
|
|
|
#Returns intersection of line with point.
|
|
# plane_perp_dir points in direction of plane's normal and does not need to be normalized
|
|
static func intersect_plane(ray_origin:Vector3, ray_dir:Vector3, plane_origin:Vector3, plane_perp_dir:Vector3)->Vector3:
|
|
var s:float = (plane_origin - ray_origin).dot(plane_perp_dir) / ray_dir.dot(plane_perp_dir)
|
|
return ray_origin + ray_dir * s
|
|
|
|
static func intersects_triangle(ray_origin:Vector3, ray_dir:Vector3, p0:Vector3, p1:Vector3, p2:Vector3)->bool:
|
|
#Godot uses clockwise winding
|
|
var tri_area_x2:Vector3 = MathUtil.triangle_area_x2(p0, p1, p2)
|
|
|
|
var p_hit:Vector3 = MathUtil.intersect_plane(ray_origin, ray_dir, p0, tri_area_x2)
|
|
if !p_hit.is_finite():
|
|
return false
|
|
|
|
if MathUtil.triangle_area_x2(p_hit, p0, p1).dot(tri_area_x2) < 0:
|
|
return false
|
|
if MathUtil.triangle_area_x2(p_hit, p1, p2).dot(tri_area_x2) < 0:
|
|
return false
|
|
if MathUtil.triangle_area_x2(p_hit, p2, p0).dot(tri_area_x2) < 0:
|
|
return false
|
|
|
|
return true
|
|
|
|
class IntersectTriangleResult:
|
|
var position:Vector3
|
|
var normal:Vector3
|
|
|
|
static func intersect_triangle(ray_origin:Vector3, ray_dir:Vector3, p0:Vector3, p1:Vector3, p2:Vector3)->IntersectTriangleResult:
|
|
#Godot uses clockwise winding
|
|
var tri_area_x2:Vector3 = MathUtil.triangle_area_x2(p0, p1, p2)
|
|
|
|
var p_hit:Vector3 = MathUtil.intersect_plane(ray_origin, ray_dir, p0, tri_area_x2)
|
|
if !p_hit.is_finite():
|
|
return null
|
|
|
|
if MathUtil.triangle_area_x2(p_hit, p0, p1).dot(tri_area_x2) < 0:
|
|
return null
|
|
if MathUtil.triangle_area_x2(p_hit, p1, p2).dot(tri_area_x2) < 0:
|
|
return null
|
|
if MathUtil.triangle_area_x2(p_hit, p2, p0).dot(tri_area_x2) < 0:
|
|
return null
|
|
|
|
var result:IntersectTriangleResult = IntersectTriangleResult.new()
|
|
result.position = p_hit
|
|
result.normal = tri_area_x2.normalized()
|
|
return result
|
|
|
|
#Returns the closest point on the line to the ray
|
|
static func closest_point_on_line(ray_origin:Vector3, ray_dir:Vector3, line_origin:Vector3, line_dir:Vector3)->Vector3:
|
|
var a:Vector3 = ray_dir.cross(line_dir)
|
|
var w_perp:Vector3 = ray_dir.cross(a)
|
|
return intersect_plane(line_origin, line_dir, ray_origin, w_perp)
|
|
|
|
static func closest_point_on_plane(point:Vector3, plane_origin:Vector3, plane_dir:Vector3)->Vector3:
|
|
return point - (point - plane_origin).project(plane_dir)
|
|
|
|
static func closest_point_on_segment(ray_origin:Vector3, ray_dir:Vector3, seg_start:Vector3, seg_end:Vector3)->Vector3:
|
|
var seg_span:Vector3 = seg_end - seg_start
|
|
var p:Vector3 = closest_point_on_line(ray_origin, ray_dir, seg_start, seg_span)
|
|
var offset:Vector3 = p - seg_start
|
|
if offset.dot(seg_span) < 0:
|
|
return seg_start
|
|
if offset.length_squared() > seg_span.length_squared():
|
|
return seg_end
|
|
return p
|
|
|
|
#Shortest distance from point to given ray. Returns NAN if point is behind origin of ray.
|
|
static func distance_to_ray(ray_origin:Vector3, ray_dir:Vector3, point:Vector3):
|
|
var offset = point - ray_origin
|
|
var parallel:Vector3 = offset.project(ray_dir)
|
|
if parallel.dot(ray_dir) < 0:
|
|
return NAN
|
|
|
|
var perp:Vector3 = offset - parallel
|
|
return perp.length()
|
|
|
|
|
|
static func trianglate_face(points:PackedVector3Array, normal:Vector3)->PackedVector3Array:
|
|
var result:PackedVector3Array
|
|
|
|
while (points.size() >= 3):
|
|
var num_points:int = points.size()
|
|
for i in range(0, num_points):
|
|
var p0:Vector3 = points[i]
|
|
var p1:Vector3 = points[wrap(i + 1, 0, num_points)]
|
|
var p2:Vector3 = points[wrap(i + 2, 0, num_points)]
|
|
|
|
#Godot uses clockwise winding
|
|
var tri_norm_dir:Vector3 = (p2 - p0).cross(p1 - p0)
|
|
if tri_norm_dir.dot(normal) > 0:
|
|
result.append(p0)
|
|
result.append(p1)
|
|
result.append(p2)
|
|
|
|
points.remove_at(i + 1)
|
|
break
|
|
|
|
return result
|
|
|
|
static func trianglate_face_indices(points:PackedVector3Array, indices:Array[int], normal:Vector3)->Array[int]:
|
|
var result:Array[int] = []
|
|
|
|
# print("trianglate_face_indices %s" % points)
|
|
|
|
while (points.size() >= 3):
|
|
var num_points:int = points.size()
|
|
var added_point:bool = false
|
|
|
|
for i in range(0, num_points):
|
|
var idx0:int = i
|
|
var idx1:int = wrap(i + 1, 0, num_points)
|
|
var idx2:int = wrap(i + 2, 0, num_points)
|
|
var p0:Vector3 = points[idx0]
|
|
var p1:Vector3 = points[idx1]
|
|
var p2:Vector3 = points[idx2]
|
|
|
|
#Godot uses clockwise winding
|
|
var tri_norm_dir:Vector3 = (p2 - p0).cross(p1 - p0)
|
|
if tri_norm_dir.dot(normal) > 0:
|
|
result.append(indices[idx0])
|
|
result.append(indices[idx1])
|
|
result.append(indices[idx2])
|
|
|
|
# print("adding indices %s %s %s" % [indices[idx0], indices[idx1], indices[idx2]])
|
|
|
|
points.remove_at(idx1)
|
|
indices.remove_at(idx1)
|
|
added_point = true
|
|
break
|
|
|
|
assert(added_point, "failed to add point in triangulation")
|
|
# print("tri_done %s" % str(result))
|
|
|
|
return result
|
|
|
|
static func trianglate_face_vertex_indices(points:PackedVector3Array, normal:Vector3)->Array[int]:
|
|
var result:Array[int] = []
|
|
var fv_indices:Array = range(0, points.size())
|
|
# print("trianglate_face_indices %s" % points)
|
|
|
|
while (points.size() >= 3):
|
|
var num_points:int = points.size()
|
|
var added_point:bool = false
|
|
|
|
for i in range(0, num_points):
|
|
var idx0:int = i
|
|
var idx1:int = wrap(i + 1, 0, num_points)
|
|
var idx2:int = wrap(i + 2, 0, num_points)
|
|
var p0:Vector3 = points[idx0]
|
|
var p1:Vector3 = points[idx1]
|
|
var p2:Vector3 = points[idx2]
|
|
|
|
#Godot uses clockwise winding
|
|
var tri_norm_dir:Vector3 = (p2 - p0).cross(p1 - p0)
|
|
if tri_norm_dir.dot(normal) > 0:
|
|
result.append(fv_indices[idx0])
|
|
result.append(fv_indices[idx1])
|
|
result.append(fv_indices[idx2])
|
|
|
|
# print("adding indices %s %s %s" % [indices[idx0], indices[idx1], indices[idx2]])
|
|
|
|
points.remove_at(idx1)
|
|
fv_indices.remove_at(idx1)
|
|
added_point = true
|
|
break
|
|
|
|
assert(added_point, "failed to add point in triangulation")
|
|
# print("tri_done %s" % str(result))
|
|
|
|
return result
|
|
|
|
static func flip_plane(plane:Plane)->Plane:
|
|
return Plane(-plane.normal, plane.get_center())
|
|
|
|
#Returns a vector pointing along the normal in the clockwise winding direction with a length equal to twice the area of the triangle
|
|
static func triangle_area_x2(p0:Vector3, p1:Vector3, p2:Vector3)->Vector3:
|
|
return (p2 - p0).cross(p1 - p0)
|
|
|
|
#Returns a vector pointing along the normal in the clockwise winding direction with a lengh equal to twice the area of the face
|
|
static func face_area_x2(points:PackedVector3Array)->Vector3:
|
|
if points.size() <= 1:
|
|
return Vector3.ZERO
|
|
|
|
var result:Vector3
|
|
var p0:Vector3 = points[0]
|
|
|
|
for i in range(1, points.size() - 1):
|
|
var p1:Vector3 = points[i]
|
|
var p2:Vector3 = points[i + 1]
|
|
|
|
result += (p2 - p0).cross(p1 - p0)
|
|
|
|
return result
|
|
|
|
static func face_area_x2_2d(points:PackedVector2Array)->float:
|
|
if points.size() <= 1:
|
|
return 0
|
|
|
|
var result:float
|
|
var p0:Vector2 = points[0]
|
|
|
|
for i in range(1, points.size() - 1):
|
|
var p1:Vector2 = points[i]
|
|
var p2:Vector2 = points[i + 1]
|
|
|
|
result += triange_area_2x_2d(p1 - p0, p2 - p0)
|
|
|
|
return result
|
|
|
|
static func fit_plane(points:PackedVector3Array)->Plane:
|
|
var normal:Vector3 = face_area_x2(points).normalized()
|
|
return Plane(normal, points[0])
|
|
|
|
static func snap_to_best_axis_normal(vector:Vector3)->Vector3:
|
|
if abs(vector.x) > abs(vector.y) and abs(vector.x) > abs(vector.z):
|
|
return Vector3(1, 0, 0) if vector.x > 0 else Vector3(-1, 0, 0)
|
|
elif abs(vector.y) > abs(vector.z):
|
|
return Vector3(0, 1, 0) if vector.y > 0 else Vector3(0, -1, 0)
|
|
else:
|
|
return Vector3(0, 0, 1) if vector.z > 0 else Vector3(0, 0, -1)
|
|
|
|
static func get_longest_axis(vector:Vector3)->Axis:
|
|
if abs(vector.x) > abs(vector.y) and abs(vector.x) > abs(vector.z):
|
|
return Axis.X
|
|
elif abs(vector.y) > abs(vector.z):
|
|
return Axis.Y
|
|
else:
|
|
return Axis.Z
|
|
|
|
static func calc_bounds(points:PackedVector3Array)->AABB:
|
|
if points.is_empty():
|
|
return AABB(Vector3.ZERO, Vector3.ZERO)
|
|
|
|
var result:AABB = AABB(points[0], Vector3.ZERO)
|
|
for i in range(1, points.size()):
|
|
result = result.expand(points[i])
|
|
return result
|
|
|
|
#Returns value equal to twise the area between the two vectors. Clockwise windings have negative area
|
|
static func triange_area_2x_2d(a:Vector2, b:Vector2)->float:
|
|
return a.x * b.y - a.y * b.x
|
|
|
|
#Finds the bouding polygons of this set of points with a clockwise winding
|
|
static func bounding_polygon_2d(base_points:PackedVector2Array)->PackedVector2Array:
|
|
if base_points.size() <= 2:
|
|
return base_points
|
|
|
|
|
|
#Start with leftmost vertex, topmost if more than one
|
|
var p_init:Vector2 = base_points[0]
|
|
for p in base_points:
|
|
if p.x < p_init.x or (p.x == p_init.x and p.y > p_init.y):
|
|
p_init = p
|
|
|
|
|
|
var p_cur:Vector2 = p_init
|
|
var last_segment_dir = Vector2(0, 1)
|
|
|
|
var polygon:PackedVector2Array
|
|
|
|
while true:
|
|
var best_point:Vector2
|
|
var best_dir:Vector2
|
|
var best_angle:float = 0
|
|
|
|
for p in base_points:
|
|
if p.is_equal_approx(p_cur):
|
|
continue
|
|
|
|
var point_dir:Vector2 = (p - p_cur).normalized()
|
|
var angle:float = acos(-last_segment_dir.dot(point_dir))
|
|
|
|
if angle > best_angle or (angle == best_angle and p_cur.distance_squared_to(p) > p_cur.distance_squared_to(best_point)):
|
|
best_point = p
|
|
best_dir = point_dir
|
|
best_angle = angle
|
|
|
|
p_cur = best_point
|
|
last_segment_dir = best_dir
|
|
polygon.append(best_point)
|
|
|
|
if best_point.is_equal_approx(p_init):
|
|
break
|
|
|
|
return polygon
|
|
|
|
#static func bounding_polygon(base_points:PackedVector3Array, plane:Plane)->PackedVector3Array:
|
|
static func bounding_polygon_3d(base_points:PackedVector3Array, normal:Vector3)->PackedVector3Array:
|
|
if base_points.size() <= 2:
|
|
return base_points
|
|
|
|
var quat:Quaternion = Quaternion(normal, Vector3.FORWARD)
|
|
|
|
# var xform:Transform3D = Transform3D(Basis(quat), -base_points[0])
|
|
var xform:Transform3D = Transform3D(Basis(quat))
|
|
xform = xform.translated_local(-base_points[0])
|
|
var xform_inv = xform.inverse()
|
|
|
|
#print("xform %s" % xform)
|
|
|
|
var points_local:PackedVector2Array
|
|
|
|
for p in base_points:
|
|
var p_local = xform * p
|
|
points_local.append(Vector2(p_local.x, p_local.y))
|
|
|
|
var points_bounds:PackedVector2Array = bounding_polygon_2d(points_local)
|
|
|
|
var result:PackedVector3Array
|
|
for p in points_bounds:
|
|
var p_result = xform_inv * Vector3(p.x, p.y, 0)
|
|
result.append(p_result)
|
|
|
|
return result
|
|
|
|
static func points_are_colinear(points:PackedVector3Array)->bool:
|
|
if points.size() <= 2:
|
|
return true
|
|
|
|
var p0:Vector3 = points[0]
|
|
var p1:Vector3 = p0
|
|
var index:int = 0
|
|
for i in range(1, points.size()):
|
|
if !points[i].is_equal_approx(p0):
|
|
p1 = points[i]
|
|
index = i
|
|
break
|
|
|
|
if index == 0:
|
|
return true
|
|
|
|
var v10:Vector3 = p1 - p0
|
|
for i in range(index + 1, points.size()):
|
|
if !triangle_area_x2(p0, p1, points[i]).is_zero_approx():
|
|
return false
|
|
|
|
return true
|
|
|
|
|
|
static func furthest_point_from_line(line_origin:Vector3, line_dir:Vector3, points:PackedVector3Array)->Vector3:
|
|
var best_point:Vector3
|
|
var best_dist:float = 0
|
|
|
|
for p in points:
|
|
var offset:Vector3 = p - line_origin
|
|
var along:Vector3 = offset.project(line_dir)
|
|
var perp:Vector3 = offset - along
|
|
var dist:float = perp.length_squared()
|
|
if dist > best_dist:
|
|
best_dist = dist
|
|
best_point = p
|
|
|
|
return best_point
|
|
|
|
static func furthest_point_from_plane(plane:Plane, points:PackedVector3Array)->Vector3:
|
|
var best_point:Vector3
|
|
var best_distance:float = 0
|
|
|
|
for p in points:
|
|
var dist = abs(plane.distance_to(p))
|
|
if dist > best_distance:
|
|
best_point = p
|
|
best_distance = dist
|
|
|
|
return best_point
|
|
|
|
static func planar_volume_contains_point(planes:Array[Plane], point:Vector3)->bool:
|
|
# print("candidate %s" % point)
|
|
|
|
for p in planes:
|
|
var is_over:bool = p.is_point_over(point)
|
|
var is_on:bool = p.has_point(point)
|
|
if !is_over && !is_on:
|
|
# print("reject by %s" % p)
|
|
return false
|
|
# print("passed %s" % point)
|
|
return true
|
|
|
|
static func get_convex_hull_points_from_planes(planes:Array[Plane])->Array[Vector3]:
|
|
#Check for overlapping planes
|
|
for i0 in range(0, planes.size()):
|
|
for i1 in range(i0 + 1, planes.size()):
|
|
var p0:Plane = planes[i0]
|
|
var p1:Plane = flip_plane(planes[i1])
|
|
if p0.is_equal_approx(p1):
|
|
return []
|
|
|
|
var points:Array[Vector3]
|
|
|
|
for i0 in range(0, planes.size()):
|
|
for i1 in range(i0 + 1, planes.size()):
|
|
for i2 in range(i1 + 1, planes.size()):
|
|
var result = planes[i0].intersect_3(planes[i1], planes[i2])
|
|
|
|
if result == null:
|
|
continue
|
|
#print("candidate %s" % result)
|
|
if !planar_volume_contains_point(planes, result):
|
|
continue
|
|
if points.any(func(p):return p.is_equal_approx(result)):
|
|
continue
|
|
#print("adding %s" % result)
|
|
points.append(result)
|
|
|
|
return points
|
|
|
|
static func dist_to_segment_squared_2d(point:Vector2, seg_start:Vector2, seg_end:Vector2)->float:
|
|
if seg_start.is_equal_approx(seg_end):
|
|
return point.distance_squared_to(seg_start)
|
|
|
|
var dist_sq_p0:float = point.distance_squared_to(seg_start)
|
|
var dist_sq_p1:float = point.distance_squared_to(seg_end)
|
|
var seg_span:Vector2 = seg_end - seg_start
|
|
|
|
var offset:Vector2 = point - seg_start
|
|
var offset_proj:Vector2 = offset.project(seg_span)
|
|
var perp_dist_sq:float = (offset - offset_proj).length_squared()
|
|
|
|
if seg_span.dot(offset) < 0:
|
|
return dist_sq_p0
|
|
elif offset_proj.length_squared() > seg_span.length_squared():
|
|
return dist_sq_p1
|
|
return perp_dist_sq
|
|
|
|
class Segment2d extends RefCounted:
|
|
var p0:Vector2
|
|
var p1:Vector2
|
|
|
|
func _init(p0:Vector2, p1:Vector2):
|
|
self.p0 = p0
|
|
self.p1 = p1
|
|
|
|
func reverse()->Segment2d:
|
|
return Segment2d.new(p1, p0)
|
|
|
|
func _to_string():
|
|
return "[%s %s]" % [p0, p1]
|
|
|
|
static func extract_loop_2d(seg_stack:Array[Segment2d])->Loop2D:
|
|
var segs_sorted:Array[Segment2d] = []
|
|
var seg_tail = seg_stack.pop_back()
|
|
segs_sorted.append(seg_tail)
|
|
var seg_head = seg_tail
|
|
|
|
while !seg_stack.is_empty():
|
|
var found_seg:bool = false
|
|
for s_idx in seg_stack.size():
|
|
var cur_seg:Segment2d = seg_stack[s_idx]
|
|
|
|
if cur_seg.p0.is_equal_approx(seg_tail.p1):
|
|
#print("matching %s with %s" % [seg_tail, cur_seg])
|
|
segs_sorted.append(cur_seg)
|
|
seg_stack.remove_at(s_idx)
|
|
seg_tail = cur_seg
|
|
found_seg = true
|
|
break
|
|
elif cur_seg.p1.is_equal_approx(seg_tail.p1):
|
|
#print("matching %s with %s" % [seg_tail, cur_seg])
|
|
cur_seg = cur_seg.reverse()
|
|
segs_sorted.append(cur_seg)
|
|
seg_stack.remove_at(s_idx)
|
|
seg_tail = cur_seg
|
|
found_seg = true
|
|
break
|
|
elif cur_seg.p1.is_equal_approx(seg_head.p0):
|
|
#print("matching %s with %s" % [seg_head, cur_seg])
|
|
segs_sorted.insert(0, cur_seg)
|
|
seg_stack.remove_at(s_idx)
|
|
seg_head = cur_seg
|
|
found_seg = true
|
|
break
|
|
elif cur_seg.p0.is_equal_approx(seg_head.p0):
|
|
#print("matching %s with %s" % [seg_head, cur_seg])
|
|
cur_seg = cur_seg.reverse()
|
|
segs_sorted.insert(0, cur_seg)
|
|
seg_stack.remove_at(s_idx)
|
|
seg_head = cur_seg
|
|
found_seg = true
|
|
break
|
|
|
|
if !found_seg:
|
|
# push_warning("loop not continuous")
|
|
break
|
|
|
|
#print("segs_sorted %s" % str(segs_sorted))
|
|
|
|
var result:Loop2D = Loop2D.new()
|
|
result.closed = true
|
|
for s in segs_sorted:
|
|
result.points.append(s.p0)
|
|
|
|
if seg_head.p0 != seg_tail.p1:
|
|
result.points.append(seg_tail.p1)
|
|
result.closed = false
|
|
|
|
if face_area_x2_2d(result.points) < 0:
|
|
result.reverse()
|
|
|
|
#print("loop %s" % str(result))
|
|
|
|
return result
|
|
|
|
static func get_loops_from_segments_2d(segments:PackedVector2Array)->Array[Loop2D]:
|
|
#print("segments %s" % segments)
|
|
var loops:Array[Loop2D] = []
|
|
|
|
var seg_stack:Array[Segment2d] = []
|
|
for i in range(0, segments.size(), 2):
|
|
seg_stack.append(Segment2d.new(segments[i], segments[i + 1]))
|
|
|
|
# print("segs %s" % str(seg_stack))
|
|
|
|
while !seg_stack.is_empty():
|
|
var loop:Loop2D = extract_loop_2d(seg_stack)
|
|
loops.append(loop)
|
|
|
|
#print("result %s" % str(loops))
|
|
return loops
|
|
|
|
static func create_transform(translation:Vector3, rotation_axis:Vector3, rotation_angle:float, scale:Vector3, pivot:Vector3)->Transform3D:
|
|
var xform:Transform3D = Transform3D.IDENTITY
|
|
|
|
xform = xform.translated_local(pivot + translation)
|
|
xform = xform.rotated_local(rotation_axis, rotation_angle)
|
|
xform = xform.scaled_local(scale)
|
|
xform = xform.translated_local(-pivot)
|
|
|
|
return xform
|
|
|
|
static func create_circle_points(center:Vector3, normal:Vector3, radius:float, num_segments:int)->PackedVector3Array:
|
|
var result:PackedVector3Array
|
|
|
|
var axis:Axis = get_longest_axis(normal)
|
|
var perp_normal:Vector3
|
|
match axis:
|
|
Axis.X:
|
|
perp_normal = normal.cross(Vector3.UP)
|
|
Axis.Y:
|
|
perp_normal = normal.cross(Vector3.FORWARD)
|
|
Axis.Z:
|
|
perp_normal = normal.cross(Vector3.UP)
|
|
|
|
var angle_incrment = (PI * 2 / num_segments)
|
|
for i in num_segments:
|
|
var offset:Vector3 = perp_normal.rotated(normal, i * angle_incrment)
|
|
result.append(offset * radius + center)
|
|
|
|
return result
|
|
|
|
static func get_axis_aligned_tangent_and_binormal(normal:Vector3)->Array[Vector3]:
|
|
var axis:MathUtil.Axis = MathUtil.get_longest_axis(normal)
|
|
#calc tangent and binormal
|
|
var u_normal:Vector3
|
|
var v_normal:Vector3
|
|
match axis:
|
|
MathUtil.Axis.Y:
|
|
u_normal = normal.cross(Vector3.FORWARD)
|
|
v_normal = u_normal.cross(normal)
|
|
return [u_normal, v_normal]
|
|
MathUtil.Axis.X:
|
|
u_normal = normal.cross(Vector3.UP)
|
|
v_normal = u_normal.cross(normal)
|
|
return [u_normal, v_normal]
|
|
MathUtil.Axis.Z:
|
|
u_normal = normal.cross(Vector3.UP)
|
|
v_normal = u_normal.cross(normal)
|
|
return [u_normal, v_normal]
|
|
|
|
return []
|
|
|
|
#Returns the planes of a frustum for the rectangular region on the camera's near
|
|
# plane with all planes pointing toward the interior of the frustum
|
|
static func calc_frustum_camera_rect(cam:Camera3D, p0:Vector2, p1:Vector2)->Array[Plane]:
|
|
|
|
var x0 = min(p0.x, p1.x)
|
|
var x1 = max(p0.x, p1.x)
|
|
var y0 = min(p0.y, p1.y)
|
|
var y1 = max(p0.y, p1.y)
|
|
|
|
var p00:Vector2 = Vector2(x0, y0)
|
|
var p01:Vector2 = Vector2(x0, y1)
|
|
var p10:Vector2 = Vector2(x1, y0)
|
|
var p11:Vector2 = Vector2(x1, y1)
|
|
|
|
# print("cam rect %s" % str([p00, p11]))
|
|
|
|
#Cam project_position does not work if we set distance to far plane, so back off a bit
|
|
var far_scalar:float = .95
|
|
|
|
var p000:Vector3 = cam.project_position(p00, cam.near)
|
|
var p100:Vector3 = cam.project_position(p10, cam.near)
|
|
var p010:Vector3 = cam.project_position(p01, cam.near)
|
|
var p110:Vector3 = cam.project_position(p11, cam.near)
|
|
var p001:Vector3 = cam.project_position(p00, cam.far * far_scalar)
|
|
var p101:Vector3 = cam.project_position(p10, cam.far * far_scalar)
|
|
var p011:Vector3 = cam.project_position(p01, cam.far * far_scalar)
|
|
var p111:Vector3 = cam.project_position(p11, cam.far * far_scalar)
|
|
|
|
# print("points %s" % str([p000, p100, p010, p110, p001, p101, p011, p111, ]))
|
|
|
|
var plane_left:Plane = Plane(p001, p011, p010)
|
|
var plane_right:Plane = Plane(p101, p110, p111)
|
|
var plane_top:Plane = Plane(p011, p111, p110)
|
|
var plane_bottom:Plane = Plane(p001, p100, p101)
|
|
var plane_near:Plane = Plane(p000, p110, p100)
|
|
var plane_far:Plane = Plane(p001, p111, p011)
|
|
|
|
return [plane_left, plane_right, plane_top, plane_bottom, plane_near, plane_far]
|
|
|
|
static func clip_polygon(points:PackedVector3Array, plane:Plane)->PackedVector3Array:
|
|
var result:PackedVector3Array
|
|
|
|
#Cut at planr intersection
|
|
var points_on_or_over:PackedVector3Array
|
|
|
|
for p_idx0 in points.size():
|
|
var p_idx1:int = wrap(p_idx0 + 1, 0, points.size())
|
|
|
|
var p0:Vector3 = points[p_idx0]
|
|
var p1:Vector3 = points[p_idx1]
|
|
|
|
var on0:bool = plane.has_point(p0)
|
|
var over0:bool = plane.is_point_over(p0)
|
|
var under0:bool = !on0 && !over0
|
|
var on1:bool = plane.has_point(p1)
|
|
var over1:bool = plane.is_point_over(p1)
|
|
var under1:bool = !on1 && !over1
|
|
|
|
if on0 || over0:
|
|
points_on_or_over.append(p0)
|
|
|
|
if (under0 && over1) || (over0 && under1):
|
|
points_on_or_over.append(plane.intersects_segment(p0, p1))
|
|
|
|
return points_on_or_over
|
|
|
|
|
|
#Snaps point to a point appearing in the list if distance to it is <= radius. Otherwise appends
|
|
# point to point list
|
|
static func snap_point_to_point_list_or_append(point:Vector3, list:PackedVector3Array, radius:float = .005):
|
|
for p in list:
|
|
if p.distance_squared_to(point) < radius * radius:
|
|
return p
|
|
list.append(point)
|
|
return point
|
|
|
|
|
|
static func create_loop_from_directed_segments(segs:Array[Segment3], snap_radius:float = .005)->PackedVector3Array:
|
|
var snap_list:PackedVector3Array
|
|
for seg in segs:
|
|
seg.p0 = snap_point_to_point_list_or_append(seg.p0, snap_list, snap_radius)
|
|
seg.p1 = snap_point_to_point_list_or_append(seg.p1, snap_list, snap_radius)
|
|
|
|
|
|
var seg_stack:Array[Segment3]
|
|
var sorted_segs:Array[Segment3]
|
|
|
|
for s in segs:
|
|
if !is_zero_approx(s.length_squared()):
|
|
seg_stack.append(s)
|
|
|
|
|
|
sorted_segs.append(seg_stack.pop_back())
|
|
while !seg_stack.is_empty():
|
|
var found_seg:bool = false
|
|
var min_dist:float = 10000
|
|
for i in seg_stack.size():
|
|
var s:Segment3 = seg_stack[i]
|
|
|
|
# if s.p0.is_equal_approx(sorted_segs.back().p1):
|
|
var dist:float = s.p0.distance_to(sorted_segs.back().p1)
|
|
min_dist = min(min_dist, dist)
|
|
|
|
if dist < .005:
|
|
# if s.p0.is_equal_approx(sorted_segs.back().p1):
|
|
sorted_segs.append(s)
|
|
seg_stack.remove_at(i)
|
|
found_seg = true
|
|
break
|
|
# if s.p1.is_equal_approx(sorted_segs.back().p1):
|
|
# sorted_segs.append(s.reversed())
|
|
# seg_stack.remove_at(i)
|
|
# found_seg = true
|
|
# break
|
|
|
|
if !found_seg:
|
|
print("Error: could not form loop")
|
|
return []
|
|
|
|
var result:PackedVector3Array
|
|
for s in sorted_segs:
|
|
result.append(s.p0)
|
|
|
|
return result
|
|
|
|
static func clip_polygon_separate(points:PackedVector3Array, plane:Plane)->ClipPolyResult:
|
|
|
|
#Clip points to plane.
|
|
var clipped_points:PackedVector3Array = clip_polygon(points, plane)
|
|
|
|
#Every point should now be on or above the plane
|
|
var is_over:Array[bool]
|
|
var all_over:bool = true
|
|
var none_over:bool = true
|
|
for p in clipped_points:
|
|
var is_on:bool = plane.has_point(p)
|
|
if is_on:
|
|
all_over = false
|
|
else:
|
|
none_over = false
|
|
|
|
is_over.append(!is_on)
|
|
|
|
if all_over:
|
|
return ClipPolyResult.new([clipped_points])
|
|
|
|
if none_over:
|
|
return ClipPolyResult.new()
|
|
|
|
var start_idx:int = -1
|
|
for p_idx0 in clipped_points.size():
|
|
var p_idx1:int = wrap(p_idx0 + 1, 0, clipped_points.size())
|
|
|
|
var over0:bool = is_over[p_idx0]
|
|
var over1:bool = is_over[p_idx1]
|
|
|
|
if !over0 && over1:
|
|
start_idx = p_idx0
|
|
break
|
|
|
|
#If you think of the clipped_points as a string where every point on the plane is
|
|
# represented by the character 'n' and every point over the plane is the character
|
|
# 'v', then every sub polygon will be a string that can be represented by the
|
|
# regular expression "(nv+n)"
|
|
var results:Array[PackedVector3Array]= []
|
|
var cut_segments:Array[Segment3]
|
|
|
|
var writing_shape:bool = true
|
|
var sub_poly:PackedVector3Array
|
|
for i in clipped_points.size():
|
|
var p_idx0:int = wrap(i + start_idx, 0, clipped_points.size())
|
|
var p_idx1:int = wrap(i + start_idx + 1, 0, clipped_points.size())
|
|
|
|
if is_over[p_idx1]:
|
|
sub_poly.append(clipped_points[p_idx0])
|
|
|
|
elif is_over[p_idx0]:
|
|
sub_poly.append(clipped_points[p_idx0])
|
|
sub_poly.append(clipped_points[p_idx1])
|
|
|
|
cut_segments.append(Segment3.new(sub_poly[sub_poly.size() - 1], sub_poly[0]))
|
|
results.append(sub_poly.duplicate())
|
|
sub_poly.clear()
|
|
|
|
return ClipPolyResult.new(results, cut_segments)
|
|
|
|
|
|
|
|
static func polygon_intersects_frustum(points:PackedVector3Array, frustum:Array[Plane])->bool:
|
|
var points_i:PackedVector3Array = points
|
|
|
|
for plane in frustum:
|
|
points_i = clip_polygon(points_i, plane)
|
|
if points_i.is_empty():
|
|
return false
|
|
|
|
return true
|
|
|
|
|
|
static func frustum_contians_point(planes:Array[Plane], point:Vector3)->bool:
|
|
for plane in planes:
|
|
if !plane.is_point_over(point) && !plane.has_point(point):
|
|
return false
|
|
return true
|
|
|
|
static func frustum_intersects_sphere(planes:Array[Plane], center:Vector3, radius:float)->bool:
|
|
for plane in planes:
|
|
var dist:float = plane.distance_to(center)
|
|
|
|
if dist < -radius:
|
|
return false
|
|
|
|
return true
|
|
|
|
|
|
func plane_intesects_point_cloud(points:PackedVector3Array, plane:Plane)->bool:
|
|
|
|
var is_over:bool = false
|
|
var is_under:bool = false
|
|
for p in points:
|
|
if plane.has_point(p):
|
|
continue
|
|
|
|
if plane.is_point_over(p):
|
|
is_over = true
|
|
else:
|
|
is_under = true
|
|
if is_over && is_under:
|
|
return true
|
|
|
|
return false
|
|
|
|
#Returns vector with [R, Q] where R is the orthogonal basis
|
|
# and Q is a triangular matrix such that basis = R * Q
|
|
static func gram_schmidt_decomposition(basis:Basis)->Array[Basis]:
|
|
#https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process
|
|
var v0:Vector3 = basis.x
|
|
var v1:Vector3 = basis.y
|
|
var v2:Vector3 = basis.z
|
|
|
|
var u0:Vector3 = v0
|
|
var u1:Vector3 = v1 - v1.project(u0)
|
|
var u2:Vector3 = v2 - v2.project(u0) - v2.project(u1)
|
|
|
|
var R:Basis = Basis(u0.normalized(), u1.normalized(), u2.normalized())
|
|
var R_inv:Basis = R.inverse()
|
|
var Q:Basis = R_inv * basis
|
|
|
|
return [R, Q]
|
|
|
|
#Decomposes matrix into translate, rotate, scale and shear vectors where
|
|
# M = T * R * Sh * S
|
|
# where:
|
|
# T - translate matrix
|
|
# R - rotate matrix
|
|
# Sh - shear matrix
|
|
# S - scale matrix
|
|
#
|
|
# Shear matrix for vector (x, y, z) is
|
|
# [1 x y]
|
|
# [0 1 z]
|
|
# [0 0 1]
|
|
static func decompose_matrix_3d(m:Transform3D, order:EulerOrder = EULER_ORDER_YXZ)->Dictionary:
|
|
|
|
if is_zero_approx(m.basis.determinant()):
|
|
return {"valid": false}
|
|
|
|
var basis:Basis = m.basis
|
|
var gram_schmidt = gram_schmidt_decomposition(basis)
|
|
var rot_mtx = gram_schmidt[0]
|
|
var euler:Vector3 = rot_mtx.get_euler(order)
|
|
|
|
var scale_shear = gram_schmidt[1]
|
|
var scale:Vector3 = Vector3(scale_shear.x.x, scale_shear.y.y, scale_shear.z.z)
|
|
var scale_mat:Basis = Basis.from_scale(scale)
|
|
var shear:Basis = scale_shear * scale_mat.inverse()
|
|
|
|
#print(shear)
|
|
|
|
return {
|
|
"valid": true,
|
|
"translate": m.origin,
|
|
"rotate": euler,
|
|
"scale": scale,
|
|
"shear": Vector3(shear.y.x, shear.z.x, shear.z.y)
|
|
}
|
|
|
|
static func compose_matrix_3d(translate:Vector3, rotate:Vector3 = Vector3.ZERO, order:EulerOrder = EULER_ORDER_YXZ, shear:Vector3 = Vector3.ZERO, scale:Vector3 = Vector3.ONE)->Transform3D:
|
|
var scale_mat:Basis = Basis.from_scale(scale)
|
|
var shear_mat:Basis = Basis(
|
|
Vector3(1, 0, 0),
|
|
Vector3(shear.x, 1, 0),
|
|
Vector3(shear.y, shear.z, 1))
|
|
var rot_mat:Basis = Basis.from_euler(rotate, order)
|
|
var basis:Basis = rot_mat * shear_mat * scale_mat
|
|
|
|
return Transform3D(basis, translate)
|
|
|
|
static func clip_segment_to_plane_3d(p:Plane, v0:Vector3, v1:Vector3)->PackedVector3Array:
|
|
var clip_v0:bool = !p.is_point_over(v0)
|
|
var clip_v1:bool = !p.is_point_over(v1)
|
|
if clip_v0 && clip_v1:
|
|
return []
|
|
|
|
if clip_v0:
|
|
v0 = p.intersects_segment(v0, v1)
|
|
elif clip_v1:
|
|
v1 = p.intersects_segment(v0, v1)
|
|
|
|
return [v0, v1]
|
|
|
|
|
|
static func blend_over_with_alpha(src:Color, dest:Color):
|
|
#https://en.wikipedia.org/wiki/Alpha_compositing
|
|
var a0:float = src.a + dest.a * (1 - src.a)
|
|
var r0:float = (src.r * src.a + dest.r * dest.a * (1 - src.a)) / a0
|
|
var g0:float = (src.g * src.a + dest.g * dest.a * (1 - src.a)) / a0
|
|
var b0:float = (src.b * src.a + dest.b * dest.a * (1 - src.a)) / a0
|
|
return Color(r0, g0, b0, a0)
|
|
|
|
static func blend_colors_with_alpha(src:Color, dest:Color, weight:float)->Color:
|
|
var col:Color = blend_over_with_alpha(src, dest)
|
|
col.a *= weight
|
|
return blend_over_with_alpha(col, dest)
|
|
|
|
static func blend_colors_ignore_alpha(src:Color, dest:Color, weight:float)->Color:
|
|
return weight * src + (1 - weight) * dest
|
|
|