From 0353e6f2cfbc706ceeb614273cc26adb7e7d7bb6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 3 Jun 2025 17:21:25 +0200 Subject: [PATCH] HumanoidControl support --- LinearAlgebra/src/Direction.cs | 18 +++++----- LinearAlgebra/src/SwingTwist.cs | 11 ++++-- Unity/Thing.cs | 58 +++++++++++++++++++++----------- src/Messages/LowLevelMessages.cs | 23 +++++++++---- src/Messages/PoseMsg.cs | 8 +++-- 5 files changed, 78 insertions(+), 40 deletions(-) diff --git a/LinearAlgebra/src/Direction.cs b/LinearAlgebra/src/Direction.cs index 297eff9..d544d90 100644 --- a/LinearAlgebra/src/Direction.cs +++ b/LinearAlgebra/src/Direction.cs @@ -3,8 +3,7 @@ using System; using Vector3Float = UnityEngine.Vector3; #endif -namespace LinearAlgebra -{ +namespace LinearAlgebra { /// /// A direction in 3D space @@ -25,10 +24,10 @@ namespace LinearAlgebra horizontal = 0; vertical = 0; } - public Direction(Angle horizontal, Angle vertical) { - this.horizontal = horizontal.inDegrees; + // public Direction(Angle horizontal, Angle vertical) { + // this.horizontal = horizontal.inDegrees; - } + // } // public Direction(float horizontal, float vertical) { // this.horizontal = horizontal; // this.vertical = vertical; @@ -40,7 +39,7 @@ namespace LinearAlgebra horizontal = horizontal, vertical = vertical }; - //Normalize(); + d.Normalize(); return d; } public static Direction Radians(float horizontal, float vertical) { @@ -48,7 +47,7 @@ namespace LinearAlgebra horizontal = horizontal * Angle.Rad2Deg, vertical = vertical * Angle.Rad2Deg }; - //Normalize(); + d.Normalize(); return d; } @@ -60,14 +59,15 @@ namespace LinearAlgebra public readonly static Direction right = Degrees(90, 0); public void Normalize() { + this.vertical = Angles.Normalize(this.vertical); if (this.vertical > 90 || this.vertical < -90) { this.horizontal += 180; this.vertical = 180 - this.vertical; } + this.horizontal = Angles.Normalize(this.horizontal); } - public Vector3Float ToVector3() - { + public Vector3Float ToVector3() { float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad; float horizontalRad = this.horizontal * Angle.Deg2Rad; float cosVertical = (float)Math.Cos(verticalRad); diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs index 58c1a1a..04aa57a 100644 --- a/LinearAlgebra/src/SwingTwist.cs +++ b/LinearAlgebra/src/SwingTwist.cs @@ -28,18 +28,25 @@ namespace LinearAlgebra { return r; } + public static SwingTwist Degrees(float horizontalSwing, float verticalSwing, float twist) { + SwingTwist r = new(horizontalSwing, verticalSwing, twist); + 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 SwingTwist(angles.y, angles.x, angles.z); + SwingTwist r = new(angles.y, -angles.x, -angles.z); return r; } public Quaternion ToQuaternion() { Quaternion q = Quaternion.Euler(-this.swing.vertical, this.swing.horizontal, - this.twist); + -this.twist); + System.Console.Write($"{q.eulerAngles}"); return q; } #endif diff --git a/Unity/Thing.cs b/Unity/Thing.cs index a54adb7..035b3ae 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -238,24 +238,41 @@ namespace RoboidControl.Unity { } private void ScanForThings(Transform rootTransform) { - // Thing[] thingArray = allThings.ToArray(); + RoboidControl.Thing[] thingArray = this.core.owner.things.ToArray(); - // for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) { - // Thing thing = thingArray[thingIx]; - // GameObject foundObj = FindThingByName(thing, rootTransform); - // if (foundObj != null && foundObj != thing.gameObject) { - // Thing foundThing = foundObj.GetComponent(); - // if (foundThing == null) { - // allThings.Remove(thing); + for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) { + RoboidControl.Thing thing = thingArray[thingIx]; + GameObject foundObj = FindThingByName(thing, rootTransform); + if (foundObj != null && foundObj != thing.component.gameObject) { + Thing foundThing = foundObj.GetComponent(); + if (foundThing == null) { + Debug.Log($"move thing [{thing.owner.networkId}/{thing.id}] to {foundObj.name}"); + foundThing = foundObj.AddComponent(); + foundThing.core = thing; + foundThing.core.position = LinearAlgebra.Spherical.FromVector3(foundObj.transform.localPosition); + foundThing.core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(foundObj.transform.localRotation); - // foundThing = foundObj.AddComponent(); - // foundThing.networkId = thing.networkId; - // foundThing.objectId = thing.objectId; - // allThings.Add(foundThing); - // Destroy(thing.gameObject); - // } - // } - // } + Destroy(thing.component.gameObject); + thing.component = foundThing; + } + } + } + } + + private GameObject FindThingByName(RoboidControl.Thing thing, Transform rootTransform) { + if (rootTransform == null || thing == null) + return null; + + if (rootTransform.name == thing.name) + return rootTransform.gameObject; + + for (int childIx = 0; childIx < rootTransform.childCount; childIx++) { + Transform child = rootTransform.GetChild(childIx); + GameObject foundObj = FindThingByName(thing, child); + if (foundObj != null) + return foundObj; + } + return null; } @@ -266,10 +283,11 @@ namespace RoboidControl.Unity { /// If a velocity is not zero, the position and/or orientation update will be ignored protected virtual void HandlePose() { this.transform.localRotation = core.orientation.ToQuaternion(); - if (core.linearVelocity.distance == 0) - this.transform.localPosition = core.position.ToVector3(); - if (core.angularVelocity.distance == 0) - this.transform.localRotation = core.orientation.ToQuaternion(); + this.transform.localPosition = core.position.ToVector3(); + // if (core.linearVelocity.distance == 0) + // this.transform.localPosition = core.position.ToVector3(); + // if (core.angularVelocity.distance == 0) + // this.transform.localRotation = core.orientation.ToQuaternion(); } diff --git a/src/Messages/LowLevelMessages.cs b/src/Messages/LowLevelMessages.cs index e8c8062..4f90d75 100644 --- a/src/Messages/LowLevelMessages.cs +++ b/src/Messages/LowLevelMessages.cs @@ -34,25 +34,36 @@ namespace RoboidControl { buffer[ix++] = (byte)qw; } public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) { - Quat32 q = new Quat32( + Quat32 q32 = new Quat32( (data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F, data[ix++] / 255.0F); - return q; + System.Console.Write($"receive q32: {q32.x} {q32.y} {q32.z} {q32.w}"); + return q32; } public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) { Quat32 q32 = Quat32.FromSwingTwist(s); + System.Console.Write($"send q32: {q32.x} {q32.y} {q32.z} {q32.w}"); SendQuat32(buffer, ref ix, q32); + } + + public static void SendSwingTwist(byte[] buffer, ref byte ix, SwingTwist r) { + System.Console.Write($"send st: {r.swing.horizontal} {r.swing.vertical} {r.twist}"); + + SendAngle8(buffer, ref ix, r.swing.horizontal); + SendAngle8(buffer, ref ix, r.swing.vertical); + SendAngle8(buffer, ref ix, r.twist); } public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) { - Quat32 q32 = ReceiveQuat32(data, ref ix); - // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); - // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + float horizontal = ReceiveAngle8(data, ref ix); + float vertical = ReceiveAngle8(data, ref ix); + float twist = ReceiveAngle8(data, ref ix); + System.Console.Write($"receive st: {horizontal} {vertical} {twist}"); - SwingTwist r = SwingTwist.FromQuat32(q32); + SwingTwist r = SwingTwist.Degrees(horizontal, vertical, twist); return r; } diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs index f0d24a9..fac392e 100644 --- a/src/Messages/PoseMsg.cs +++ b/src/Messages/PoseMsg.cs @@ -108,7 +108,8 @@ 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); if ((this.poseType & Pose_AngularVelocity) != 0) @@ -132,9 +133,10 @@ 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); + LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); if ((poseType & Pose_AngularVelocity) != 0) LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); return ix;