Code

extends ARVRController

onready var grab_area = $Area
onready var grab_raycast = $GrabCast
onready var grab_pos = $GrabPos
onready var controller_mesh = $Controller
onready var Tele_raycast : RayCast = $TeleCast

var controller_vel = Vector3(0,0,0)
var prior_controller_pos = Vector3(0,0,0)
var prior_controller_vel = []

var held_object = null
var held_object_data = {"mode":RigidBody.MODE_RIGID, "layer":1, "mask":1}

var grab_mode = "AREA"
var tele_pos
var tele_mesh
var tele_button_down

const CONT_DEADZONE = 0.5
const MOVEMENT_SPEED = 2.5
#var directional_move = false

func _ready():
	tele_mesh = get_tree().root.get_node("Game/Teleport_ShowPos")
	tele_button_down = false

	grab_mode = "AREA"
	get_node("Sleep_Area").connect("body_entered",self,"sleep_area_entered")
	get_node("Sleep_Area").connect("body_exited",self,"sleep_area_exited")

	connect("button_pressed",self,"button_pressed")
	connect("button_release",self,"button_released")

func _physics_process(delta):
	if tele_button_down:
		Tele_raycast.force_raycast_update() # Maybe Error here?
		if Tele_raycast.is_colliding(): # Error Here. Won't detect Collision.
			print("Tele Raycast hit!")
			if Tele_raycast.get_collider() is StaticBody:
				print("Tele Raycast hit StaticBody!")
				if Tele_raycast.get_collision_normal().y >= 0.85:
					tele_pos = Tele_raycast.get_collision_point()
					print("Hit Location: ",tele_pos)
					tele_mesh.global_transform.origin = tele_pos

	if get_is_active():
		controller_vel = Vector3(0,0,0)
		if prior_controller_vel.size() > 0:
			for vel in prior_controller_vel:
				controller_vel += vel
			controller_vel = controller_vel / prior_controller_vel.size()
		prior_controller_vel.append((global_transform.origin - prior_controller_pos)/delta)
		controller_vel += (global_transform.origin - prior_controller_pos) / delta
		prior_controller_pos = global_transform.origin
		if prior_controller_vel.size() >= 30:
			prior_controller_vel.remove(0)

	if held_object:
		var held_scale = held_object.scale
		held_object.global_transform = grab_pos.global_transform
		held_object.scale = held_scale

	# Directional Movement Code Here

func button_pressed(button_index):
	if button_index == 15:
		if held_object:
			if held_object.has_method("interact"):
				held_object.interact()
		else:
			if not tele_mesh.visible and not held_object:
				tele_button_down = true
				tele_mesh.visible = true
				Tele_raycast.visible = true
	if button_index == 2:
		if tele_button_down:
			return
		if not held_object:
			var RB = null
			if grab_mode == "AREA":
				var bodies = grab_area.get_overlapping_bodies()
				if len(bodies) > 0:
					for body in bodies:
						if body is RigidBody:
							if not "NO_PICKUP" in body:
								RB = body
								break
			elif grab_mode == "RAYCAST":
				grab_raycast.force_raycast_update()
				if grab_raycast.is_colliding():
					if grab_raycast.get_collider() is RigidBody and not "NO_PICKUP" in grab_raycast.gey_collider():
						RB = grab_raycast.get_collider()
			if RB:
				held_object = RB
				held_object_data["mode"] = held_object.mode
				held_object_data["layer"] = held_object.collision_layer
				held_object_data["mask"] = held_object.collision_mask
				held_object.mode = RigidBody.MODE_STATIC
				held_object.collision_layer = 0
				held_object.collision_mask = 0
				controller_mesh.visible = false
				grab_raycast.visible = false
				if held_object.has_method("picked_up"):
					held_object.picked_up()
				if "controller" in held_object:
					held_object.controller = self
		else:
			held_object.mode = held_object_data["mode"]
			held_object.collision_layer = held_object_data["layer"]
			held_object.collision_mask = held_object_data["mask"]
			held_object.apply_impulse(Vector3(0,0,0), controller_vel)
			if held_object.has_method("dropped"):
				held_object.dropped()
			if "controller" in held_object:
				held_object.controller = null
			held_object = null
			controller_mesh.visible = true
			if grab_mode == "RAYCAST":
				grab_raycast.visible = true;
	if button_index == 1:
		if grab_mode == "AREA":
			grab_mode = "RAYCAST"
			if not held_object:
				grab_raycast.visible = true
		else:
			grab_mode = "AREA"
			grab_raycast.visible = false
		print("Grab Mode changed to ",grab_mode," on ",get_name())

func button_released(button_index):
	if button_index == 15:
		if tele_button_down:
			if tele_pos and tele_mesh.visible:
				var camera_offset = get_parent().get_node("Player_Camera").global_transform.origin - get_parent().global_transform.origin
				camera_offset.y = 0
				get_parent().global_transform.origin = tele_pos - camera_offset
			tele_button_down = false
			tele_mesh.visible = false
			Tele_raycast.visible = false
			tele_pos = null

func sleep_area_entered(body):
	if "can_sleep" in body:
		body.can_sleep = false
		body.sleeping = false
func sleep_area_exited(body):
	if "can_sleep" in body:
		body.can_sleep = false