Tool for drawing 3D lines using actual meshes (Godot 3)

draw_tool_3d_mesh.gd

class_name DrawTool3DMesh extends Spatial

#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=
#        DrawTool3DMesh 0.7.3 (Godot 3)
#
#     Uses a MultiMeshInstance to draw instances of a cube or cylinder,
#     stretched to represent lines, and spheres for 3D points.
#     For each line AB, scale an instance of a cube in one axis to equal the
#     distance from A to B, and then rotate it accordingly using
#     'transform.looking_at()'.
#     Cylinders are by default upright, along the Y axis, so they also have to
#     be manually rotated to compensate for this.
#
#  Notes:
#     '_WIDTH_FACTOR' affects the line width. Draw lines of width 1 to tweak it.
#     Basically it's how thick the unit cube should be in order to properly
#     represent a line of thickness 1.
#
#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=
# TODO:
#   - consider periodically lowering the 'instance_count', since currently it
#     grows as needed, and stays as is to serve as an object pool,
#     but it never comes back down (not sure it's really an issue)
#
#   - draw quads
#   - draw polygons
#
#   - create cylinders without caps through code (and cones) ?

#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=

const _DOT_THRESHOLD := 0.999
const _WIDTH_FACTOR := 0.005

const _RADIAL_SEGMENTS := 8
const _SPHERE_RINGS := 4
const _CIRCLE_SEGMENTS := 16

const _USE_TRANSPARENCY := false
const _MAX_ALPHA := 0.5
const _UNSHADED := true    # actually looks great shaded, except for the cylinder caps
const _DRAW_OUTLINES := false


var _mms:Dictionary
var _use_cylinders_for_lines := true



#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=

#    initializeation

#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=
func _init(_name=null) -> void:
	name = "DrawTool3DMesh" if not _name else _name


func _ready() -> void:
	var mat = _create_material()

	# ----------------------------------------
	# create lines
	if _use_cylinders_for_lines:
		# Note: cylinders look like the cubes with 4 radial_segments
		var cylinder := CylinderMesh.new()
		cylinder.top_radius = _WIDTH_FACTOR
		cylinder.bottom_radius = _WIDTH_FACTOR
		cylinder.height = 1
		cylinder.radial_segments = _RADIAL_SEGMENTS
		cylinder.rings = 0
		cylinder.material = mat

		_mms["lines"] = _create_multimesh(cylinder)
	else:
		# this is deprecated, but the cylinder code isn't well tested yet
		# so this is here just in case
		var cube := CubeMesh.new()
		cube.size = Vector3(_WIDTH_FACTOR, _WIDTH_FACTOR, 1)
		cube.material = mat

		_mms["lines"] = _create_multimesh(cube)

	# ----------------------------------------
	# create cones (for vectors)
	var cone := CylinderMesh.new()
	cone.top_radius = 0
	cone.bottom_radius = _WIDTH_FACTOR*4
	cone.height = 0.04
	cone.radial_segments = _RADIAL_SEGMENTS
	cone.rings = 0
	cone.material = mat

	_mms["cones"] = _create_multimesh(cone)

	# ----------------------------------------
	# create spheres
	var sphere := SphereMesh.new()
	sphere.radius = _WIDTH_FACTOR
	sphere.height = _WIDTH_FACTOR*2
	sphere.radial_segments = _RADIAL_SEGMENTS
	sphere.rings = _SPHERE_RINGS
	sphere.material = mat

	_mms["spheres"] = _create_multimesh(sphere)


func _create_material() -> SpatialMaterial:
	var mat := SpatialMaterial.new()
	mat.vertex_color_use_as_albedo = true
	mat.flags_unshaded = _UNSHADED
	mat.flags_no_depth_test = true
	mat.flags_do_not_receive_shadows = true
	mat.flags_transparent = _USE_TRANSPARENCY
	mat.albedo_color = Color(1,1,1, _MAX_ALPHA) if _USE_TRANSPARENCY else Color.white

	if _DRAW_OUTLINES:
		var np_mat := SpatialMaterial.new()
		np_mat.flags_unshaded = _UNSHADED
		np_mat.flags_do_not_receive_shadows = true
		np_mat.flags_no_depth_test = true
		np_mat.albedo_color = Color.black
		np_mat.params_grow = true
		np_mat.params_grow_amount = 0.002

		mat.next_pass = np_mat

	return mat


func _create_multimesh(mesh:Mesh) -> MultiMesh:
	var mm = MultiMesh.new()
	mm.transform_format = MultiMesh.TRANSFORM_3D
	mm.color_format = MultiMesh.COLOR_FLOAT
	mm.mesh = mesh
	mm.visible_instance_count = 0
	mm.instance_count = 256

	var mmi = MultiMeshInstance.new()
	mmi.multimesh = mm
	add_child(mmi)
	return mm



#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=

#    internal stuff

#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=
func _create_circle_points(position:Vector3, radius:float, axis:Vector3, color:Color) -> Array:
	axis = axis.normalized()

	var points := []
#	point(position, Color.green, 5) # draw debug point

	var cross = axis.cross(Vector3.RIGHT).normalized() *  radius

	var dot = abs(cross.dot(axis))
	if dot > 0.9:
		print("I WAS HERE") # this seems to never run
		cross = axis.cross(Vector3.UP) * radius

	# draw a debug cross-product vector
#	line(position, position+cross, color, 2)
#	cone(position+cross, cross, color, 2)

	for r in range(0, 360, 360/_CIRCLE_SEGMENTS):
		var p = position + cross.rotated(axis, deg2rad(r)) * radius
		points.append(p)
#		point(p, Color.yellow, 5) # draw debug point

	return points


func _create_circle_points_OLD(position:Vector3, radius:Vector3, axis:Vector3) -> Array:
	var points := []

	for r in range(0, 360, 360/_RADIAL_SEGMENTS):
		var p = position + radius.rotated(axis, deg2rad(r))
		points.append(p)

	return points

# http://kidscancode.org/godot_recipes/3.x/3d/3d_align_surface/
func align_with_y(tr:Transform, new_y:Vector3) -> Transform:
	tr.basis.y = new_y
	tr.basis.x = -tr.basis.z.cross(new_y)
	tr.basis = tr.basis.orthonormalized()
	return tr


#TODO: Test if it's really better to add many instances once in a while
#      versus adding one instance every time it's needed.
#      Maybe there's a tradeoff between too many and too few.
func _add_instance_to(mm:MultiMesh) -> int:
	# the index of a new instance is count-1
	var idx := mm.visible_instance_count
	mm.visible_instance_count += 1

	# if the visible count reaches the instance count, then more instances are needed
	if mm.instance_count <= mm.visible_instance_count:
		# this is enough to make the MultiMesh create more instances internally
		mm.instance_count += 256

	return idx


func _commit_instance(mm:MultiMesh, idx:int, transform:Transform, color:Color) -> void:
	mm.set_instance_transform(idx, transform)
	mm.set_instance_color(idx, color)


func _add_line(a:Vector3, b:Vector3, color:Color, thickness:=1.0) -> void:
	if _use_cylinders_for_lines:
		_add_line_cylinder(a, b, color, thickness)
	else:
		_add_line_cube(a, b, color, thickness)


func _check_equal_points(a:Vector3, b:Vector3) -> bool:
	if a != b: return false
#	push_warning("points 'a' and 'b' are the same: %s == %s" % [a, b])
	return true


func _add_line_cube(a:Vector3, b:Vector3, color:Color, thickness:=1.0) -> void:
	# I had issues here with 'looking_at', which I can't quite remember,
	# but I solved somehow. I posted it here:
	#     https://godotforums.org/d/27860-transform-looking-at-not-working
	var mm:MultiMesh = _mms["lines"]
	if _check_equal_points(a, b): return

	# adding an instance is basically just raising the visible_intance_count
	# and then using that index to get and set properties of the instance
	var idx := _add_instance_to(mm)

	# if transform is to be orthonormalized, do it here before applying any
	# scaling, or it will revert the scaling
	var transform := mm.get_instance_transform(idx).orthonormalized()
#	var transform := Transform()
	transform.origin = (a+b)/2

	var target_direction := (b-transform.origin).normalized()
	transform = transform.looking_at(b,
		Vector3.UP if abs(target_direction.dot(Vector3.UP)) < _DOT_THRESHOLD
		else Vector3.BACK
	)

	# TODO: this probably accumulates scaling if this instance was scaled before,
	#       but I've never seen any issues, so... I could be wrong.
	transform.basis.x *= thickness
	transform.basis.y *= thickness
	transform.basis.z *= a.distance_to(b)

	_commit_instance(mm, idx, transform, color)


func _add_line_cylinder(a:Vector3, b:Vector3, color:Color, thickness:=1.0) -> void:
	if _check_equal_points(a, b): return

	var mm:MultiMesh = _mms["lines"]
	var idx := _add_instance_to(mm)

	var transform := mm.get_instance_transform(idx).orthonormalized()
	transform.origin = (a+b)/2

	var target_direction := (b-transform.origin).normalized()

	transform = align_with_y(transform, target_direction)

	transform.basis.x *= thickness
	transform.basis.y *= a.distance_to(b) # stretch the Y instead
	transform.basis.z *= thickness

	_commit_instance(mm, idx, transform, color)


func _add_cone(position:Vector3, direction:Vector3, color:Color, thickness:=1.0):
	var mm:MultiMesh = _mms["cones"]

	var idx := _add_instance_to(mm)
	var transform := Transform()
	transform.origin = position

	transform = align_with_y(transform, direction)

	transform.basis = transform.basis.scaled(Vector3.ONE * thickness)

	_commit_instance(mm, idx, transform, color)


func _add_sphere(position:Vector3, color:Color, size:=1.0) -> void:
	var mm:MultiMesh = _mms["spheres"]

	var idx := _add_instance_to(mm)
#	var transform := mm.get_instance_transform(idx).orthonormalized()
	var transform := Transform()

	transform.origin = position
	transform.basis = transform.basis.scaled(Vector3.ONE * size)

	_commit_instance(mm, idx, transform, color)


func _add_circle():
	pass



#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=

#    Public API

#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=
func clear() -> void:
	# keep the real 'instance_count' up, to serve as a pool
	for mm in _mms.values():
		mm.visible_instance_count = 0


func line(a:Vector3, b:Vector3, color:Color, thickness:=1.0) -> void:
	_add_line(a, b, color, thickness)

# points = contiguous Array[Vector3]
func polyline(points:Array, color:Color, thickness:=1.0) -> void:
	for i in range(1, points.size(), 1):
		_add_line(points[i-1], points[i], color, thickness)

# lines = array of arrays: [a, b, color, thickness]
func bulk_lines(lines:Array) -> void:
	for l in lines:
		_add_line(l[0], l[1], l[2], l[3])


# useful for drawing vectors as arrows, for example
func cone(position:Vector3, direction:Vector3, color:Color, thickness:=1.0) -> void:
	_add_cone(position, direction, color, thickness)

# cones = array of arrays: [position, direction, color, thickness]
func bulk_cones(cones:Array) -> void:
	for c in cones:
		var tip = c[0]+c[1]
		if c.size() > 3:
			_add_cone(c[0], c[1], c[2], c[3])
		else:
			_add_cone(c[0], c[1], c[2])


func point(position:Vector3, color:Color, size:=1.0) -> void:
	_add_sphere(position, color, size)

# points = contiguous Array[Vector3]
func points(points:Array, color:Color, size:=1.0) -> void:
	for p in points:
		_add_sphere(p, color, size)

# points = array of arrays: [position, color, size]
func bulk_points(points:Array) -> void:
	for p in points:
		if p.size() > 2: _add_sphere(p[0], p[1], p[2])
		else:            _add_sphere(p[0], p[1])


func circle(position:Vector3, radius:float, axis:Vector3, color:Color, thickness:=1.0):
	var points := _create_circle_points(position, radius, axis, color)
	points.append(points[0])
	polyline(points, color, thickness)


func bulk_circles(circles:Array) -> void:
	for c in circles:
		circle(c[0], c[1], c[2], c[3], c[4])