Xamarin 方向传感器矢量角

我在用着Xamarin.Essentials.OrientationSensor在我的项目中，我无法解释QuaternionAPI 为我提供了新的读物。

问题描述

Here's a figure describing my problem:

我需要确定方向向量v与手机平面垂直/正交（即从手机背面出来的矢量）。然后我需要转换这个向量v进入 3D 状态空间（纬度、经度、海拔）。这将使我能够计算手机方向和距离向量之间的角度d到某个固定点F, where d = F - P。然后可以通过使用点积、余弦和大小来获得角度v and d.

到目前为止我所拥有的

根据官方文档 https://learn.microsoft.com/en-us/xamarin/essentials/orientation-sensor，手机的本地坐标系描述如下：

设备（通常是手机或平板电脑）具有 3D 坐标系 具有以下轴：正 X 轴指向右侧 以纵向模式显示。 Y 轴正方向指向顶部 设备处于纵向模式。正 Z 轴指向 屏幕。

这意味着向量v我正在寻找的是v=(0,0,-1)在手机的本地坐标系中。

In 这个问题 https://stackoverflow.com/questions/54540095/xamarin-orientation-sensor-quaternion，我已经了解到，给定的四元数是相对的，我需要一个参考四元数centerQ对于我的转变：

void onNewOrientation(Quaternion q)
{
    if (centerQ == Quaternion.Identity)
    {
        centerQ = Quaternion.Inverse(q);
        return;
    }
}

然后我可以使用Quaternion.Transform(v, centerQ)转变v进入地球坐标系，根据文档定义为

地球的 3D 坐标系有以下轴： 正 X 轴与地球表面相切并且点 东方。正 Y 轴也与地球表面相切 并指向北。正 Z 轴垂直于表面 地球并指向上方。

然而我需要转型v进入 WGS+海拔，这就是棘手的地方。

我试图定义两个额外的四元数对于从地球坐标系（如文档中所述）到 WGS 的转换，如下所示：

var phi = -1 * lastloc.Latitude * Util.DEGTORAD;
var lam = lastloc.Longitude * Util.DEGTORAD;

var qLat = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), (float)phi);
var qLon = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)lam);

因为纬度基本上分别是绕 x 轴旋转和经度绕 y 轴旋转。

然后我必须将三个四元数相乘centerQ, qLat and qLon。然而，我已经尝试了这三者之间所有可能的顺序组合，结果都没有任何意义。我知道四元数乘法是不可交换的。

到目前为止，这是我的代码

namespace Foo.ViewModels
{
    public class DebugViewModel : BaseViewModel
    {
        private Quaternion centerQ;
        private static Vector3 vec3out = new Vector3(0f, 0f, -1f);
        private static Location fixPos;
        private Location lastloc;

        public DebugViewModel()
        {
            centerQ = Quaternion.Identity;
            lastloc = null;

            fixPos = new Location()
            {
                Latitude = 0,  // anonymized
                Longitude = 0, // anonymized
                Altitude = 0   // anonymized
            };

            // Create reactive observable
            Observable.FromEventPattern<OrientationSensorChangedEventArgs>(
                    ev => OrientationSensor.ReadingChanged += ev,
                    ev => OrientationSensor.ReadingChanged -= ev)
                .Select(eventPattern => eventPattern.EventArgs.Reading.Orientation)
                //.Throttle(TimeSpan.FromMilliseconds(20))
                .Subscribe(this.onNewOrientation);

            OrientationSensor.Start(SensorSpeed.UI);
        }

        void onNewOrientation(Quaternion q)
        {
            if (centerQ == Quaternion.Identity)
            {
                centerQ = Quaternion.Inverse(q);
                return;
            }

            if (lastloc == null)
            {
                return;
            }

            var qi = Quaternion.Inverse(q);
            var qq = Quaternion.Multiply(centerQ, q);

            // get local quaternion
            var phi = -1 * lastloc.Latitude * Util.DEGTORAD;
            var lam = lastloc.Longitude * Util.DEGTORAD;

            var qLat = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), (float)phi);
            var qLon = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)lam);
            var qLati = Quaternion.Inverse(qLat);
            var qLoni = Quaternion.Inverse(qLon);

            var q1 = Quaternion.Multiply(qLat, qLon);
            var q2 = Quaternion.Multiply(q1, qq);

            Vector3 aa = Vector3.Transform(vec3out, q2);

            Vector3 myP = new Vector3(
                (float)lastloc.Latitude, 
                (float)lastloc.Longitude, 
                (float)lastloc.Altitude);

            Vector3 drP = new Vector3(
                (float)twrPos.Latitude,
                (float)twrPos.Longitude,
                (float)twrPos.Altitude);

            Vector3 d = Vector3.Normalize(drP - myP);

            float alpha = Util.vecAngle(d, aa) * (float)Util.RADTODEG;
        }
    }
}

我很感激有关这个问题的任何提示或想法。