unity GPS coordinates to (X,Y,Z), rotating north

Question

I'm making an app with Unity3D which uses GPS coordinates to place a "thing" in the 3D world, as you walk in real world close to it, you will find it getting close. kinda like PokemonGO

So the problem is: I managed to place the object at the right place and distance, using a function dist(lat1,lon1,lat2,lon2) which give me the distance in meters between the 2 coordinates (see below).

What i want to do next is to make the scene turn around the Y axis in order to head north (Z axis+ should be pointing North). how do i do this?

What i did was this:

float xx = dist (0, Tlon, 0, lon), zz = dist (Tlat, 0, lat, 0);
Vector3 position = new Vector3 (xx, 0, zz);
position.Normalize ();
float beta = Mathf.Acos(position.z) * Mathf.Rad2Deg;
position = Quaternion.Euler (0,beta-Input.compass.trueHeading,0)*position;
transform.position = position;

But it does weird stuff, the "right" position is both at my right and at my left (why? idk)

{dist function, used to calculate distances between 2 coords:}

float dist(float lat1, float lon1, float lat2, float lon2){  // generally used geo measurement function
    var R = 6378.137; // Radius of earth in KM
    var dLat = (lat2 - lat1) * Mathf.PI / 180;
    var dLon = (lon2 - lon1) * Mathf.PI / 180;
    var a = Mathf.Sin(dLat/2) * Mathf.Sin(dLat/2) +
        Mathf.Cos(lat1 * Mathf.PI / 180) * Mathf.Cos(lat2 * Mathf.PI / 180) *
        Mathf.Sin(dLon/2) * Mathf.Sin(dLon/2);
    var c = 2 * Mathf.Atan2(Mathf.Sqrt(a), Mathf.Sqrt(1-a));
    var d = R * c;
    return (float)(d * 1000); // meters
}

Answer 1

Simplest way to do this will be if you will have every map object assigned to empty as a parent and then rotate. This way you only need set transform.rotation = Quaternion.Euler (0,Input.compass.trueHeading,0) on empty as

Input.compass.trueHeading - The heading in degrees relative to the geographic North Pole

Looking at your code it should look like:

float xx = dist (0, Tlon, 0, lon), zz = dist (Tlat, 0, lat, 0);
Vector3 position = new Vector3 (xx, 0, zz);
position.Normalize ();
position = Quaternion.AngleAxis(Input.compass.trueHeading, Vector3.up) * position;
transform.position = position;

EDIT. I don't know why in example is a minus, maybe it should be in my code too.

transform.rotation = Quaternion.Euler(0, -Input.compass.trueHeading, 0);