Xamarin Датчик направления вектора угла

Question

Я использую Xamarin.Essentials.OrientationSensor в моем проекте, и у меня возникают проблемы с интерпретацией Quaternion API дает мне новые показания.

описание проблемы

Вот рисунок, описывающий мою проблему:

Мне нужно определить вектор ориентации v быть нормальным / ортогональным к плоскости телефона (т.е. вектор, выходящий из задней части телефона). Затем мне нужно преобразовать этот вектор v в трехмерном пространстве состояний (широта, долгота, высота). Это позволит мне вычислить угол между ориентацией телефона и вектором расстояния d до определенной точки F, где d = F - P, Угол может быть получен с помощью скалярного произведения, косинуса и величин v а также d,

Что у меня пока

Согласно официальным документам, локальная система координат телефона описывается следующим образом:

Устройство (как правило, телефон или планшет) имеет 3D-систему координат со следующими осями: Положительная ось X указывает вправо от дисплея в портретном режиме. Положительная ось Y указывает на верхнюю часть устройства в портретном режиме. Положительная ось Z указывает на экран.

Это означает, что вектор v Я ищу будет V =(0,0,-1) в локальной системе координат телефона.

В этом вопросе я уже узнал, что данные кватернионы являются относительными и мне нужен эталонный кватернион centerQ для моих преобразований:

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

И тогда я мог бы использовать Quaternion.Transform(v, centerQ) преобразовывать v в систему координат Земли, которая в соответствии с документами определяется как

Трехмерная система координат Земли имеет следующие оси: Положительная ось 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 а также 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;
        }
    }
}

Буду признателен за любую подсказку или мысль по этой проблеме.