RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Merge commit '33b1c0cf52774541fa2eea0c008750193f14737c'

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-03 17:25:18 +02:00
commit 29b37d91c1
2 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
LinearAlgebra/src/SwingTwist.cs
View File

@ -33,12 +33,11 @@ namespace LinearAlgebra {
return r; return r;
} }
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
public static SwingTwist FromQuaternion(Quaternion q) { public static SwingTwist FromQuaternion(Quaternion q) {
// q.ToAngles(out float right, out float up, out float forward); // q.ToAngles(out float right, out float up, out float forward);
UnityEngine.Vector3 angles = q.eulerAngles; 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; return r;
} }
@ -46,7 +45,6 @@ namespace LinearAlgebra {
Quaternion q = Quaternion.Euler(-this.swing.vertical, Quaternion q = Quaternion.Euler(-this.swing.vertical,
this.swing.horizontal, this.swing.horizontal,
-this.twist); -this.twist);
System.Console.Write($"{q.eulerAngles}");
return q; return q;
} }
#endif #endif

6
src/Messages/PoseMsg.cs
View File

@ -108,10 +108,10 @@ namespace RoboidControl {
if ((this.poseType & Pose_Position) != 0) if ((this.poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_Orientation) != 0) 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); this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
if ((this.poseType & Pose_LinearVelocity) != 0) 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) if ((this.poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
} }
@ -133,7 +133,7 @@ namespace RoboidControl {
if ((poseType & Pose_Position) != 0) if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position); LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
if ((poseType & Pose_Orientation) != 0) 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); LowLevelMessages.SendSwingTwist(buffer, ref ix, this.orientation);
if ((poseType & Pose_LinearVelocity) != 0) if ((poseType & Pose_LinearVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);