C# Vector3 四元数/Matrix4x4 旋转

Question

我有一个传感器，可以根据 ISO 8855-2011 在坐标系中传递对象数据。 我的传感器位于 [0, 0, 0]，但可能“未对准”。因此我想补偿横滚、俯仰和偏航。

在我的应用程序中，我使用 System.Numerics Lib 执行必要的旋转，更具体地说，使用 vector3 作为数据点 ([x, y, z])，并尝试使用 Matrix4x4 或四元数作为旋转矩阵。

在没有评估最终结果的情况下，三步轮换似乎效果很好（乍一看似乎是我的可视化中所期望的）。为了执行旋转，我首先为每个轴指定三个四元数，然后一个接一个地执行旋转。

Vector3 vecMeas = new Vector3(x, y, z);
Quaternion rotRoll = Quaternion.CreateFromAxisAngle(new Vector3(1.0f, 0.0f, 0.0f), rollAngle);
Quaternion rotPitch = Quaternion.CreateFromAxisAngle(new Vector3(0.0f, 1.0f, 0.0f), pitchAngle);
Quaternion rotYaw = Quaternion.CreateFromAxisAngle(new Vector3(0.0f, 0.0f, 1.0f), yawAngle);


var vecRolled = Vector3.Transform(vecMeas, rotRoll);
var vecPitched = Vector3.Transform(vecRolled, rotPitch);
var vecResult = Vector3.Transform(vecPitched, rotYaw);

虽然这似乎有效，但对我来说看起来很笨拙，因为类 Matrix4x4 和 Quaternion 提供了一种名为“CreateFromYawPitchRoll”的方法。我尝试了该方法的每种输入组合（如果我没记错的话，有 6 种俯仰、横滚、偏航顺序的组合，以及 [x, y, z] 的顺序和符号的 48 种输入组合），但无法只需 3 步即可产生与我的旋转相同的结果。

示例（约 288 种组合之一）：

Vector3 vecMeas = new Vector3(x, y, z);
Quaternion rot = Quaternion.CreateFromYawPitchRoll(pitch, roll, yaw);
var vecResult = Vector3.Transform(vecMeas, rot);

上面显示的“一步”解决方案总是看起来不对。

不幸的是，我无法找到有关 Matrix4x4 / 四元数的预期输入的旋向性（或不存在旋向性）或执行“CreateFromYawPitchRoll”旋转的顺序的大量信息。您是否知道如何重构我的代码以使其更加优雅（也可能是一步解决方案）或如何正确使用 CreateFromYawPitchRoll？旋转元素的乘法没有提供预期的结果（rotRoll * rotPitch * rotYaw）。

提前致谢！

更新： 抱歉，我想我已经明白了。 就我而言，我必须执行以下操作：

Quaternion rot = Quaternion.CreateFromYawPitchRoll(yawAngle, pitchAngle, rollAngle);
var vecRot = Vector3.Transform(new Vector3(Y, Z, X), rot);

结果将是：

var vecRes = new Vector3(vecRot.Z, vecRot.X, vecRot.Y);

Answer 1

或者可以做

Vector3 vecMeas = new Vector3(x, y, z);
// CreateFromYawPitchRoll expects different rotation axis, so use Pitch/Roll/Yaw (instead of Roll Pitch Yaw) 
var rotationQ = Quaternion.CreateFromYawPitchRoll((float)pitchInRadians, (float)rollInRadians, (float)yawInRadians);
var vecRes = System.Numerics.Vector3.Transform(vecMeas, rotationQ);