108 lines
3.7 KiB
C#
108 lines
3.7 KiB
C#
using UnityEngine;
|
|
using ARLocation;
|
|
using System;
|
|
|
|
public class ARPostionManager : MonoBehaviour
|
|
{
|
|
// Start is called before the first frame update
|
|
|
|
|
|
public static Vector3 GetGameObjectPositionForLocation(Transform arLocationRoot, Vector3 userPosition, Location userLocation, Location objectLocation, bool heightIsRelative)
|
|
{
|
|
|
|
//1. ARLocation库原本计算矢量的方法,FirstLocation作为坐标系原点(0,0,0)
|
|
//var displacementVector = VectorFromTo(userLocation, objectLocation, objectLocation.IgnoreAltitude || heightIsRelative)
|
|
// .toVector3();
|
|
//var displacementPosition = arLocationRoot ? arLocationRoot.TransformVector(displacementVector) : displacementVector;
|
|
//return userPosition + displacementPosition + new Vector3(0, (heightIsRelative && !objectLocation.IgnoreAltitude) ? ((float)objectLocation.Altitude - userPosition.y) : 0, 0);
|
|
|
|
//2 以ARMainCamera作为作为坐标系原点(0,0,0)的矢量计算方法
|
|
var displacementVector = VectorFromTo(userLocation, objectLocation, objectLocation.IgnoreAltitude || heightIsRelative)
|
|
.toVector3();
|
|
return displacementVector;
|
|
|
|
|
|
//3 以ARMainCamera作为作为坐标系原点(0,0,0)的矢量计算方法，计算出的世界坐标经过相机的旋转转换到相机目前的坐标系
|
|
//Quaternion quaternionMainCamera = ARLocationManager.Instance.MainCamera.transform.rotation;
|
|
//Vector3 displacementVectorRotated = ARLocationManager.Instance.MainCamera.transform.InverseTransformDirection(displacementVector); //(3.66, 1.90, 9.68)
|
|
//return displacementVectorRotated;
|
|
}
|
|
|
|
|
|
private static DVector3 VectorFromTo(Location l1, Location l2, bool ignoreHeight = false)
|
|
{
|
|
var horizontal = HorizontalVectorFromTo(l1, l2);
|
|
var height = l2.Altitude - l1.Altitude;
|
|
|
|
return new DVector3(horizontal.y, ignoreHeight ? 0 : height, horizontal.x);
|
|
}
|
|
|
|
private static DVector2 HorizontalVectorFromTo(Location l1, Location l2)
|
|
{
|
|
#if ARGPS_USE_CUSTOM_GEO_CALC
|
|
return ArGpsCustomGeoCalc.HorizontalVectorFromTo(l1, l2);
|
|
#else
|
|
return VectorFromToEcefEnu(l1, l2);
|
|
|
|
#endif
|
|
}
|
|
|
|
|
|
private static DVector2 VectorFromToEcefEnu(Location l1, Location l2)
|
|
{
|
|
var rad = Math.PI / 180;
|
|
var lat = l1.Latitude * rad;
|
|
var lon = l1.Longitude * rad;
|
|
var p1 = LocationToEcef(l1);
|
|
var p2 = LocationToEcef(l2);
|
|
var delta = p2 - p1;
|
|
|
|
var slat = Math.Sin(lat);
|
|
var clat = Math.Cos(lat);
|
|
var slon = Math.Sin(lon);
|
|
var clon = Math.Cos(lon);
|
|
|
|
var e = -slon * delta.x + clon * delta.y;
|
|
var n = -clon * slat * delta.x - slat * slon * delta.y + clat * delta.z;
|
|
|
|
return new DVector2(n, e);
|
|
}
|
|
|
|
|
|
private static DVector3 LocationToEcef(Location l)
|
|
{
|
|
var rad = Math.PI / 180;
|
|
|
|
var lat = l.Latitude * rad;
|
|
var lon = l.Longitude * rad;
|
|
var a = ARLocation.ARLocation.Config.EarthEquatorialRadiusInKM * 1000;
|
|
var e2 = ARLocation.ARLocation.Config.EarthFirstEccentricitySquared;
|
|
var N = a / Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(lat), 2));
|
|
|
|
var x = N * Math.Cos(lat) * Math.Cos(lon);
|
|
var y = N * Math.Cos(lat) * Math.Sin(lon);
|
|
var z = (1 - e2) * N * Math.Sin(lat);
|
|
|
|
return new DVector3(x, y, z);
|
|
}
|
|
|
|
|
|
|
|
void Start()
|
|
{
|
|
//LocationReading locReadFirst = ARLocationManager.Instance.arLocationProvider.Provider.FirstLocation;
|
|
//Location userLocationFirst = locReadFirst.ToLocation();
|
|
|
|
//LocationReading locReadCurrent = ARLocationManager.Instance.arLocationProvider.Provider.CurrentLocation;
|
|
//Location userLocationCurrent = locReadCurrent.ToLocation();
|
|
|
|
|
|
}
|
|
|
|
// Update is called once per frame
|
|
void Update()
|
|
{
|
|
|
|
}
|
|
}
|