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;
}
#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

6
src/Messages/PoseMsg.cs
View File

@ -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);