Physics.gd

extends Node3D


var space_state : PhysicsDirectSpaceState3D
var default_layer_mask : int = 429467295
var sphere_shape : SphereShape3D = SphereShape3D.new()
var capsule_shape : CapsuleShape3D = CapsuleShape3D.new()


func _ready():
    process_priority = -100


func _physics_process(_delta):
    space_state = get_world_3d().direct_space_state

# Works, can have problems if include_overlap==false and called while shape is touching collider where cast_motion()
# thinks a collision should happen at cast_motion_data[1] but get_rest_info() at cast_motion_data[1] doesn't record it
func shape_cast(shape: Shape3D, shape_transform: Transform3D, destination: Vector3, out_results: Dictionary = {}, include_overlap: bool = false, layer_mask: int = default_layer_mask, collide_areas:bool = true, collide_bodies:bool = true) -> bool:
    var query : PhysicsShapeQueryParameters3D = PhysicsShapeQueryParameters3D.new()
    query.collide_with_areas = collide_areas
    query.collide_with_bodies = collide_bodies
    query.shape = shape
    query.collision_mask = layer_mask
    query.transform = shape_transform
    if include_overlap:
        var hit_data : Dictionary = space_state.get_rest_info(query)
        if hit_data.size() != 0:
            out_results["point"] = hit_data["point"]
            out_results["normal"] = hit_data["normal"]
            out_results["safe_distance"] = 0
            out_results["hit_distance"] = 0
            return true
    query.motion = destination - shape_transform.origin
    var cast_motion_data : Array = space_state.cast_motion(query)
    var cast_collided : bool = !(cast_motion_data[0] == 1 && cast_motion_data[1] == 1)
    if cast_collided:
        query.transform.origin += query.motion * cast_motion_data[1]
        var hit_data : Dictionary = space_state.get_rest_info(query)
        if hit_data.size() != 0:
            out_results["point"] = hit_data["point"]
            out_results["normal"] = hit_data["normal"]
            out_results["safe_distance"] = cast_motion_data[0]
            out_results["hit_distance"] = cast_motion_data[1]
            return true
        else:
            print("cast_motion() predicted a hit but get_rest_info() didn't pick it up.")
            return false
    return false

# Works
func sphere_cast(origin: Vector3, destination: Vector3, radius: float, out_results: Dictionary = {}, include_overlap: bool = false, layer_mask: int = default_layer_mask, collide_areas: bool = true, collide_bodies: bool = true) -> bool:
    sphere_shape.radius = radius
    var shape_transform : Transform3D = Transform3D.IDENTITY
    shape_transform.origin = origin
    return shape_cast(sphere_shape, shape_transform, destination, out_results, include_overlap, layer_mask, collide_areas, collide_bodies)

# Works
func sphere_cast_direction(origin: Vector3, direction: Vector3, distance: float, radius: float, out_results: Dictionary = {}, include_overlap: bool = false, layer_mask: int = default_layer_mask, collide_areas: bool = true, collide_bodies: bool = true) -> bool:
    return sphere_cast(origin, origin + (direction * distance), radius, out_results, include_overlap, layer_mask, collide_areas, collide_bodies)

# Works
func capsule_cast(point1: Vector3, point2: Vector3, radius: float, direction: Vector3, distance: float, out_results: Dictionary = {}, include_overlap: bool = false, layer_mask: int = default_layer_mask, collide_areas: bool = true, collide_bodies: bool = true) -> bool:
    var point1_to_point2 = point2 - point1
    var point1_to_point2_length : float = (point1_to_point2).length()
    capsule_shape.height = point1_to_point2_length + radius * 2
    var shape_transform : Transform3D = Transform3D.IDENTITY
    shape_transform.origin = point1 + (point1_to_point2 * 0.5)
    shape_transform.basis = Basis(Quaternion(Vector3.UP, point1_to_point2 / point1_to_point2_length))
    return shape_cast(capsule_shape, shape_transform, shape_transform.origin + (direction.normalized() * distance), out_results, include_overlap, layer_mask, collide_areas, collide_bodies)