diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs index 22eb0bb..58c1a1a 100644 --- a/LinearAlgebra/src/SwingTwist.cs +++ b/LinearAlgebra/src/SwingTwist.cs @@ -29,11 +29,18 @@ namespace LinearAlgebra { } #if UNITY_5_3_OR_NEWER + public static SwingTwist FromQuaternion(Quaternion q) { + // q.ToAngles(out float right, out float up, out float forward); + UnityEngine.Vector3 angles = q.eulerAngles; + SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z); + return r; + } + public Quaternion ToQuaternion() { Quaternion q = Quaternion.Euler(-this.swing.vertical, this.swing.horizontal, this.twist); - return q; + return q; } #endif } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 03ac532..326aa7c 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -88,6 +88,7 @@ namespace RoboidControl.Unity { /// protected virtual void FixedUpdate() { UpdateThing(); + core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation); } /// diff --git a/src/Thing.cs b/src/Thing.cs index 03b11f5..e642388 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -340,6 +340,7 @@ namespace RoboidControl { if (_orientation != value) { _orientation = value; orientationUpdated = true; + updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); //OnOrientationChanged?.Invoke(); } }