diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs index 04aa57a..025b2d9 100644 --- a/LinearAlgebra/src/SwingTwist.cs +++ b/LinearAlgebra/src/SwingTwist.cs @@ -33,12 +33,11 @@ namespace LinearAlgebra { return r; } - #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(angles.y, -angles.x, -angles.z); + SwingTwist r = Degrees(angles.y, -angles.x, -angles.z); return r; } @@ -46,7 +45,6 @@ namespace LinearAlgebra { Quaternion q = Quaternion.Euler(-this.swing.vertical, this.swing.horizontal, -this.twist); - System.Console.Write($"{q.eulerAngles}"); return q; } #endif diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs index fac392e..1ee0b34 100644 --- a/src/Messages/PoseMsg.cs +++ b/src/Messages/PoseMsg.cs @@ -108,10 +108,10 @@ namespace RoboidControl { if ((this.poseType & Pose_Position) != 0) this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); if ((this.poseType & Pose_Orientation) != 0) - // this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix)); + //this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix)); this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix); if ((this.poseType & Pose_LinearVelocity) != 0) - this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); if ((this.poseType & Pose_AngularVelocity) != 0) this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); } @@ -133,7 +133,7 @@ namespace RoboidControl { if ((poseType & Pose_Position) != 0) LowLevelMessages.SendSpherical(buffer, ref ix, this.position); if ((poseType & Pose_Orientation) != 0) - // LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); + //LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); LowLevelMessages.SendSwingTwist(buffer, ref ix, this.orientation); if ((poseType & Pose_LinearVelocity) != 0) LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);