GDscript projectile motion - attempt 2

extends CharacterBody3D

var sensitivity := Vector2(0.2, 0.2)

@export var camera : Camera3D
@export var debug : MeshInstance3D
@onready var marker := $Marker3D

# Player related controls
const SPEED = 5.0
const JUMP_VELOCITY = 4.5

# Projectile prediction parameters
const SHAPE_RAYCAST_RADIUS := 0.3
const LINE_POINT_COUNT : int = 50
const TIME_BETWEEN_POINTS : float = .45

const PROJECTILE_POWER = 8
const STARTING_POSITION_MULTIPLIER = 5

# Get the gravity from the project settings to be synced with RigidBody nodes.
var gravity: float = ProjectSettings.get_setting("physics/3d/default_gravity")


func _physics_process(delta: float) -> void:
	calculate_projectile()
	# ... player movement code

var query = PhysicsShapeQueryParameters3D.new()
func _ready() -> void:
	query.shape_rid = PhysicsServer3D.sphere_shape_create()
	query.exclude = [self]
	PhysicsServer3D.shape_set_data(query.shape_rid, SHAPE_RAYCAST_RADIUS)


func calculate_projectile():
#	(v0=initial velocity Vector3, t=time/simulated time passed, and g=gravity)
#	x = v0.x * t * cos(theta),
#	y = v0.y * t * sin(theta) - 1/2 * g * t^2
#	z = v0.z * t * cos(theta),

	var starting_pos : Vector3 = marker.position
	# If in 3rd person, translate camera.transform to player's transform + 1 to the y, then get the basis.z
	var v0 := camera.basis.z * PROJECTILE_POWER
	var theta := starting_pos.angle_to(v0)

	print(starting_pos, " ",
		v0, " ",
		theta)
	var p : Vector3
	var time := 0.0
	for t in LINE_POINT_COUNT:
		# From a Unity tutorial which didn't work:
		# var new_point : Vector3 = starting_pos + (time * starting_velocity)
		# new_point.y = starting_pos.y + (starting_velocity.y * time) - (0.5 * gravity + pow(time, 2)

		var new_point := Vector3(
			v0.x * t * cos(theta),
			v0.y * t * sin(theta) - 1/2 * -GRAVITY * pow(t, 2),
			v0.z * t * cos(theta)
		)
		p = new_point
		# As new_point is local to Player, we move the query's location by the players position + new_point
		query.transform = global_transform.translated_local(new_point)

		# We use a query that's already been instantiated so we don't constantly recreate
		if not get_world_3d().direct_space_state.intersect_shape(query, 1).is_empty():
			# get the point where the shape first intersected. Should never be an empty dict.
			var collision := get_world_3d().direct_space_state.get_rest_info(query)
			if !collision.is_empty():
				debug.global_position = collision.point
			break

		debug.global_position = p
		time += TIME_BETWEEN_POINTS