174 lines
6.0 KiB
GDScript
174 lines
6.0 KiB
GDScript
###
|
|
### geometry utility code
|
|
### some of this might not work yet, please dont use it
|
|
###
|
|
extends Object
|
|
|
|
class_name GDBGeoUtility
|
|
|
|
class Grid3D:
|
|
var size : Vector3
|
|
var data : Array
|
|
|
|
func _init(size_ : Vector3, init_val = null):
|
|
size = size_
|
|
data = []
|
|
data.resize(size.x * size.y * size.z)
|
|
for i in range(data.size()):
|
|
data[i] = init_val
|
|
|
|
func _idx(x, y, z):
|
|
return x + y * size.x + z * size.x * size.y
|
|
|
|
func get_at(x, y, z):
|
|
return data[_idx(x, y, z)]
|
|
|
|
func set_at(x, y, z, val):
|
|
data[_idx(x, y, z)] = val
|
|
|
|
################
|
|
# public stuff #
|
|
################
|
|
func _init() -> void:
|
|
assert(0, "This class should not be instantiated.")
|
|
|
|
static func full_aabb(root : Spatial):
|
|
var aabb = AABB()
|
|
for vi in GDBUtility.find_nodes_by_type(root, VisualInstance):
|
|
var local_aabb = vi.get_aabb()
|
|
local_aabb = vi.global_transform.xform(local_aabb)
|
|
aabb = aabb.merge(local_aabb)
|
|
return aabb
|
|
|
|
static func gen_aabb(points : Array, point_transform = Transform()) -> AABB:
|
|
var aabb = AABB()
|
|
for point in points:
|
|
aabb = aabb.expand(point_transform.xform(point))
|
|
return aabb
|
|
|
|
static func collsion_aabb(collision_object : CollisionObject) -> AABB:
|
|
var aabb := AABB()
|
|
for owner_id in collision_object.get_shape_owners():
|
|
var trans = collision_object.shape_owner_get_transform(owner_id)
|
|
for shape_id in range(collision_object.shape_owner_get_shape_count(owner_id)):
|
|
var shape = collision_object.shape_owner_get_shape(owner_id, shape_id)
|
|
var new_aabb = shape_aabb(shape)
|
|
if new_aabb.size:
|
|
aabb = aabb.merge(trans.xform(new_aabb))
|
|
return aabb
|
|
|
|
static func shape_aabb(shape : Shape) -> AABB:
|
|
if shape is BoxShape:
|
|
return AABB(-shape.extents, 2.0 * shape.extents)
|
|
elif shape is CapsuleShape:
|
|
return AABB(Vector3(-shape.radius, -shape.radius - 0.5 * shape.height, -shape.radius), \
|
|
Vector3(2.0 * shape.radius, 2.0 * shape.radius + shape.height, 2.0 * shape.radius))
|
|
elif shape is CylinderShape:
|
|
return AABB(Vector3(-shape.radius, -0.5 * shape.height, -shape.radius), \
|
|
Vector3(2.0 * shape.radius, shape.height, 2.0 * shape.radius))
|
|
elif shape is PlaneShape:
|
|
return AABB()
|
|
elif shape is SphereShape:
|
|
return AABB(-Vector3(shape.radius, shape.radius, shape.radius), 2.0 * Vector3(shape.radius, shape.radius, shape.radius))
|
|
else:
|
|
# TODO: polygon shapes
|
|
return AABB()
|
|
|
|
static func orphan_global_transform(node : Node):
|
|
var transform : Transform
|
|
if node is Spatial:
|
|
transform = node.transform
|
|
var parent = node.get_parent()
|
|
if parent != null:
|
|
transform = orphan_global_transform(parent) * transform
|
|
return transform
|
|
|
|
static func voxelize_surf(mesh : ArrayMesh, surf : int, particle_size = -1.0) -> PoolVector3Array:
|
|
var arrays = mesh.surface_get_arrays(surf)
|
|
var points = arrays[Mesh.ARRAY_VERTEX]
|
|
return PoolVector3Array(points)
|
|
|
|
static func voxelize(mesh : ArrayMesh, particle_size = -1.0) -> PoolVector3Array:
|
|
var points = PoolVector3Array()
|
|
for surf in range(mesh.get_surface_count()):
|
|
points.append_array(voxelize_surf(mesh, surf, particle_size))
|
|
return points
|
|
|
|
static func transform_to(node : Node, root : Node):
|
|
var transform = Transform()
|
|
var node_ = node
|
|
while node_ != root && node_ != null:
|
|
if node_ is Spatial:
|
|
transform = transform * node_.transform
|
|
node_ = node_.get_parent()
|
|
return transform
|
|
|
|
static func mesh_to_grid(mesh : ArrayMesh, grid_size : float, point_transform = Transform()):
|
|
var aabb = AABB()
|
|
for i in range(mesh.get_surface_count()):
|
|
var points = mesh.surface_get_arrays(i)[Mesh.ARRAY_VERTEX]
|
|
aabb = aabb.merge(gen_aabb(points, point_transform))
|
|
var grid_x = ceil(aabb.size.x / grid_size)
|
|
var grid_y = ceil(aabb.size.y / grid_size)
|
|
var grid_z = ceil(aabb.size.z / grid_size)
|
|
var grid = Grid3D.new(Vector3(grid_x, grid_y, grid_z), 0)
|
|
var grid_origin = aabb.position
|
|
|
|
for i in range(mesh.get_surface_count()):
|
|
__insert_surf(mesh, i, grid_origin, grid_size, grid, point_transform)
|
|
|
|
var result = []
|
|
for x in range(grid.size.x):
|
|
for y in range(grid.size.y):
|
|
var inside = false
|
|
for z in range(grid.size.z):
|
|
var cell_value = grid.get_at(x, y, z) % 2
|
|
if cell_value == 1:
|
|
inside = !inside
|
|
if inside || cell_value > 0:
|
|
result.append(Vector3(x, y, z))
|
|
return result
|
|
|
|
# http://www.boris-belousov.net/2016/12/01/quat-dist/
|
|
static func basis_angle(basis0 : Basis, basis1 : Basis):
|
|
var basis_diff = basis0 * basis1.transposed()
|
|
var tr : float = basis_diff.x.x + basis_diff.y.y + basis_diff.z.z
|
|
return acos((tr - 1) / 2)
|
|
|
|
static func angle_normalize(angle : float) -> float:
|
|
while angle < -PI:
|
|
angle += 2.0 * PI
|
|
while angle > PI:
|
|
angle -= 2.0 * PI
|
|
return angle
|
|
|
|
static func angle_diff(angle0 : float, angle1 : float) -> float:
|
|
return angle_normalize(angle0 - angle1)
|
|
|
|
#################
|
|
# private stuff #
|
|
#################
|
|
static func __insert_tri(a : Vector3, b : Vector3, c : Vector3, grid_origin : Vector3, grid_size : float, grid : Grid3D, point_transform : Transform):
|
|
var ray_dir = Vector3(0, 0, 1)
|
|
for x in range(grid.size.x):
|
|
for y in range(grid.size.y):
|
|
var ray_origin = grid_origin + grid_size * Vector3(x, y, 0)
|
|
var inters = Geometry.ray_intersects_triangle(ray_origin, ray_dir, point_transform.xform(a), point_transform.xform(b), point_transform.xform(c))
|
|
if inters != null:
|
|
var z = floor(0.99 * (inters.z - grid_origin.z) / grid_size)
|
|
grid.set_at(x, y, z, grid.get_at(x, y, z) + 1)
|
|
|
|
static func __insert_surf(mesh : ArrayMesh, surf : int, grid_origin : Vector3, grid_size : float, grid : Grid3D, point_transform : Transform):
|
|
var arrays = mesh.surface_get_arrays(surf)
|
|
if arrays.size() >= Mesh.ARRAY_INDEX - 1 && arrays[Mesh.ARRAY_INDEX].size() > 0:
|
|
# index mode
|
|
var indices = arrays[Mesh.ARRAY_INDEX]
|
|
var vertices = arrays[Mesh.ARRAY_VERTEX]
|
|
for i in range(0, indices.size() - 2, 3):
|
|
__insert_tri(vertices[indices[i]], vertices[indices[i + 1]], vertices[indices[i + 2]], grid_origin, grid_size, grid, point_transform)
|
|
else:
|
|
# normal mode
|
|
var vertices = arrays[Mesh.ARRAY_VERTEX]
|
|
for i in range(0, vertices.size() - 2, 3):
|
|
__insert_tri(vertices[i], vertices[i + 1], vertices[i + 2], grid_origin, grid_size, grid, point_transform)
|