38 lines
1.5 KiB
C#
38 lines
1.5 KiB
C#
 
|
using UnityEngine;
|
||
|
|
|
||
|
|
namespace HexapodSim
|
||
|
|
{
|
||
|
|
// Which Unity axis the robot's forward (+X) maps to. The old scene marks
|
||
|
|
// the legs on Unity -Z as "invertForLeftLeg", which makes -X forward.
|
||
|
|
public enum ForwardAxis { NegativeX, PositiveX }
|
||
|
|
|
||
|
|
// Conversions between the firmware's body frame (right-handed, mm:
|
||
|
|
// +X forward, +Y left, +Z up) and Unity (left-handed, Y up). Everything
|
||
|
|
// physical in the simulation is computed in the robot frame and only
|
||
|
|
// converted here, in one place.
|
||
|
|
public static class RobotFrame
|
||
|
|
{
|
||
|
|
public static Vector3 ToUnity(Vector3 r, ForwardAxis f) =>
|
||
|
|
f == ForwardAxis.NegativeX ? new Vector3(-r.x, r.z, -r.y) : new Vector3(r.x, r.z, r.y);
|
||
|
|
|
||
|
|
public static Vector3 ToRobot(Vector3 u, ForwardAxis f) =>
|
||
|
|
f == ForwardAxis.NegativeX ? new Vector3(-u.x, -u.z, u.y) : new Vector3(u.x, u.z, u.y);
|
||
|
|
|
||
|
|
// A rotation expressed in the robot frame, as the equivalent Unity rotation.
|
||
|
|
// (The mapping is a reflection, so this is M * R * M^-1, built from two axes.)
|
||
|
|
public static Quaternion ToUnity(Quaternion robotRot, ForwardAxis f)
|
||
|
|
{
|
||
|
|
Vector3 fwd = ToUnity(robotRot * ToRobot(Vector3.forward, f), f);
|
||
|
|
Vector3 up = ToUnity(robotRot * ToRobot(Vector3.up, f), f);
|
||
|
|
return Quaternion.LookRotation(fwd, up);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static float WrapDeg(float d)
|
||
|
|
{
|
||
|
|
d %= 360f;
|
||
|
|
if (d > 180f) d -= 360f;
|
||
|
|
if (d < -180f) d += 360f;
|
||
|
|
return d;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|