add first few vmc messages

src/lib.rs

@@ -1,14 +1,306 @@
-pub fn add(left: usize, right: usize) -> usize {
+use godot::{
-    left + right
+    builtin::{Basis, Quaternion},
+    prelude::{Transform3D, Vector3},
+};
+use rosc::{OscMessage, OscType};
+
+pub trait MessageBehavior {
+    fn to_sendable_osc_message(&self) -> OscMessage;
+    fn parse_from_osc_message(msg: OscMessage) -> Result<Self, String>
+    where
+        Self: Sized;
 }
 
-#[cfg(test)]
+struct VmcExtOk {
-mod tests {
+    loaded: i32,
-    use super::*;
+    calibration_state: Option<i32>,
+    calibration_mode: Option<i32>,
+    tracking_status: Option<i32>,
+}
 
-    #[test]
+impl MessageBehavior for VmcExtOk {
-    fn it_works() {
+    fn to_sendable_osc_message(&self) -> OscMessage {
-        let result = add(2, 2);
+        let addr = String::from("/VMC/Ext/OK");
-        assert_eq!(result, 4);
+        let mut args: Vec<OscType> = vec![OscType::from(self.loaded)];
+        if self.calibration_state.is_some() {
+            args.append(&mut vec![
+                OscType::from(self.calibration_state.unwrap()),
+                OscType::from(self.calibration_mode.unwrap()),
+            ]);
+        }
+        if self.tracking_status.is_some() {
+            args.push(OscType::from(self.tracking_status.unwrap()));
+        }
+        return OscMessage { addr, args };
+    }
+    fn parse_from_osc_message(msg: OscMessage) -> Result<Self, String> {
+        let mut result = VmcExtOk {
+            loaded: -1,
+            calibration_state: None,
+            calibration_mode: None,
+            tracking_status: None,
+        };
+        if msg.args.len() == 1 || msg.args.len() == 3 || msg.args.len() == 4 {
+            match msg.args[0] {
+                OscType::Int(i) => {
+                    result.loaded = i;
+                }
+                _ => {
+                    return Err(String::from("arg type invalid"));
+                }
+            }
+        } else {
+            return Err(String::from("arg count invalid"));
+        }
+        if msg.args.len() == 3 || msg.args.len() == 4 {
+            match msg.args[1] {
+                OscType::Int(i) => {
+                    result.calibration_state = Some(i);
+                }
+                _ => {
+                    return Err(String::from("arg type invalid"));
+                }
+            }
+            match msg.args[2] {
+                OscType::Int(i) => {
+                    result.calibration_mode = Some(i);
+                }
+                _ => {
+                    return Err(String::from("arg type invalid"));
+                }
+            }
+        }
+        if msg.args.len() == 4 {
+            match msg.args[3] {
+                OscType::Int(i) => {
+                    result.tracking_status = Some(i);
+                }
+                _ => {
+                    return Err(String::from("arg type invalid"));
+                }
+            }
+        }
+        return Ok(result);
+    }
+}
+
+struct VmcExtT {
+    time: f32,
+}
+
+impl MessageBehavior for VmcExtT {
+    fn to_sendable_osc_message(&self) -> OscMessage {
+        return OscMessage {
+            addr: String::from("/VMC/Ext/T"),
+            args: vec![OscType::from(self.time)],
+        };
+    }
+    fn parse_from_osc_message(msg: OscMessage) -> Result<Self, String> {
+        if msg.args.len() != 1 {
+            return Err(String::from("arg count invalid"));
+        }
+        match msg.args[0] {
+            OscType::Float(f) => return Ok(VmcExtT { time: f }),
+            _ => return Err(String::from("arg type invalid")),
+        }
+    }
+}
+
+struct VmcExtRootPos {
+    name: String,
+    transform: Transform3D,
+    mr_scale: Option<Vector3>,
+    mr_offset: Option<Vector3>,
+}
+
+impl MessageBehavior for VmcExtRootPos {
+    fn to_sendable_osc_message(&self) -> OscMessage {
+        let addr = String::from("/VMC/Ext/Root/Pos");
+        let quat = self.transform.basis.to_quat();
+        let mut args: Vec<OscType> = vec![
+            OscType::from(self.name.to_owned()),
+            OscType::from(self.transform.origin.x),
+            OscType::from(self.transform.origin.y),
+            OscType::from(self.transform.origin.z),
+            OscType::from(quat.x),
+            OscType::from(quat.y),
+            OscType::from(quat.z),
+            OscType::from(quat.w),
+        ];
+        if self.mr_offset.is_some() {
+            args.append(&mut vec![
+                OscType::from(self.mr_scale.unwrap().x),
+                OscType::from(self.mr_scale.unwrap().y),
+                OscType::from(self.mr_scale.unwrap().z),
+                OscType::from(self.mr_offset.unwrap().x),
+                OscType::from(self.mr_offset.unwrap().y),
+                OscType::from(self.mr_offset.unwrap().z),
+            ]);
+        }
+        return OscMessage { addr, args };
+    }
+    fn parse_from_osc_message(msg: OscMessage) -> Result<Self, String> {
+        let mut result: VmcExtRootPos;
+        if msg.args.len() == 8 || msg.args.len() == 14 {
+            let mut origin = Vector3::new(0.0, 0.0, 0.0);
+            let name: String;
+            match &msg.args[0] {
+                OscType::String(s) => name = s.to_owned(),
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[1] {
+                OscType::Float(f) => origin.x = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[2] {
+                OscType::Float(f) => origin.y = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[3] {
+                OscType::Float(f) => origin.z = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            let mut quat = Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                w: 0.0,
+            };
+            match msg.args[4] {
+                OscType::Float(f) => quat.x = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[5] {
+                OscType::Float(f) => quat.y = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[6] {
+                OscType::Float(f) => quat.z = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[7] {
+                OscType::Float(f) => quat.w = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            result = VmcExtRootPos {
+                name,
+                transform: Transform3D::new(Basis::from_quat(quat), origin),
+                mr_scale: None,
+                mr_offset: None,
+            };
+        } else {
+            return Err(String::from("arg count invalid"));
+        }
+        if msg.args.len() == 14 {
+            result.mr_scale = Some(Vector3 {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+            });
+            result.mr_offset = Some(Vector3 {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+            });
+            match msg.args[8] {
+                OscType::Float(f) => result.mr_scale.unwrap().x = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[9] {
+                OscType::Float(f) => result.mr_scale.unwrap().y = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[10] {
+                OscType::Float(f) => result.mr_scale.unwrap().z = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[11] {
+                OscType::Float(f) => result.mr_offset.unwrap().x = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[12] {
+                OscType::Float(f) => result.mr_offset.unwrap().y = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+            match msg.args[13] {
+                OscType::Float(f) => result.mr_offset.unwrap().z = f,
+                _ => return Err(String::from("arg type invalid")),
+            }
+        }
+        return Ok(result);
+    }
+}
+
+struct VmcExtBonePos {
+    name: String,
+    transform: Transform3D,
+}
+
+impl MessageBehavior for VmcExtBonePos {
+    fn to_sendable_osc_message(&self) -> OscMessage {
+        let addr = String::from("/VMC/Ext/Bone/Pos");
+        let quat = self.transform.basis.to_quat();
+        return OscMessage {
+            addr,
+            args: vec![
+                OscType::from(self.name.to_owned()),
+                OscType::from(self.transform.origin.x),
+                OscType::from(self.transform.origin.y),
+                OscType::from(self.transform.origin.z),
+                OscType::from(quat.x),
+                OscType::from(quat.y),
+                OscType::from(quat.z),
+                OscType::from(quat.w),
+            ],
+        };
+    }
+    fn parse_from_osc_message(msg: OscMessage) -> Result<Self, String> {
+        let name: String;
+        let mut origin = Vector3::new(0.0, 0.0, 0.0);
+        if msg.args.len() != 8 {
+            return Err(String::from("arg count invalid"));
+        }
+        match &msg.args[0] {
+            OscType::String(s) => name = s.to_owned(),
+            _ => return Err(String::from("arg type invalid")),
+        }
+        match msg.args[1] {
+            OscType::Float(f) => origin.x = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        match msg.args[2] {
+            OscType::Float(f) => origin.y = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        match msg.args[3] {
+            OscType::Float(f) => origin.z = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        let mut quat = Quaternion {
+            x: 0.0,
+            y: 0.0,
+            z: 0.0,
+            w: 0.0,
+        };
+        match msg.args[4] {
+            OscType::Float(f) => quat.x = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        match msg.args[5] {
+            OscType::Float(f) => quat.y = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        match msg.args[6] {
+            OscType::Float(f) => quat.z = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        match msg.args[7] {
+            OscType::Float(f) => quat.w = f,
+            _ => return Err(String::from("arg type invalid")),
+        }
+        return Ok(VmcExtBonePos {
+            name,
+            transform: Transform3D::new(Basis::from_quat(quat), origin),
+        });
     }
 }