136 lines
3.3 KiB
GDScript
136 lines
3.3 KiB
GDScript
extends Node3D
|
|
class_name OriginHands
|
|
|
|
@export var tracking : UltraleapDeviceNode
|
|
@export var use_interpolation : bool = true
|
|
|
|
var gizmos : Array[Node3D] = []
|
|
var dry_run : bool = false
|
|
|
|
func _ready():
|
|
var scene : PackedScene = preload("res://models/Origin.tscn")
|
|
|
|
dry_run = true
|
|
var n : int = set_hand(UltraleapHand.new(), 0)
|
|
n = set_hand(UltraleapHand.new(), n)
|
|
dry_run = false
|
|
|
|
print(str(n) + " bones")
|
|
|
|
for i in range(n):
|
|
var instance : Node3D = scene.instantiate()
|
|
add_child(instance)
|
|
instance.owner = self
|
|
instance.name = "Joint" + str(i)
|
|
gizmos.append(instance)
|
|
|
|
|
|
func _process(_delta):
|
|
var n : int = 0
|
|
|
|
if tracking == null:
|
|
return
|
|
|
|
var frame : UltraleapFrame = tracking.get_last_frame()
|
|
|
|
# Dirty hack to check if the device is initialised.
|
|
# Should implement something like a flag later
|
|
if frame == null:
|
|
return
|
|
|
|
if use_interpolation:
|
|
var interpolated_frame : UltraleapFrame = tracking.get_interpolated_frame(Time.get_ticks_usec())
|
|
if interpolated_frame != null:
|
|
n = set_hand(interpolated_frame.left_hand, n)
|
|
n = set_hand(interpolated_frame.right_hand, n)
|
|
return
|
|
|
|
n = set_hand(frame.left_hand, n)
|
|
n = set_hand(frame.right_hand, n)
|
|
|
|
func set_hand(hand : UltraleapHand, index : int) -> int:
|
|
if hand == null:
|
|
hand = UltraleapHand.new()
|
|
|
|
index = set_finger("thumb", hand, index)
|
|
index = set_finger("index", hand, index)
|
|
index = set_finger("middle", hand, index)
|
|
index = set_finger("ring", hand, index)
|
|
index = set_finger("pinky", hand, index)
|
|
index = set_arm(hand, index)
|
|
|
|
return index
|
|
|
|
func set_finger(finger_name : String, hand : UltraleapHand, index : int) -> int:
|
|
var finger : UltraleapDigit = hand.get(finger_name)
|
|
|
|
if finger == null:
|
|
finger = UltraleapDigit.new()
|
|
|
|
if finger_name != "thumb":
|
|
index = set_bone("metacarpal", finger, index)
|
|
index = set_bone("proximal", finger, index)
|
|
index = set_bone("intermediate", finger, index)
|
|
index = set_bone("distal", finger, index)
|
|
index = set_tip(finger, index)
|
|
|
|
return index
|
|
|
|
func set_bone(bone_name : String, finger : UltraleapDigit, index : int):
|
|
if dry_run:
|
|
return index + 1
|
|
|
|
var bone : UltraleapBone = finger.get(bone_name)
|
|
|
|
if bone == null:
|
|
bone = UltraleapBone.new()
|
|
|
|
var bone_transform : Transform3D = tracking.global_transform * Transform3D(
|
|
Basis(bone.orientation),
|
|
bone.prev_joint
|
|
)
|
|
|
|
gizmos[index].global_transform = bone_transform
|
|
return index + 1
|
|
|
|
func set_tip(finger : UltraleapDigit, index : int) -> int:
|
|
if dry_run:
|
|
return index + 1
|
|
|
|
var tip : UltraleapBone = finger.get("distal")
|
|
|
|
if tip == null:
|
|
tip = UltraleapBone.new()
|
|
|
|
var tip_transform : Transform3D = tracking.global_transform * Transform3D(
|
|
Basis(tip.orientation),
|
|
tip.next_joint
|
|
)
|
|
|
|
gizmos[index].global_transform = tip_transform
|
|
return index + 1
|
|
|
|
func set_arm(hand : UltraleapHand, index : int) -> int:
|
|
if dry_run:
|
|
return index + 2
|
|
|
|
var arm : UltraleapBone = hand.arm
|
|
|
|
if arm == null:
|
|
arm = UltraleapBone.new()
|
|
|
|
var wrist_transform : Transform3D = tracking.global_transform * Transform3D(
|
|
Basis(arm.orientation),
|
|
arm.prev_joint
|
|
)
|
|
|
|
var elbow_transform : Transform3D = tracking.global_transform * Transform3D(
|
|
Basis(arm.orientation),
|
|
arm.next_joint
|
|
)
|
|
|
|
gizmos[index].global_transform = elbow_transform
|
|
gizmos[index + 1].global_transform = wrist_transform
|
|
|
|
return index + 2
|