Xamarin Orientation Sensor Vector Angle

问题

I'm using Xamarin.Essentials.OrientationSensor in my project and I'm having trouble interpreting the Quaternion the API gives me on new readings.

Problem Description

Here's a figure describing my problem:

I need to determine an orientation vector v being normal/orthogonal to the phone's plane (i.e. a vector coming out of the back of the phone). I then need to transform this vector v into a 3D state space being (latitude, longitude, altitude). This will enable me to compute the angle between the phone's orientation and the distance vector d to a certain fix point F, where d = F - P. The angle can then be obtained by using dot product, cosinus and the magnitudes of v and d.

What I have so far

According to the official docs, the phone's local coordinate system describes as follows:

The device (generally a phone or tablet) has a 3D coordinate system with the following axes: The positive X axis points to the right of the display in portrait mode. The positive Y axis points to the top of the device in portrait mode. The positive Z axis points out of the screen.

This means that the vector v I'm looking for would be v=(0,0,-1) in the phone's local coordinate system.

In this question, I already learned, that the given quaternions are relative and I need a reference quaternion centerQ for my transformations:

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

And then I could use Quaternion.Transform(v, centerQ) to transform v into the earth's coordinate system, which according to the docs is defined as

The 3D coordinate system of the Earth has the following axes: The positive X axis is tangent to the surface of the Earth and points east. The positive Y axis is also tangent to the surface of the Earth and points north. The positive Z axis is perpendicular to the surface of the Earth and points up.

However, I need to transform v into WGS+altitude, and this is where it gets tricky.

I tried to define two additional quaternions for transformations from the earth's coordinate system - as described in the docs - to WGS as follows:

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

Because latitude is basically a rotation around the x-axis and longitude around the y-axis, respectively.

And then I would have to multiply the three quaternions centerQ, qLat and qLon. However, I've tried all possible combinations of orders between the three and the results all don't make any sense. I know that quaternion multiplication is not commutative.

Here's my code so far

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

I'd appreciate any hint or thought on this problem.

来源：https://stackoverflow.com/questions/54589349/xamarin-orientation-sensor-vector-angle