feat(spatial): set_transform_flex and associated helper functions

src/nodes/spatial.rs

@@ -1,7 +1,7 @@
 use super::core::Node;
 use crate::core::client::Client;
-use anyhow::{anyhow, bail, ensure, Result};
+use anyhow::{anyhow, bail, Result};
-use glam::Mat4;
+use glam::{Mat4, Quat, Vec3};
 use libstardustxr::flex::flexbuffer_from_vector_arguments;
 use libstardustxr::push_to_vec;
 use libstardustxr::{flex_to_quat, flex_to_vec3};
@@ -37,28 +37,10 @@ impl<'a> Spatial<'a> {
 		Ok(())
 	}
 
-	pub fn set_transform_flex(node: &Node, calling_client: Rc<Client>, data: &[u8]) -> Result<()> {
+	pub fn space_to_space_matrix(from: Option<&Spatial>, to: Option<&Spatial>) -> Mat4 {
-		let root = flexbuffers::Reader::get_root(data)?;
+		let space_to_world_matrix = from.map_or(Mat4::IDENTITY, |from| from.global_transform());
-		let flex_vec = root.get_vector()?;
+		let world_to_space_matrix = to.map_or(Mat4::IDENTITY, |to| to.global_transform().inverse());
-		let client = node
+		world_to_space_matrix * space_to_world_matrix
-			.get_client()
-			.ok_or_else(|| anyhow!("Node somehow has no client"))?;
-		let other_spatial = calling_client
-			.get_scenegraph()
-			.get_node(flex_vec.idx(0).as_str())
-			.ok_or_else(|| anyhow!("Other spatial node not found"))?;
-		ensure!(
-			other_spatial.borrow().spatial.is_some(),
-			"Node is not a Spatial!"
-		);
-		let pos = flex_to_vec3!(flex_vec.idx(1));
-		let rot = flex_to_quat!(flex_vec.idx(2));
-		let scl = flex_to_vec3!(flex_vec.idx(3));
-		node.spatial
-			.as_ref()
-			.unwrap()
-			.set_transform_components(other_spatial, pos, rot, scl);
-		Ok(())
 	}
 
 	pub fn local_transform(&self) -> Mat4 {
@@ -70,21 +52,35 @@ impl<'a> Spatial<'a> {
 			None => self.transform.get(),
 		}
 	}
-
+	pub fn set_local_transform(&self, transform: Mat4) {
-	pub fn set_transform_components(
+		self.transform.set(transform);
-		&self,
-		relative_space: RcCell<Node>,
-		pos: Option<mint::Vector3<f32>>,
-		rot: Option<mint::Quaternion<f32>>,
-		scl: Option<mint::Vector3<f32>>,
-	) {
-		todo!()
 	}
+	pub fn set_local_transform_components(
+		&self,
+		reference_space: Option<&Spatial>,
+		pos: Option<Vec3>,
+		rot: Option<Quat>,
+		scl: Option<Vec3>,
+	) {
+		let reference_to_parent_transform =
+			Spatial::space_to_space_matrix(reference_space, self.parent.borrow().as_deref());
+		let mut local_transform_in_reference_space =
+			reference_to_parent_transform.inverse() * self.local_transform();
+		let (mut reference_space_scl, mut reference_space_rot, mut reference_space_pos) =
+			local_transform_in_reference_space.to_scale_rotation_translation();
 
-	pub fn space_to_space_matrix(from: &Spatial, to: &Spatial) -> Mat4 {
+		pos.map(|pos| reference_space_pos = pos);
-		let space_to_world_matrix = from.global_transform();
+		rot.map(|rot| reference_space_rot = rot);
-		let world_to_space_matrix = to.global_transform().inverse();
+		scl.map(|scl| reference_space_scl = scl);
-		world_to_space_matrix * space_to_world_matrix
+
+		local_transform_in_reference_space = Mat4::from_scale_rotation_translation(
+			reference_space_scl,
+			reference_space_rot,
+			reference_space_pos,
+		);
+		self.set_local_transform(
+			reference_to_parent_transform * local_transform_in_reference_space,
+		);
 	}
 
 	pub fn get_transform_flex(
@@ -103,9 +99,11 @@ impl<'a> Spatial<'a> {
 			.and_then(|node| node.borrow().spatial.clone())
 			.ok_or_else(|| anyhow!("Space not found"))?;
 
-		let (scale, rotation, position) =
+		let (scale, rotation, position) = Spatial::space_to_space_matrix(
-			Spatial::space_to_space_matrix(this_spatial.as_ref(), relative_spatial.as_ref())
+			Some(this_spatial.as_ref()),
-				.to_scale_rotation_translation();
+			Some(relative_spatial.as_ref()),
+		)
+		.to_scale_rotation_translation();
 
 		Ok(flexbuffer_from_vector_arguments(|vec| {
 			push_to_vec!(
@@ -116,6 +114,37 @@ impl<'a> Spatial<'a> {
 			);
 		}))
 	}
+	pub fn set_transform_flex(node: &Node, calling_client: Rc<Client>, data: &[u8]) -> Result<()> {
+		let root = flexbuffers::Reader::get_root(data)?;
+		let flex_vec = root.get_vector()?;
+		let spatial = node
+			.spatial
+			.as_ref()
+			.ok_or_else(|| anyhow!("Node somehow is not spatial"))?;
+		let reference_space_path = flex_vec.idx(0).as_str();
+		let reference_space_transform = if reference_space_path != "" {
+			Some(
+				calling_client
+					.get_scenegraph()
+					.get_node(reference_space_path)
+					.ok_or_else(|| anyhow!("Other spatial node not found"))?
+					.borrow()
+					.spatial
+					.as_ref()
+					.ok_or_else(|| anyhow!("Node is not a Spatial!"))?
+					.clone(),
+			)
+		} else {
+			None
+		};
+		spatial.set_local_transform_components(
+			reference_space_transform.as_deref(),
+			flex_to_vec3!(flex_vec.idx(1)).map(|v| v.into()),
+			flex_to_quat!(flex_vec.idx(2)).map(|v| v.into()),
+			flex_to_vec3!(flex_vec.idx(3)).map(|v| v.into()),
+		);
+		Ok(())
+	}
 }
 
 pub fn create_interface(client: Rc<Client>) {