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椭球体赤道半径
|
||
private const double e2 = 0.00669437999013; // 第一偏心率平方
|
||
|
||
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;
|
||
|
||
// 设置线段顶点数（如两点构成一条直线）
|
||
lineRenderer.positionCount = 2;
|
||
// // 设置宽度（起始和结束宽度）
|
||
lineRenderer.startWidth = 0.1f;
|
||
lineRenderer.endWidth = 0.1f;
|
||
// // 设置颜色（支持渐变）
|
||
lineRenderer.startColor = Color.red;
|
||
lineRenderer.endColor = Color.blue;
|
||
// // 设置材质（需指定Shader）
|
||
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($"此时B1点的坐标为:{relativePos1}", 0);
|
||
// CustomLogger.LogWithTag($"此时B2点的坐标为:{relativePos2}", 0);
|
||
// 设置起点和终点坐标（世界坐标系）
|
||
// lineRenderer.SetPosition(0, relativePos1); // 起点
|
||
// lineRenderer.SetPosition(1, relativePos2);// 终点
|
||
// 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($"此时B点的坐标为:{relativePositionB}", 0);
|
||
// CustomLogger.LogWithTag($"此时C点的坐标为:{relativePostionC}", 0);
|
||
// capsule1.position = relativePositionB;
|
||
// capsule2.position = relativePostionC;
|
||
|
||
|
||
}
|
||
#region WGS2ECEF
|
||
// WGS84转ECEF
|
||
private static Vector3 WGS84ToECEF(double lat, double lon)
|
||
{
|
||
double alt = 10;
|
||
// 将经纬度从度转为弧度
|
||
double latRad = Mathf.Deg2Rad * (float)lat;
|
||
double lonRad = Mathf.Deg2Rad * (float)lon;
|
||
|
||
// 计算大地水准面曲率半径
|
||
double N = R / Math.Sqrt(1 - e2 * Math.Sin(latRad) * Math.Sin(latRad));
|
||
|
||
// 转换为 ECEF 坐标系
|
||
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);
|
||
|
||
// 返回 ECEF 坐标
|
||
return new Vector3((float)x, (float)y, (float)z);
|
||
|
||
}
|
||
|
||
// 计算 B 点相对 A 点的偏移量
|
||
public static Vector3 CalculateRelativePosition(double latA, double lonA, double latB, double lonB)
|
||
{
|
||
// 将 A 和 B 的 WGS84 坐标转换为 ECEF 坐标
|
||
Vector3 ecefA = WGS84ToECEF(latA, lonA);
|
||
Vector3 ecefB = WGS84ToECEF(latB, lonB);
|
||
|
||
// 计算 B 点相对于 A 点的偏移量
|
||
Vector3 delta = ecefB - ecefA;
|
||
Vector3 unityPosition = new Vector3(delta.x, delta.z, -delta.y);
|
||
|
||
return unityPosition;
|
||
}
|
||
#endregion
|
||
}
|
||
}
|