well this is insane but it tracks the hand now

examples/xr.rs

@@ -1,3 +1,5 @@
+use std::f32::consts::PI;
+
 use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin};
 use bevy::prelude::*;
 use bevy::transform::components::Transform;
@@ -16,13 +18,13 @@ fn main() {
     info!("Running `openxr-6dof` skill");
     App::new()
         .add_plugins(DefaultXrPlugins)
-        .add_plugins(OpenXrDebugRenderer) //new debug renderer adds gizmos to
+        //.add_plugins(OpenXrDebugRenderer) //new debug renderer adds gizmos to
         .add_plugins(LogDiagnosticsPlugin::default())
         .add_plugins(FrameTimeDiagnosticsPlugin)
         .add_systems(Startup, setup)
         .add_systems(Update, proto_locomotion)
         .add_systems(Startup, spawn_controllers_example)
-        .add_systems(Update, spawn_skeleton)
+        .add_systems(Update, draw_skeleton_hand)
         .insert_resource(PrototypeLocomotionConfig::default())
         .run();
 }
@@ -70,7 +72,24 @@ fn setup(
     },));
 }
 
-fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) {
+fn draw_skeleton_hand(mut commands: Commands,
+    mut gizmos: Gizmos,
+    right_controller_query: Query<(
+    &GlobalTransform,
+    With<OpenXRRightController>,
+)>, ) {
+    //draw debug for controller grip center to match palm to
+    let right_transform = right_controller_query.get_single().unwrap().0;
+    let right_translation = right_transform.compute_transform().translation;
+    let right_quat = right_transform.compute_transform().rotation;
+    gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK);
+    //we need to flip this i dont know why
+    let flip = Quat::from_rotation_x(PI);
+    let flap = Quat::from_rotation_z(-45.0*(PI/180.0));
+    let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y);
+    let controller_backward = right_quat.mul_quat(flip).mul_quat(flap);
+    gizmos.ray(right_translation, controller_forward, Color::PINK);
+
     let hand_pose: [Posef; 26] = [
         Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y:  0.849, z: 0.204, w:  0.407}}, //palm
         Posef { position: Vector3f {x: -0.548, y: 0.161,  z: -0.137}, orientation: Quaternionf {x: -0.267, y:  0.849, z: 0.204, w:  0.407}},
@@ -99,74 +118,81 @@ fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) {
         Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002,  y: -0.745, z: 0.444, w:  -0.498}}, 
         Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045,  y: -0.780, z: 0.378, w:  -0.496}},
     ];
-    //cursed wrist math
-    let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 };
-    let huh = Quaternionf {x: -0.267,y:  0.849, z: 0.204,w:  0.407}.to_quat();
-    let why: Vec3 = huh.mul_vec3(wrist_dist);
 
-    let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 };
+    //cursed wrist math
+    let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015};
+
+    //cursed offset
+    let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 };
+    let offset = right_translation;
+
+    //old stuff dont touch for now
     let palm = hand_pose[HandJoint::PALM];
-    gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE);
+    gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE);
-    let wrist = hand_pose[HandJoint::WRIST];
+
-    gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY);
+    let rotated_wfp =  palm.position.to_vec3()  + right_quat.mul_quat(flip).mul_vec3(wrist_dist);
+    gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY);
+
+
     let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL];
-    gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED);
+    draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset);
+
     let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL];
-    gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED);
+    draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset);
     let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL];
-    gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED);
+    draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset);
     let thumb_tip = hand_pose[HandJoint::THUMB_TIP];
-    gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED);
+    draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset);
 
     let index_meta = hand_pose[HandJoint::INDEX_METACARPAL];
-    gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE);
+    draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset);
     let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL];
-    gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE);
+    draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset);
     let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE];
-    gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE);
+    draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset);
     let index_dist = hand_pose[HandJoint::INDEX_DISTAL];
-    gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE);
+    draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset);
     let index_tip = hand_pose[HandJoint::INDEX_TIP];
-    gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE);
+    draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset);
 
     let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL];
-    gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW);
+    draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset);
     let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL];
-    gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW);
+    draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset);
     let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE];
-    gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW);
+    draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset);
     let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL];
-    gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW);
+    draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset);
     let middle_tip = hand_pose[HandJoint::MIDDLE_TIP];
-    gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW);
+    draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset);
 
     let ring_meta = hand_pose[HandJoint::RING_METACARPAL];
-    gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN);
+    draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset);
     let ring_prox = hand_pose[HandJoint::RING_PROXIMAL];
-    gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN);
+    draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset);
     let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE];
-    gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN);
+    draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset);
     let ring_dist = hand_pose[HandJoint::RING_DISTAL];
-    gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN);
+    draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset);
     let ring_tip = hand_pose[HandJoint::RING_TIP];
-    gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN);
+    draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset);
 
     let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL];
-    gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE);
+    draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset);
     let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL];
-    gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE);
+    draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset);
     let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE];
-    gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE);
+    draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset);
     let little_dist = hand_pose[HandJoint::LITTLE_DISTAL];
-    gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE);
+    draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset);
     let little_tip = hand_pose[HandJoint::LITTLE_TIP];
-    gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE);
+    draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset);
-        
-    
-    
 
 
+}
 
+fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) {
+    gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color);
 }