float testCode(MAVState MAV, PointLatLngAlt Location)
{
// this is GeoFenceDist from currentstate
try
{
float disttotal = 99999;
var R = 6371e3;
var currenthash = MAV.fencepoints.GetHashCode();
lock (this)
if (currenthash != listhash)
{
list = MAV.fencepoints
.Where(a => a.Value.command != (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT)
.ChunkByField((a, b, count) =>
{
// these fields types stand alone
if (a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION ||
a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
return(false);
}
if (count >= b.Value.param1)
{
return(false);
}
return(a.Value.command == b.Value.command);
}).ToList();
listhash = currenthash;
}
// check all sublists
foreach (var sublist in list)
{
// process circles
if (sublist.Count() == 1)
{
var item = sublist.First().Value;
if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION)
{
var lla = new PointLatLngAlt(item.x / 1e7,
item.y / 1e7);
var dist = lla.GetDistance(Location);
if (dist < item.param1)
{
return(0);
}
disttotal = (float)Math.Min(dist - item.param1, disttotal);
}
else if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
var lla = new PointLatLngAlt(item.x / 1e7,
item.y / 1e7);
var dist = lla.GetDistance(Location);
if (dist > item.param1)
{
return(0);
}
disttotal = (float)Math.Min(item.param1 - dist, disttotal);
}
}
if (sublist == null || sublist.Count() < 3)
{
continue;
}
if (PolygonTools.isInside(
sublist.CloseLoop().Select(a => new PolygonTools.Point(a.Value.y / 1e7, a.Value.x / 1e7)).ToList(),
new PolygonTools.Point(Location.Lng, Location.Lat)))
{
if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION)
{
return(0);
}
}
else
{
if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION)
{
return(0);
}
}
PointLatLngAlt lineStartLatLngAlt = null;
// check all segments
foreach (var mavlinkFencePointT in sublist.CloseLoop())
{
if (lineStartLatLngAlt == null)
{
lineStartLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
continue;
}
// crosstrack distance
var lineEndLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var lineDist = lineStartLatLngAlt.GetDistance2(lineEndLatLngAlt);
var distToLocation = lineStartLatLngAlt.GetDistance2(Location);
var bearToLocation = lineStartLatLngAlt.GetBearing(Location);
var lineBear = lineStartLatLngAlt.GetBearing(lineEndLatLngAlt);
var angle = bearToLocation - lineBear;
if (angle < 0)
{
angle += 360;
}
var alongline = Math.Cos(angle * MathHelper.deg2rad) * distToLocation;
// check to see if our point is still within the line length
if (alongline < 0 || alongline > lineDist)
{
lineStartLatLngAlt = lineEndLatLngAlt;
continue;
}
var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation;
var dXt = Math.Asin(Math.Sin(distToLocation / R) * Math.Sin(angle * MathHelper.deg2rad)) * R;
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
lineStartLatLngAlt = lineEndLatLngAlt;
}
}
// check also distance from the points - because if we are outside the polygon, we may be on a corner segment
foreach (var sublist in list)
{
foreach (var mavlinkFencePointT in sublist)
{
if (mavlinkFencePointT.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
continue;
}
var pathpoint = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var dXt2 = pathpoint.GetDistance(Location);
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
}
}
return(disttotal);
}
catch
{
return(0);
}
}