-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add ability to instantiate multiple tracker source nodes.
- Loading branch information
1 parent
94c61a4
commit fb4464c
Showing
8 changed files
with
211 additions
and
138 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,36 @@ | ||
extends Node | ||
|
||
|
||
## Axis Studio Plugin Node | ||
## | ||
## This node provides an Axis Studio tracker as a plugin autoload singleton. | ||
|
||
|
||
# Tracker source | ||
var _source : AxisStudioSource | ||
|
||
|
||
# On entering the scene-tree, construct the tracker source and start listening | ||
# for incoming packets. | ||
func _enter_tree() -> void: | ||
# Get the body tracker name | ||
var body_tracker_name : String = ProjectSettings.get_setting( | ||
"axis_studio_tracker/tracking/body_tracker_name", | ||
"/axis_studio/body") | ||
|
||
# Get the position mode | ||
var position_mode = ProjectSettings.get_setting( | ||
"axis_studio_tracker/tracking/position_mode", | ||
0) | ||
|
||
# Get the MVN port number | ||
var udp_listener_port : int = ProjectSettings.get_setting( | ||
"axis_studio_tracker/network/udp_listener_port", | ||
7004) | ||
|
||
_source = AxisStudioSource.new(body_tracker_name, position_mode, udp_listener_port) | ||
|
||
|
||
# On frame processing, poll the tracker source for updates. | ||
func _process(_delta: float) -> void: | ||
_source.poll() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,145 @@ | ||
class_name AxisStudioSource | ||
extends Object | ||
|
||
|
||
## Axis Studio Tracker Script | ||
## | ||
## This script processes Axis Studio packets into XRBodyTracker data for | ||
## driving avatars. | ||
|
||
|
||
## Enumeration of position modes | ||
enum PositionMode { | ||
FREE, ## Free movement | ||
CALIBRATE, ## Calibrate horizontal position on the first frame | ||
LOCKED ## Lock horizontal position | ||
} | ||
|
||
|
||
## Body tracking flags | ||
const BODY_TRACKING := \ | ||
XRBodyTracker.BODY_FLAG_UPPER_BODY_SUPPORTED | \ | ||
XRBodyTracker.BODY_FLAG_LOWER_BODY_SUPPORTED | \ | ||
XRBodyTracker.BODY_FLAG_HANDS_SUPPORTED | ||
|
||
## Joint tracking flags | ||
const JOINT_TRACKING := \ | ||
XRBodyTracker.JOINT_FLAG_ORIENTATION_TRACKED | \ | ||
XRBodyTracker.JOINT_FLAG_ORIENTATION_VALID | \ | ||
XRBodyTracker.JOINT_FLAG_POSITION_TRACKED | \ | ||
XRBodyTracker.JOINT_FLAG_POSITION_VALID | ||
|
||
|
||
# Axis Studio reader instance | ||
var _axis_studio_reader : AxisStudioReader = AxisStudioReader.new() | ||
|
||
# Body tracker instance to publish tracking data | ||
var _body_tracker : XRBodyTracker = XRBodyTracker.new() | ||
|
||
# Position mode | ||
var _position_mode : PositionMode = PositionMode.FREE | ||
|
||
# Array of joint absolute positions | ||
var _abs_positions : Array[Vector3] = [] | ||
|
||
# Array of joint absolute rotations | ||
var _abs_rotations : Array[Quaternion] = [] | ||
|
||
# Position calibration | ||
var _position_calibration : Vector3 = Vector3.ZERO | ||
|
||
# Calibrated flag | ||
var _position_calibrated : bool = false | ||
|
||
|
||
# On initialization, construct and register the body tracker and start listening | ||
# for incoming packets. | ||
func _init( | ||
body_tracker_name : String, | ||
position_mode : int, | ||
udp_listener_port : int) -> void: | ||
|
||
# Fill the position and rotation arrays | ||
_abs_positions.resize(AxisStudioBody.Joint.COUNT) | ||
_abs_rotations.resize(AxisStudioBody.Joint.COUNT) | ||
_abs_positions.fill(Vector3.ZERO) | ||
_abs_rotations.fill(Quaternion.IDENTITY) | ||
|
||
# Register the body tracker | ||
XRServer.add_body_tracker(body_tracker_name, _body_tracker) | ||
|
||
# Save the position mode | ||
_position_mode = position_mode | ||
|
||
# Start listening for VMC packets | ||
_axis_studio_reader.on_axis_studio_packet.connect(_on_axis_studio_packet) | ||
_axis_studio_reader.listen(udp_listener_port) | ||
|
||
|
||
# Poll for incoming packets | ||
func poll() -> void: | ||
_axis_studio_reader.poll() | ||
|
||
|
||
# Handle received Axis Studio packet data | ||
func _on_axis_studio_packet(data : AxisStudioBody.JointData) -> void: | ||
# Flatten the joint tree to absolute positions | ||
for joint in AxisStudioBody.Joint.COUNT: | ||
# Get the relative position data and parent | ||
var parent_joint := AxisStudioBody.JOINT_PARENT[joint] | ||
var pos := data.positions[joint] | ||
var rot := data.rotations[joint] | ||
|
||
# If hips then consider position calibration | ||
if joint == AxisStudioBody.Joint.HIPS: | ||
match _position_mode: | ||
PositionMode.CALIBRATE: | ||
# Calibrate on first position | ||
if not _position_calibrated: | ||
_position_calibrated = true | ||
_position_calibration = pos.slide(Vector3.UP) | ||
|
||
# Apply calibration | ||
pos -= _position_calibration | ||
|
||
PositionMode.LOCKED: | ||
# Project position to vertical axis | ||
pos = pos.project(Vector3.UP) | ||
|
||
# If child-joint then convert relative to absolute | ||
if parent_joint >= 0: | ||
var parent_pos := _abs_positions[parent_joint] | ||
var parent_rot := _abs_rotations[parent_joint] | ||
pos = parent_pos + parent_rot * pos | ||
rot = parent_rot * rot | ||
|
||
# Save absolute position | ||
_abs_positions[joint] = pos | ||
_abs_rotations[joint] = rot | ||
|
||
# Apply to the XRBodyTracker | ||
for joint in AxisStudioBody.JOINT_MAPPING: | ||
var body : XRBodyTracker.Joint = joint["body"] | ||
var native : AxisStudioBody.Joint = joint["native"] | ||
var roll : Quaternion = joint["roll"] | ||
|
||
# Set the joint transform | ||
_body_tracker.set_joint_transform( | ||
body, | ||
Transform3D( | ||
Basis(_abs_rotations[native] * roll), | ||
_abs_positions[native])) | ||
|
||
# Set the joint flags | ||
_body_tracker.set_joint_flags(body, JOINT_TRACKING) | ||
|
||
# Calculate and set the root joint under the hips | ||
var root := _body_tracker.get_joint_transform(XRBodyTracker.JOINT_HIPS) | ||
root.basis = Basis.IDENTITY | ||
root.origin = root.origin.slide(Vector3.UP) | ||
_body_tracker.set_joint_transform(XRBodyTracker.JOINT_ROOT, root) | ||
_body_tracker.set_joint_flags(XRBodyTracker.JOINT_ROOT, JOINT_TRACKING) | ||
|
||
# Indicate we are tracking the body | ||
_body_tracker.body_flags = BODY_TRACKING | ||
_body_tracker.has_tracking_data = true |
154 changes: 19 additions & 135 deletions
154
addons/godot_axis_studio_tracker/axis_studio_tracker.gd
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,150 +1,34 @@ | ||
class_name AxisStudioTracker | ||
extends Node | ||
|
||
|
||
## Axis Studio Tracker Script | ||
## Axis Studio Tracker Node | ||
## | ||
## This script processes Axis Studio packets into XRBodyTracker data for | ||
## driving avatars. | ||
## This node provides an Axis Studio tracker as a scene-tree node. It may also | ||
## be instantiated as an autoload to provide for multiple trackers on different | ||
## ports. | ||
|
||
|
||
## Enumeration of position modes | ||
enum PositionMode { | ||
FREE, ## Free movement | ||
CALIBRATE, ## Calibrate horizontal position on the first frame | ||
LOCKED ## Lock horizontal position | ||
} | ||
## Body tracker name | ||
@export var body_tracker_name : String = "/mvn/body" | ||
|
||
## Position mode | ||
@export_enum("Free", "Calibrate", "Locked") var position_mode : int = 0 | ||
|
||
## Body tracking flags | ||
const BODY_TRACKING := \ | ||
XRBodyTracker.BODY_FLAG_UPPER_BODY_SUPPORTED | \ | ||
XRBodyTracker.BODY_FLAG_LOWER_BODY_SUPPORTED | \ | ||
XRBodyTracker.BODY_FLAG_HANDS_SUPPORTED | ||
## UDP listener port | ||
@export var udp_listener_port : int = 7004 | ||
|
||
## Joint tracking flags | ||
const JOINT_TRACKING := \ | ||
XRBodyTracker.JOINT_FLAG_ORIENTATION_TRACKED | \ | ||
XRBodyTracker.JOINT_FLAG_ORIENTATION_VALID | \ | ||
XRBodyTracker.JOINT_FLAG_POSITION_TRACKED | \ | ||
XRBodyTracker.JOINT_FLAG_POSITION_VALID | ||
|
||
|
||
# Axis Studio reader instance | ||
var _axis_studio_reader : AxisStudioReader = AxisStudioReader.new() | ||
|
||
# Body tracker instance to publish tracking data | ||
var _body_tracker : XRBodyTracker = XRBodyTracker.new() | ||
|
||
# Position mode | ||
var _position_mode : PositionMode = PositionMode.FREE | ||
|
||
# Array of joint absolute positions | ||
var _abs_positions : Array[Vector3] = [] | ||
|
||
# Array of joint absolute rotations | ||
var _abs_rotations : Array[Quaternion] = [] | ||
|
||
# Position calibration | ||
var _position_calibration : Vector3 = Vector3.ZERO | ||
|
||
# Calibrated flag | ||
var _position_calibrated : bool = false | ||
# Tracker source | ||
var _source : AxisStudioSource | ||
|
||
|
||
# On entering the scene-tree, construct the tracker source and start listening | ||
# for incoming packets. | ||
func _enter_tree() -> void: | ||
# Fill the position and rotation arrays | ||
_abs_positions.resize(AxisStudioBody.Joint.COUNT) | ||
_abs_rotations.resize(AxisStudioBody.Joint.COUNT) | ||
_abs_positions.fill(Vector3.ZERO) | ||
_abs_rotations.fill(Quaternion.IDENTITY) | ||
|
||
# Get the body tracker name | ||
var body_tracker_name : String = ProjectSettings.get_setting( | ||
"axis_studio_tracker/tracking/body_tracker_name", | ||
"/axis_studio/body") | ||
|
||
# Register the body tracker | ||
XRServer.add_body_tracker(body_tracker_name, _body_tracker) | ||
|
||
# Get the position mode | ||
_position_mode = ProjectSettings.get_setting( | ||
"axis_studio_tracker/tracking/position_mode", | ||
0) | ||
|
||
# Get the Axis Studio port number | ||
var udp_listener_port : int = ProjectSettings.get_setting( | ||
"axis_studio_tracker/network/udp_listener_port", | ||
7004) | ||
|
||
# Start listening for VMC packets | ||
_axis_studio_reader.on_axis_studio_packet.connect(_on_axis_studio_packet) | ||
_axis_studio_reader.listen(udp_listener_port) | ||
|
||
|
||
func _process(_delta : float) -> void: | ||
# Poll for incoming packets | ||
_axis_studio_reader.poll() | ||
|
||
|
||
# Handle received Axis Studio packet data | ||
func _on_axis_studio_packet(data : AxisStudioBody.JointData) -> void: | ||
# Flatten the joint tree to absolute positions | ||
for joint in AxisStudioBody.Joint.COUNT: | ||
# Get the relative position data and parent | ||
var parent_joint := AxisStudioBody.JOINT_PARENT[joint] | ||
var pos := data.positions[joint] | ||
var rot := data.rotations[joint] | ||
|
||
# If hips then consider position calibration | ||
if joint == AxisStudioBody.Joint.HIPS: | ||
match _position_mode: | ||
PositionMode.CALIBRATE: | ||
# Calibrate on first position | ||
if not _position_calibrated: | ||
_position_calibrated = true | ||
_position_calibration = pos.slide(Vector3.UP) | ||
|
||
# Apply calibration | ||
pos -= _position_calibration | ||
|
||
PositionMode.LOCKED: | ||
# Project position to vertical axis | ||
pos = pos.project(Vector3.UP) | ||
|
||
# If child-joint then convert relative to absolute | ||
if parent_joint >= 0: | ||
var parent_pos := _abs_positions[parent_joint] | ||
var parent_rot := _abs_rotations[parent_joint] | ||
pos = parent_pos + parent_rot * pos | ||
rot = parent_rot * rot | ||
|
||
# Save absolute position | ||
_abs_positions[joint] = pos | ||
_abs_rotations[joint] = rot | ||
|
||
# Apply to the XRBodyTracker | ||
for joint in AxisStudioBody.JOINT_MAPPING: | ||
var body : XRBodyTracker.Joint = joint["body"] | ||
var native : AxisStudioBody.Joint = joint["native"] | ||
var roll : Quaternion = joint["roll"] | ||
|
||
# Set the joint transform | ||
_body_tracker.set_joint_transform( | ||
body, | ||
Transform3D( | ||
Basis(_abs_rotations[native] * roll), | ||
_abs_positions[native])) | ||
|
||
# Set the joint flags | ||
_body_tracker.set_joint_flags(body, JOINT_TRACKING) | ||
_source = AxisStudioSource.new(body_tracker_name, position_mode, udp_listener_port) | ||
|
||
# Calculate and set the root joint under the hips | ||
var root := _body_tracker.get_joint_transform(XRBodyTracker.JOINT_HIPS) | ||
root.basis = Basis.IDENTITY | ||
root.origin = root.origin.slide(Vector3.UP) | ||
_body_tracker.set_joint_transform(XRBodyTracker.JOINT_ROOT, root) | ||
_body_tracker.set_joint_flags(XRBodyTracker.JOINT_ROOT, JOINT_TRACKING) | ||
|
||
# Indicate we are tracking the body | ||
_body_tracker.body_flags = BODY_TRACKING | ||
_body_tracker.has_tracking_data = true | ||
# On frame processing, poll the tracker source for updates. | ||
func _process(_delta: float) -> void: | ||
_source.poll() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,6 @@ | ||
[gd_scene load_steps=2 format=3 uid="uid://bnotov251uro6"] | ||
|
||
[ext_resource type="Script" path="res://addons/godot_axis_studio_tracker/axis_studio_tracker.gd" id="1_48k32"] | ||
|
||
[node name="AxisStudioTracker" type="Node"] | ||
script = ExtResource("1_48k32") |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters