public static Vector3 GetUnityPosition(VectorMapPosition vmPos, float exportScale = 1)
{
var inverseConvertedPos = new Vector3((float)vmPos.Ly, (float)vmPos.Bx, (float)vmPos.H);
inverseConvertedPos /= exportScale;
return(VectorMapUtility.GetUnityCoordinate(inverseConvertedPos));
}
//Needs optimization
int FindNearestLaneId(VectorMapPosition refPos, bool preferJunction = false)
{
//find nearest lane set first
float radius = 6.0f / MapAnnotationTool.EXPORT_SCALE_FACTOR; // reduce 6.0 to increase speed if every map annotation is on the ground.
var lnIDs = new List <int>();
for (int i = 0; i < Lanes.Count; i++)
{
var lane = Lanes[i];
var p = Points[DtLanes[lane.DID - 1].PID - 1];
var dist = Mathf.Sqrt(Mathf.Pow((float)(refPos.Bx - p.Bx), 2.0f) + Mathf.Pow((float)(refPos.Ly - p.Ly), 2.0f) + Mathf.Pow((float)(refPos.H - p.H), 2.0f));
if (dist < radius)
{
lnIDs.Add(lane.LnID);
}
}
float min = float.MaxValue;
float jctMin = float.MaxValue;
int retLnId = 0;
int nearestJctLnId = 0;
for (int i = 0; i < lnIDs.Count; i++)
{
bool jct = false;
var lane = Lanes[lnIDs[i] - 1];
if (preferJunction && lane.FLID2 > 0)
{
jct = true;
}
var p = Points[DtLanes[lane.DID - 1].PID - 1];
var dist = Mathf.Sqrt(Mathf.Pow((float)(refPos.Bx - p.Bx), 2.0f) + Mathf.Pow((float)(refPos.Ly - p.Ly), 2.0f) + Mathf.Pow((float)(refPos.H - p.H), 2.0f));
if (dist < min)
{
min = dist;
retLnId = lnIDs[i];
}
if (jct && dist < jctMin)
{
jctMin = dist;
nearestJctLnId = lnIDs[i];
}
}
if (nearestJctLnId != 0)
{
retLnId = nearestJctLnId;
}
return(retLnId);
}
//Needs optimization
public int FindNearestLaneId(VectorMapPosition refPos, bool preferJunction = false)
{
//find nearest lane set first
float radius = 1.5f / exportScaleFactor;
var lnIDs = new List <int>();
for (int i = 0; i < lanes.Count; i++)
{
var lane = lanes[i];
var p = points[dtLanes[lane.DID - 1].PID - 1];
var dist = Mathf.Sqrt(Mathf.Pow((float)(refPos.Bx - p.Bx), 2.0f) + Mathf.Pow((float)(refPos.Ly - p.Ly), 2.0f) + Mathf.Pow((float)(refPos.H - p.H), 2.0f));
if (dist < radius)
{
lnIDs.Add(lane.LnID);
}
}
float min = float.MaxValue;
float jctMin = float.MaxValue;
int retLnIdx = 0;
int nearestJctLnIdx = 0;
for (int i = 0; i < lnIDs.Count; i++)
{
bool jct = false;
var lane = lanes[lnIDs[i] - 1];
if (preferJunction && lane.FLID2 > 0)
{
jct = true;
}
var p = points[dtLanes[lane.DID - 1].PID - 1];
var dist = Mathf.Sqrt(Mathf.Pow((float)(refPos.Bx - p.Bx), 2.0f) + Mathf.Pow((float)(refPos.Ly - p.Ly), 2.0f) + Mathf.Pow((float)(refPos.H - p.H), 2.0f));
if (dist < min)
{
min = dist;
retLnIdx = lnIDs[i];
}
if (jct && dist < jctMin)
{
jctMin = dist;
nearestJctLnIdx = lnIDs[i];
}
}
if (nearestJctLnIdx != 0)
{
retLnIdx = nearestJctLnIdx;
}
return(retLnIdx);
}
int FindNearestLaneId(VectorMapPosition refPos, bool preferJunction = false)
{
float radius = 6.0f / MapAnnotationTool.EXPORT_SCALE_FACTOR; // reduce 6.0 to increase speed if every map annotation is on the ground.
int retLnId = 0;
Lane lane;
if (laneIndex.nearest((float)refPos.Bx, (float)refPos.Ly, (float)refPos.H, radius, (float x, float y, float z, Lane l) => this.laneDist(x, y, z, l), out lane))
{
retLnId = lane.LnID;
}
if (preferJunction && lane.FLID2 == 0)
{
if (laneIndex.nearest((float)refPos.Bx, (float)refPos.Ly, (float)refPos.H, radius, (float x, float y, float z, Lane l) => this.junctionDist(x, y, z, l), out lane))
{
retLnId = lane.LnID;
}
}
return(retLnId);
}
