116 lines
4.6 KiB
C#
116 lines
4.6 KiB
C#
|
using System;
|
|||
|
|
using ARLocation;
|
|||
|
|
using UnityEngine;
|
|||
|
|
using UnityEngine.XR.ARFoundation;
|
|||
|
|
|
|||
|
|
namespace Util
|
|||
|
|
{
|
|||
|
|
public class _Debugger : MonoBehaviour
|
|||
|
|
{
|
|||
|
|
public ARSessionOrigin arSessionOrigin;
|
|||
|
|
private ARLocationProvider locationProvider;
|
|||
|
|
const double R = 6378137.0; // WGS84<38><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뾶
|
|||
|
|
private const double e2 = 0.00669437999013; // <20><>һƫ<D2BB><C6AB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
|
|||
|
|
|
|||
|
|
public LineRenderer lineRenderer;
|
|||
|
|
public Transform capsule1;
|
|||
|
|
public Transform capsule2;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
Vector3 CalculateRelativeOffset(Vector2 currentA, Vector2 fixedB) {
|
|||
|
|
|
|||
|
|
double dLon = (fixedB.x - currentA.x) * Mathf.Deg2Rad;
|
|||
|
|
double dLat = (fixedB.y - currentA.y) * Mathf.Deg2Rad;
|
|||
|
|
|
|||
|
|
double x = R * dLon * Math.Cos((currentA.x + fixedB.x)/2 * Mathf.Deg2Rad);
|
|||
|
|
double z = R * dLat;
|
|||
|
|
return new Vector3((float)x, -2, (float)z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Start()
|
|||
|
|
{
|
|||
|
|
locationProvider = ARLocationProvider.Instance;
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD>߶ζ<DFB6><CEB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㹹<EFBFBD><E3B9B9>һ<EFBFBD><D2BB>ֱ<EFBFBD>ߣ<EFBFBD>
|
|||
|
|
lineRenderer.positionCount = 2;
|
|||
|
|
// // <20><><EFBFBD>ÿ<EFBFBD><C3BF>ȣ<EFBFBD><C8A3><EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȣ<EFBFBD>
|
|||
|
|
lineRenderer.startWidth = 0.1f;
|
|||
|
|
lineRenderer.endWidth = 0.1f;
|
|||
|
|
// // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɫ<EFBFBD><C9AB>֧<EFBFBD>ֽ<EFBFBD><D6BD>䣩
|
|||
|
|
lineRenderer.startColor = Color.red;
|
|||
|
|
lineRenderer.endColor = Color.blue;
|
|||
|
|
// // <20><><EFBFBD>ò<EFBFBD><C3B2>ʣ<EFBFBD><CAA3><EFBFBD>ָ<EFBFBD><D6B8>Shader<65><72>
|
|||
|
|
lineRenderer.material = new Material(Shader.Find("Sprites/Default"));
|
|||
|
|
}
|
|||
|
|
private void Update()
|
|||
|
|
{
|
|||
|
|
// Vector2 originPos =
|
|||
|
|
// new Vector2((float)locationProvider.CurrentLocation.longitude, (float)locationProvider.CurrentLocation.latitude);
|
|||
|
|
// Vector2 targetPos = new Vector2((float)117.13843113088807, (float)31.8732576897494);
|
|||
|
|
// Vector2 targetPos2 = new Vector2((float)117.1380031964643, (float)31.873258780793716);
|
|||
|
|
// var relativePos1 = CalculateRelativeOffset(originPos, targetPos);
|
|||
|
|
// var relativePos2 = CalculateRelativeOffset(originPos, targetPos2);
|
|||
|
|
// CustomLogger.LogWithTag($"<22><>ʱB1<42><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:{relativePos1}", 0);
|
|||
|
|
// CustomLogger.LogWithTag($"<22><>ʱB2<42><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:{relativePos2}", 0);
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD>꣨<EFBFBD><EAA3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
|
|||
|
|
// lineRenderer.SetPosition(0, relativePos1); // <20><><EFBFBD><EFBFBD>
|
|||
|
|
// lineRenderer.SetPosition(1, relativePos2);// <20>յ<EFBFBD>
|
|||
|
|
// capsule1.position = relativePos1;
|
|||
|
|
// capsule2.position = relativePos2;
|
|||
|
|
|
|||
|
|
// double latA = locationProvider.CurrentLocation.latitude;
|
|||
|
|
// double lonA = locationProvider.CurrentLocation.longitude;
|
|||
|
|
// double latB = 31.8732576897494;
|
|||
|
|
// double lonB = 117.13843113088807;
|
|||
|
|
// double latC = 31.873258780793716;
|
|||
|
|
// double lonC = 117.1380031964643;
|
|||
|
|
//
|
|||
|
|
// var relativePositionB = CalculateRelativePosition(latA, lonA, latB, lonB);
|
|||
|
|
// var relativePostionC = CalculateRelativePosition(latA, lonA, latC, lonC);
|
|||
|
|
// CustomLogger.LogWithTag($"<22><>ʱB<CAB1><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:{relativePositionB}", 0);
|
|||
|
|
// CustomLogger.LogWithTag($"<22><>ʱC<CAB1><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:{relativePostionC}", 0);
|
|||
|
|
// capsule1.position = relativePositionB;
|
|||
|
|
// capsule2.position = relativePostionC;
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
#region WGS2ECEF
|
|||
|
|
// WGS84תECEF
|
|||
|
|
private static Vector3 WGS84ToECEF(double lat, double lon)
|
|||
|
|
{
|
|||
|
|
double alt = 10;
|
|||
|
|
// <20><><EFBFBD><EFBFBD>γ<EFBFBD>ȴӶ<C8B4>תΪ<D7AA><CEAA><EFBFBD><EFBFBD>
|
|||
|
|
double latRad = Mathf.Deg2Rad * (float)lat;
|
|||
|
|
double lonRad = Mathf.Deg2Rad * (float)lon;
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮ<CBAE><D7BC><EFBFBD><EFBFBD><EFBFBD>ʰ뾶
|
|||
|
|
double N = R / Math.Sqrt(1 - e2 * Math.Sin(latRad) * Math.Sin(latRad));
|
|||
|
|
|
|||
|
|
// ת<><D7AA>Ϊ ECEF <20><><EFBFBD><EFBFBD>ϵ
|
|||
|
|
double x = (N + alt) * Math.Cos(latRad) * Math.Cos(lonRad);
|
|||
|
|
double y = (N + alt) * Math.Cos(latRad) * Math.Sin(lonRad);
|
|||
|
|
double z = ((1 - e2) * N + alt) * Math.Sin(latRad);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD> ECEF <20><><EFBFBD><EFBFBD>
|
|||
|
|
return new Vector3((float)x, (float)y, (float)z);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD> B <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> A <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
|
|||
|
|
public static Vector3 CalculateRelativePosition(double latA, double lonA, double latB, double lonB)
|
|||
|
|
{
|
|||
|
|
// <20><> A <20><> B <20><> WGS84 <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ ECEF <20><><EFBFBD><EFBFBD>
|
|||
|
|
Vector3 ecefA = WGS84ToECEF(latA, lonA);
|
|||
|
|
Vector3 ecefB = WGS84ToECEF(latB, lonB);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD> B <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> A <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
|
|||
|
|
Vector3 delta = ecefB - ecefA;
|
|||
|
|
Vector3 unityPosition = new Vector3(delta.x, delta.z, -delta.y);
|
|||
|
|
|
|||
|
|
return unityPosition;
|
|||
|
|
}
|
|||
|
|
#endregion
|
|||
|
|
}
|
|||
|
|
}
|