/// <summary>
/// Transforms a <see cref="Envelope"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static Envelope TransformBox(Envelope box, IMathTransform transform)
{
if (box == null)
return null;
var corners = new Coordinate[4];
#if PCL
var ll = new Coordinate(box.MinX, box.MinY);
var ur = new Coordinate(box.MaxX, box.MaxY);
var llTrans = transform.Transform(ll);
var urTrans = transform.Transform(ur);
corners[0] = new Coordinate(llTrans.X, llTrans.Y); //lower left
corners[2] = new Coordinate(llTrans.X, urTrans.Y); //upper left
corners[1] = new Coordinate(urTrans.X, urTrans.Y); //upper right
corners[3] = new Coordinate(urTrans.X, llTrans.Y); //lower right
#else
var ll = box.Min().ToDoubleArray();
var ur = box.Max().ToDoubleArray();
var llTrans = transform.Transform(ll);
var urTrans = transform.Transform(ur);
corners[0] = new Coordinate(llTrans[0], llTrans[1]); //lower left
corners[2] = new Coordinate(llTrans[0], urTrans[1]); //upper left
corners[1] = new Coordinate(urTrans[0], urTrans[1]); //upper right
corners[3] = new Coordinate(urTrans[0], llTrans[1]); //lower right
#endif
var result = new Envelope(corners[0]);
for (var i = 1; i < 4; i++)
result.ExpandToInclude(corners[i]);
return result;
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.BoundingBox"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static BoundingBox TransformBox(BoundingBox box, IMathTransform transform)
{
if (box == null)
return null;
Point[] corners = new Point[4];
corners[0] = new Point(transform.Transform(box.Min.ToDoubleArray())); //LL
corners[1] = new Point(transform.Transform(box.Max.ToDoubleArray())); //UR
corners[2] = new Point(transform.Transform(new Point(box.Min.X, box.Max.Y).ToDoubleArray())); //UL
corners[3] = new Point(transform.Transform(new Point(box.Max.X, box.Min.Y).ToDoubleArray())); //LR
BoundingBox result = corners[0].GetBoundingBox();
for (int i = 1; i < 4; i++)
result = result.Join(corners[i].GetBoundingBox());
return result;
}
/// <summary>
/// Transforms a <see cref="Envelope"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static GeoAPI.Geometries.IEnvelope TransformBox(GeoAPI.Geometries.IEnvelope box, IMathTransform transform)
{
if (box == null)
return null;
double[][] corners = new double[4][];
corners[0] = transform.Transform(ToLightStruct(box.MinX, box.MinY)); //LL
corners[1] = transform.Transform(ToLightStruct(box.MaxX, box.MaxY)); //UR
corners[2] = transform.Transform(ToLightStruct(box.MinX, box.MaxY)); //UL
corners[3] = transform.Transform(ToLightStruct(box.MaxX, box.MinY)); //LR
IEnvelope result = GeometryFactory.CreateEnvelope();
foreach (double[] p in corners)
result.ExpandToInclude(p[0], p[1]);
return result;
}
/// <summary>
/// Transforms a <see cref="IEnvelope" /> object.
/// </summary>
/// <param name="box"></param>
/// <param name="transform"></param>
/// <returns></returns>
public static IEnvelope TransformBox(IEnvelope box, IMathTransform transform)
{
if (box == null) return null;
double[][] corners = new double[4][];
corners[0] = transform.Transform(ToArray(box.MinX, box.MinY)); //LL
corners[1] = transform.Transform(ToArray(box.MaxX, box.MaxY)); //UR
corners[2] = transform.Transform(ToArray(box.MinX, box.MaxY)); //UL
corners[3] = transform.Transform(ToArray(box.MaxX, box.MinY)); //LR
IEnvelope result = new Envelope();
foreach (double[] p in corners)
result.ExpandToInclude(p[0], p[1]);
return result;
}
/// <summary>
/// Transforms coordinates of the bounding rectangle.
/// </summary>
/// <param name="box">Rectangle to transform</param>
/// <param name="transform">The transformation to apply</param>
/// <returns>The transformed rectangle</returns>
public static BoundingRectangle TransformBoundingRectangle(BoundingRectangle box, IMathTransform transform)
{
if (box == null)
return null;
ICoordinate[] corners = new ICoordinate[4];
corners[0] = PlanimetryEnvironment.NewCoordinate(transform.Transform(box.Min.Values()));
corners[1] = PlanimetryEnvironment.NewCoordinate(transform.Transform(box.Max.Values()));
corners[2] = PlanimetryEnvironment.NewCoordinate(transform.Transform(PlanimetryEnvironment.NewCoordinate(box.MinX, box.MaxY).Values()));
corners[3] = PlanimetryEnvironment.NewCoordinate(transform.Transform(PlanimetryEnvironment.NewCoordinate(box.MaxX, box.MinY).Values()));
BoundingRectangle result = new BoundingRectangle();
for (int i = 0; i < 4; i++)
result.Join(corners[i]);
return result;
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.BoundingBox"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static BoundingBox TransformBox(BoundingBox box, IMathTransform transform)
{
if (box == null)
return null;
SharpMap.Geometries.Point[] corners = new Point[4];
corners[0] = transform.Transform(box.Min); //LL
corners[1] = transform.Transform(box.Max); //UR
corners[2] = transform.Transform(new Point(box.Min.X, box.Max.Y)); //UL
corners[3] = transform.Transform(new Point(box.Max.X, box.Min.Y)); //LR
BoundingBox result = corners[0].GetBoundingBox();
for (int i = 1; i < 4; i++)
result = result.Join(corners[i].GetBoundingBox());
return result;
}
private static Coordinate Transform(IMathTransform transform, Coordinate input)
{
var ordinates = transform.Transform(input.ToDoubleArray());
if (ordinates.Length == 2)
return new Coordinate(ordinates[0], ordinates[1]);
return new Coordinate(ordinates[0], ordinates[1], ordinates[2]);
}
/// <summary>
/// Transforms a <see cref="Envelope"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static Envelope TransformBox(Envelope box, IMathTransform transform)
{
if (box == null)
return null;
var corners = new [] {
transform.Transform(new Coordinate(box.MinX, box.MinY)),
transform.Transform(new Coordinate(box.MinX, box.MaxY)),
transform.Transform(new Coordinate(box.MaxX, box.MinY)),
transform.Transform(new Coordinate(box.MaxX, box.MaxY)) };
var result = new Envelope(corners[0]);
for (var i = 1; i < 4; i++)
result.ExpandToInclude(corners[i]);
return result;
}
protected void TestDirectTransform(IMathTransform t, double original, double transformated, double sigma)
{
double[] transformado = t.Transform(new double[] { original });
Assert.AreNotEqual(transformado[0], double.NaN);
Assert.AreNotEqual(transformado[0], double.PositiveInfinity);
Assert.AreNotEqual(transformado[0], double.NegativeInfinity);
Assert.AreEqual(transformated, transformado[0], sigma);
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.BoundingBox"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static BoundingBox TransformBox(BoundingBox box, IMathTransform transform)
{
if (box == null)
return null;
Point[] corners = new Point[4];
var ll = box.Min.ToDoubleArray();
var ur = box.Max.ToDoubleArray();
var llTrans = transform.Transform(ll);
var urTrans = transform.Transform(ur);
corners[0] = new Point(llTrans); //lower left
corners[2] = new Point(llTrans[0], urTrans[1]); //upper left
corners[1] = new Point(urTrans); //upper right
corners[3] = new Point(urTrans[0], llTrans[1]); //lower right
BoundingBox result = corners[0].GetBoundingBox();
for (int i = 1; i < 4; i++)
result = result.Join(corners[i].GetBoundingBox());
return result;
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.BoundingBox"/>.
/// </summary>
/// <param name="box">BoundingBox to transform</param>
/// <param name="transform">Math Transform</param>
/// <returns>Transformed object</returns>
public static BoundingBox TransformBox(BoundingBox box, IMathTransform transform)
{
if (box == BoundingBox.Empty)
return BoundingBox.Empty;
Point[] corners = new Point[4];
corners[0] = transform.Transform(box.Min); //LL
corners[1] = transform.Transform(box.Max); //UR
corners[2] = transform.Transform(new Point(box.Min.X, box.Max.Y)); //UL
corners[3] = transform.Transform(new Point(box.Max.X, box.Min.Y)); //LR
BoundingBox result = new BoundingBox(
corners[0].GetBoundingBox(),
corners[1].GetBoundingBox(),
corners[2].GetBoundingBox(),
corners[3].GetBoundingBox());
return result;
}
static void TestGeocentricInverse(IMathTransform t, double x1, double y1, double z1, double x2, double y2, double z2, double sigma)
{
double[] transformado = t.Transform(new double[] { x2, y2, z2 });
Assert.AreNotEqual(transformado[0], double.NaN);
Assert.AreNotEqual(transformado[1], double.NaN);
Assert.AreNotEqual(transformado[2], double.NaN);
Assert.AreNotEqual(transformado[0], double.PositiveInfinity);
Assert.AreNotEqual(transformado[1], double.PositiveInfinity);
Assert.AreNotEqual(transformado[2], double.PositiveInfinity);
Assert.AreNotEqual(transformado[0], double.NegativeInfinity);
Assert.AreNotEqual(transformado[1], double.NegativeInfinity);
Assert.AreNotEqual(transformado[2], double.NegativeInfinity);
Assert.AreEqual(x1, transformado[0], sigma);
Assert.AreEqual(y1, transformado[1], sigma);
Assert.AreEqual(z1, transformado[2], sigma);
}
protected void ExecuteIterations(IMathTransform d, IMathTransform i, double lat, double lon, double sigma)
{
var transformado = new double[] { lat, lon };
bool sw = true;
for (int _i = 0; _i < 1000; _i++)
{
if (sw)
transformado = d.Transform(transformado);
else
transformado = i.Transform(transformado);
Assert.AreNotEqual(transformado[0], double.NaN);
Assert.AreNotEqual(transformado[1], double.NaN);
Assert.AreNotEqual(transformado[0], double.PositiveInfinity);
Assert.AreNotEqual(transformado[1], double.PositiveInfinity);
Assert.AreNotEqual(transformado[0], double.NegativeInfinity);
Assert.AreNotEqual(transformado[1], double.NegativeInfinity);
sw = !sw;
}
Assert.AreEqual(lat, transformado[0], sigma);
Assert.AreEqual(lon, transformado[1], sigma);
}
/// <summary>
/// Function to transform <paramref name="c"/> using <paramref name="transform"/>
/// </summary>
/// <param name="c">The array of coordinates</param>
/// <param name="transform">The transformation</param>
/// <returns>An array of transformed coordinates</returns>
private static Coordinate[] TransformCoordinates(Coordinate[] c, IMathTransform transform)
{
var res = new Coordinate[c.Length];
for (var i = 0; i < c.Length; i++ )
{
var ordinates = transform.Transform(c[i].ToDoubleArray());
res[i] = new Coordinate(ordinates[0], ordinates[1]);
}
return res;
}
/// <summary>
/// Transforms coordinates of the point geometry.
/// </summary>
/// <param name="p">Point to transform</param>
/// <param name="transform">The transformation to apply</param>
/// <returns>The transformed point</returns>
public static PointD TransformPoint(PointD p, IMathTransform transform)
{
return new PointD(transform.Transform(p.CoordsArray()));
}
protected void TestInverseTransform(IMathTransform t, double latitud, double longitud, double x, double y, double sigma)
{
double[] transformado = t.Transform(new double[] { x, y });
Assert.AreNotEqual(transformado[0], double.NaN);
Assert.AreNotEqual(transformado[1], double.NaN);
Assert.AreNotEqual(transformado[0], double.PositiveInfinity);
Assert.AreNotEqual(transformado[1], double.PositiveInfinity);
Assert.AreNotEqual(transformado[0], double.NegativeInfinity);
Assert.AreNotEqual(transformado[1], double.NegativeInfinity);
Assert.AreEqual(latitud, transformado[0], sigma);
Assert.AreEqual(longitud, transformado[1], sigma);
}
/// <summary>
/// Function to transform a <paramref name="c"/> using <paramref name="transform"/>
/// </summary>
/// <param name="c">The coordinate</param>
/// <param name="transform">The transformation</param>
/// <returns>A transformed coordinate</returns>
public static Coordinate TransformCoordinate(Coordinate c, IMathTransform transform)
{
#if PCL
var ordinates = transform.Transform(c);
return new Coordinate(ordinates);
#else
var ordinates = transform.Transform(c.ToDoubleArray());
return new Coordinate(ordinates[0], ordinates[1]);
#endif
}
static void TestInverseTransform(IMathTransform t, double latitud, double longitud, double x, double y, double sigma)
{
double[] transformado = t.Transform(new double[] { x, y });
Assert.AreEqual(longitud, transformado[0], sigma);
Assert.AreEqual(latitud, transformado[1], sigma);
}
public override void SendCommand()
{
if (masterpos.Lat == 0 || masterpos.Lng == 0)
{
return;
}
//Console.WriteLine(DateTime.Now);
//Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt);
int a = 0;
foreach (var port in MainV2.Comports)
{
if (port == Leader)
{
continue;
}
PointLatLngAlt target = new PointLatLngAlt(masterpos);
try
{
//convert Wgs84ConversionInfo to utm
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
int utmzone = (int)((masterpos.Lng - -186.0) / 6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone,
masterpos.Lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] pll1 = { target.Lng, target.Lat };
double[] p1 = trans.MathTransform.Transform(pll1);
double heading = -Leader.MAV.cs.yaw;
double length = offsets[port].length();
var x = ((HIL.Vector3)offsets[port]).x;
var y = ((HIL.Vector3)offsets[port]).y;
// add offsets to utm
p1[0] += x * Math.Cos(heading * deg2rad) - y * Math.Sin(heading * deg2rad);
p1[1] += x * Math.Sin(heading * deg2rad) + y * Math.Cos(heading * deg2rad);
if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane)
{
// project the point forwards gs*5
var gs = port.MAV.cs.groundspeed;
p1[1] += gs * 5 * Math.Cos((-heading) * deg2rad);
p1[0] += gs * 5 * Math.Sin((-heading) * deg2rad);
}
// convert back to wgs84
IMathTransform inversedTransform = trans.MathTransform.Inverse();
double[] point = inversedTransform.Transform(p1);
target.Lat = point[1];
target.Lng = point[0];
target.Alt += ((HIL.Vector3)offsets[port]).z;
if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane)
{
var dist = target.GetDistance(new PointLatLngAlt(port.MAV.cs.lat, port.MAV.cs.lng, port.MAV.cs.alt));
dist -= port.MAV.cs.groundspeed * 5;
var leadergs = Leader.MAV.cs.groundspeed;
var newspeed = (leadergs + (float)(dist / 10));
if (newspeed < 5)
{
newspeed = 5;
}
port.setParam("TRIM_ARSPD_CM", newspeed * 100.0f);
// send position
port.setGuidedModeWP(new Locationwp()
{
alt = (float)target.Alt,
lat = target.Lat,
lng = target.Lng,
id = (ushort)MAVLink.MAV_CMD.WAYPOINT
});
}
else
{
Vector3 vel = new Vector3(Math.Cos(Leader.MAV.cs.groundcourse * deg2rad) * Leader.MAV.cs.groundspeed,
Math.Sin(Leader.MAV.cs.groundcourse * deg2rad) * Leader.MAV.cs.groundspeed, Leader.MAV.cs.verticalspeed);
port.setPositionTargetGlobalInt((byte)port.sysidcurrent, (byte)port.compidcurrent, true, true, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x, vel.y, -vel.z);
}
//Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);
}
catch (Exception ex)
{
Console.WriteLine("Failed to send command " + port.ToString() + "\n" + ex.ToString());
}
a++;
}
}
/// <summary>
/// Transforms a <see cref="MultiPoint" /> object.
/// </summary>
/// <param name="factory"></param>
/// <param name="points"></param>
/// <param name="transform"></param>
/// <returns></returns>
public static IMultiPoint TransformMultiPoint(IGeometryFactory factory,
IMultiPoint points, IMathTransform transform)
{
//We assume the first point holds all the ordinates
var firstPoint = (IPoint) points.GetGeometryN(0);
var ordinateFlags = firstPoint.CoordinateSequence.Ordinates;
var ordinates = OrdinatesUtility.ToOrdinateArray(ordinateFlags);
var coordSequence = factory.CoordinateSequenceFactory.Create(points.NumPoints, ordinateFlags);
for (var i = 0; i < points.NumGeometries; i++)
{
var currPoint = (IPoint) points.GetGeometryN(i);
var seq = currPoint.CoordinateSequence;
foreach (var ordinate in ordinates)
{
double d = seq.GetOrdinate(0, ordinate);
coordSequence.SetOrdinate(i, ordinate, d);
}
}
var transPoints = transform.Transform(coordSequence);
return factory.CreateMultiPoint(transPoints);
}
/// <summary>
/// Transforms a <see cref="Point" /> object.
/// </summary>
/// <param name="factory"></param>
/// <param name="p"></param>
/// <param name="transform"></param>
/// <returns></returns>
public static IPoint TransformPoint(IGeometryFactory factory,
IPoint p, IMathTransform transform)
{
try
{
var transformed = transform.Transform(p.CoordinateSequence);
return factory.CreatePoint(transformed);
}
catch { return null; }
}
/// <summary>
/// Transforms a <see cref="LinearRing" /> object.
/// </summary>
/// <param name="factory"></param>
/// <param name="r"></param>
/// <param name="transform"></param>
/// <returns></returns>
public static ILinearRing TransformLinearRing(IGeometryFactory factory,
ILinearRing r, IMathTransform transform)
{
try
{
var coordSequence = transform.Transform(r.CoordinateSequence);
return factory.CreateLinearRing(coordSequence);
}
catch { return null; }
}
/// <summary>
/// Function to transform a <paramref name="c"/> using <paramref name="transform"/>
/// </summary>
/// <param name="c">The coordinate</param>
/// <param name="transform">The transformation</param>
/// <returns>A transformed coordinate</returns>
public static Coordinate TransformCoordinate(Coordinate c, IMathTransform transform)
{
var ordinates = transform.Transform(c.ToDoubleArray());
return(new Coordinate(ordinates[0], ordinates[1]));
}
/// <summary>
/// Function to transform a <paramref name="c"/> using <paramref name="transform"/>
/// </summary>
/// <param name="c">The coordinate</param>
/// <param name="transform">The transformation</param>
/// <returns>A transformed coordinate</returns>
public static Coordinate TransformCoordinate(Coordinate c, IMathTransform transform)
{
var ordinates = transform.Transform(c.ToDoubleArray());
return new Coordinate(ordinates[0], ordinates[1]);
}
/// <summary>
/// Transforms coordinates of the segment.
/// </summary>
/// <param name="s">Segment to transform</param>
/// <param name="transform">The transformation to apply</param>
/// <returns>The transformed segment</returns>
public static Segment TransformSegment(Segment s, IMathTransform transform)
{
Segment result = new Segment();
result.V1 = PlanimetryEnvironment.NewCoordinate(transform.Transform(s.V1.Values()));
result.V2 = PlanimetryEnvironment.NewCoordinate(transform.Transform(s.V2.Values()));
return result;
}
public override void SendCommand()
{
if (masterpos.Lat == 0 || masterpos.Lng == 0)
{
return;
}
//Console.WriteLine(DateTime.Now);
//Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt);
int a = 0;
foreach (var port in MainV2.Comports)
{
foreach (var mav in port.MAVlist)
{
if (mav == Leader)
{
continue;
}
PointLatLngAlt target = new PointLatLngAlt(masterpos);
try
{
int utmzone = (int)((masterpos.Lng - -186.0) / 6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone,
masterpos.Lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] pll1 = { target.Lng, target.Lat };
double[] p1 = trans.MathTransform.Transform(pll1);
double heading = -Leader.cs.yaw;
double length = offsets[mav].length();
var x = ((HIL.Vector3)offsets[mav]).x;
var y = ((HIL.Vector3)offsets[mav]).y;
// add offsets to utm
p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad);
p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.deg2rad);
if (mav.cs.firmware == MainV2.Firmwares.ArduPlane)
{
// project the point forwards gs*5
var gs = mav.cs.groundspeed * 5;
p1[1] += gs * Math.Cos((-heading) * MathHelper.deg2rad);
p1[0] += gs * Math.Sin((-heading) * MathHelper.deg2rad);
}
// convert back to wgs84
IMathTransform inversedTransform = trans.MathTransform.Inverse();
double[] point = inversedTransform.Transform(p1);
target.Lat = point[1];
target.Lng = point[0];
target.Alt += ((HIL.Vector3)offsets[mav]).z;
if (mav.cs.firmware == MainV2.Firmwares.ArduPlane)
{
var dist =
target.GetDistance(new PointLatLngAlt(mav.cs.lat, mav.cs.lng, mav.cs.alt));
dist -= mav.cs.groundspeed * 5;
var leadergs = Leader.cs.groundspeed;
var newspeed = (leadergs + (float)(dist / 10));
if (newspeed < 5)
{
newspeed = 5;
}
port.setParam(mav.sysid, mav.compid, "TRIM_ARSPD_CM", newspeed * 100.0f);
// send position
port.setGuidedModeWP(mav.sysid, mav.compid, new Locationwp()
{
alt = (float)target.Alt,
lat = target.Lat,
lng = target.Lng,
id = (ushort)MAVLink.MAV_CMD.WAYPOINT
});
}
else
{
Vector3 vel =
new Vector3(Math.Cos(Leader.cs.groundcourse * MathHelper.deg2rad) * Leader.cs.groundspeed,
Math.Sin(Leader.cs.groundcourse * MathHelper.deg2rad) * Leader.cs.groundspeed,
Leader.cs.verticalspeed);
// do pos/vel
port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true,
true, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x,
vel.y, -vel.z);
// do yaw
if (!gimbal)
{
// within 3 degrees dont send
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw,
100.0f, 0, 0, 0, 0, 0, false);
}
}
else
{
// gimbal direction
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false);
}
}
}
//Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);
}
catch (Exception ex)
{
Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString());
}
a++;
}
}
}
private Coordinate[] ToWgs84Coordinates(IEnumerable <LatLng> latLngs)
{
return(latLngs.Select(latLng => _wgs84ItmMathTransform.Transform(new Coordinate {
X = latLng.Lng, Y = latLng.Lat
})).ToArray());
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.Point"/>.
/// </summary>
/// <param name="p">Point to transform</param>
/// <param name="transform">MathTransform</param>
/// <returns>Transformed Point</returns>
public static Point TransformPoint(Point p, IMathTransform transform)
{
try
{
return new Point(transform.Transform(p.ToDoubleArray()));
}
catch
{
return null;
}
}
public override void SendCommand()
{
if (masterpos.Lat == 0 || masterpos.Lng == 0)
{
return;
}
Console.WriteLine(DateTime.Now);
Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt);
int a = 0;
foreach (var port in MainV2.Comports)
{
if (port == Leader)
{
continue;
}
PointLatLngAlt target = new PointLatLngAlt(masterpos);
try
{
//convert Wgs84ConversionInfo to utm
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
int utmzone = (int)((masterpos.Lng - -186.0) / 6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone,
masterpos.Lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] pll1 = { target.Lng, target.Lat };
double[] p1 = trans.MathTransform.Transform(pll1);
// add offsets to utm
p1[0] += ((HIL.Vector3)offsets[port]).x;
p1[1] += ((HIL.Vector3)offsets[port]).y;
// convert back to wgs84
IMathTransform inversedTransform = trans.MathTransform.Inverse();
double[] point = inversedTransform.Transform(p1);
target.Lat = point[1];
target.Lng = point[0];
target.Alt += ((HIL.Vector3)offsets[port]).z;
port.setGuidedModeWP(new Locationwp()
{
alt = (float)target.Alt,
lat = target.Lat,
lng = target.Lng,
id = (byte)MAVLink.MAV_CMD.WAYPOINT
});
Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);
}
catch (Exception ex)
{
Console.WriteLine("Failed to send command " + port.ToString() + "\n" + ex.ToString());
}
a++;
}
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.MultiPoint"/>.
/// </summary>
/// <param name="points">MultiPoint to transform</param>
/// <param name="transform">MathTransform</param>
/// <returns>Transformed MultiPoint</returns>
public static MultiPoint TransformMultiPoint(MultiPoint points, IMathTransform transform)
{
MultiPoint pOut = new MultiPoint(points.Points.Count);
foreach (Point p in points.Points)
pOut.Points.Add(transform.Transform(p));
return pOut;
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.Point"/>.
/// </summary>
/// <param name="p">Point to transform</param>
/// <param name="transform">MathTransform</param>
/// <returns>Transformed Point</returns>
public static Point TransformPoint(Point p, IMathTransform transform)
{
try { return(new Point(transform.Transform(p.ToDoubleArray()))); }
catch { return(null); }
}
/// <summary>
/// Transforms a <see cref="SharpMap.Geometries.Point"/>.
/// </summary>
/// <param name="p">Point to transform</param>
/// <param name="transform">MathTransform</param>
/// <returns>Transformed Point</returns>
public static Point TransformPoint(Point p, IMathTransform transform)
{
try { return transform.Transform(p); }
catch { return null; }
}
public List <Object> CalculateDataTable(string polyfile)
{
//List<GeoAPI.Geometries.IGeometry> polys = new List<GeoAPI.Geometries.IGeometry>();
List <GeoAPI.Geometries.IGeometry> squares = new List <GeoAPI.Geometries.IGeometry>();
ArrayList polys = new ArrayList();
List <Object> infoTable = new List <Object>();
double squareArea = 0;//0.015625;
double gridArea = 0;
double polygonArea = 0;
string catchmentID = "";
//////////////
string gridfile = @"M:\DotSpatTopology\tests\NLDAS_Grid_Reference.shp";//"";
string gridproj = @"M:\DotSpatTopology\tests\NLDAS_Grid_Reference.prj";
//////////////
Guid gid = Guid.NewGuid();
string directory = @"M:\\TransientStorage\\" + gid.ToString() + "\\";
//This block is for getting and setting shapefiles for NLDAS Grid
/**
* client.DownloadFile("https://ldas.gsfc.nasa.gov/nldas/gis/NLDAS_Grid_Reference.zip", @"M:\\TransientStorage\\NLDAS.zip");
* ZipFile.ExtractToDirectory(@"M:\\TransientStorage\\NLDAS.zip", @"M:\\TransientStorage\\NLDAS");
* unzippedLocation = (@"M:\\TransientStorage\\NLDAS");
* foreach (string file in Directory.GetFiles(unzippedLocation))
* {
* if (Path.GetExtension(file).Equals(".shp"))
* {
* gridfile = file;
* }
* else if (Path.GetExtension(file).Equals(".prj"))
* {
* gridproj = file;
* }
* }
* client.Dispose();**/
ShapefileDataReader reader2 = new ShapefileDataReader(gridfile, NetTopologySuite.Geometries.GeometryFactory.Default);
while (reader2.Read())
{
squares.Add(reader2.Geometry);
gridArea += reader2.Geometry.Area;
}
reader2.Dispose();
//if (polyfile.StartsWith(@"{""type"": ""FeatureCollection"""))
if (polyfile.StartsWith(@"{""type"":"))//.geojson
{
Boolean version1 = true;
string[] featureParams = new string[3];
string jsonfile = polyfile;
var readera = new NetTopologySuite.IO.GeoJsonReader();
NetTopologySuite.Features.FeatureCollection result = readera.Read <NetTopologySuite.Features.FeatureCollection>(jsonfile);
if (result[0].Attributes.GetNames().Contains("HUC_8"))
{
version1 = false;
featureParams[0] = "OBJECTID";
featureParams[1] = "HUC_8";
featureParams[2] = "HUC_12";
}
else if (result[0].Attributes.GetNames().Contains("HUC8"))
{
version1 = true;
featureParams[0] = "COMID";
featureParams[1] = "HUC8";
featureParams[2] = "HUC12";
}
else
{
version1 = false;
featureParams[0] = null;
featureParams[1] = null;
featureParams[2] = null;
}
List <Object> huc8Table = new List <Object>();
huc8Table.Add(new KeyValuePair <string, string>("HUC 8 ID: ", result[0].Attributes[featureParams[1]].ToString()));
for (int i = 0; i < result.Count; i++)
{
List <Object> huc12Table = new List <Object>();
if (version1)
{
huc12Table.Add(new KeyValuePair <string, string>("HUC 12 ID: ", null));
}
else
{
huc12Table.Add(new KeyValuePair <string, string>("HUC 12 ID: ", result[i].Attributes["HUC_12"].ToString()));
}
catchmentID = result[i].Attributes[featureParams[0]].ToString();
huc12Table.Add(new KeyValuePair <string, string>("Catchment ID: ", catchmentID));
foreach (GeoAPI.Geometries.IGeometry s in squares)
{
double interArea = 0.0;
squareArea = s.Area;
if (result[i].Geometry.Intersects(s))
{
GeoAPI.Geometries.IGeometry intersection = result[i].Geometry.Intersection(s);
interArea += intersection.Area;
double percent2 = (interArea / squareArea) * 100;
Dictionary <string, string> catchTable = new Dictionary <string, string>();
//catchTable.Add("catchmentID", catchmentID);
catchTable.Add("latitude", s.Centroid.X.ToString());
catchTable.Add("longitude", s.Centroid.Y.ToString());
catchTable.Add("cellArea", squareArea.ToString());
catchTable.Add("containedArea", interArea.ToString());
catchTable.Add("percentArea", percent2.ToString());
huc12Table.Add(catchTable);
}
}
huc8Table.Add(huc12Table);
}
infoTable.Add(huc8Table);
}
else //Huc ID
{
catchmentID = polyfile;
string ending = polyfile + ".zip";
WebClient client = new WebClient();
DirectoryInfo di = Directory.CreateDirectory(directory);
client.DownloadFile("ftp://newftp.epa.gov/exposure/NHDV1/HUC12_Boundries/" + ending, directory + ending);
string projfile = "";
string dataFile = "";
ZipFile.ExtractToDirectory(directory + ending, directory + polyfile);
string unzippedLocation = (directory + polyfile + "\\" + polyfile); //+ "\\NHDPlus" + polyfile + "\\Drainage");
foreach (string file in Directory.GetFiles(unzippedLocation))
{
if (Path.GetExtension(file).Equals(".shp"))
{
polyfile = file;
}
else if (Path.GetExtension(file).Equals(".prj"))
{
projfile = file;
}
else if (Path.GetExtension(file).Equals(".dbf"))
{
dataFile = file;
}
}
//This block is for setting projection parameters of input shapefile and projecting it to NLDAS grid
//Reprojecting of coordinates is not needed for NHDPlus V2
string line = System.IO.File.ReadAllText(projfile);
string[] projParams = { "PARAMETER", @"PARAMETER[""latitude_Of_origin"",0]," };//@"PARAMETER[""false_easting"",0],", @"PARAMETER[""false_northing"",0],", @"PARAMETER[""central_meridian"",0],", @"PARAMETER[""standard_parallel_1"",0],", @"PARAMETER[""standard_parallel_2"",0],", @"PARAMETER[""latitude_Of_origin"",0]," };
int ptr = 0;
foreach (string x in projParams)
{
if (line.Contains(x))
{
ptr = line.IndexOf(x);
}
else if (!line.Contains(x) && !x.Equals("PARAMETER"))
{
line = line.Insert(ptr, x);
}
}
string line2 = System.IO.File.ReadAllText(gridproj);
IProjectedCoordinateSystem pcs = CoordinateSystemWktReader.Parse(line) as IProjectedCoordinateSystem;
IGeographicCoordinateSystem gcs = GeographicCoordinateSystem.WGS84 as IGeographicCoordinateSystem;
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
ICoordinateTransformation transformTo = ctfac.CreateFromCoordinateSystems(pcs, gcs);
IMathTransform inverseTransformTo = transformTo.MathTransform;
//Read geometries from both shapefiles and store in array lists
//As well as calculate shapefile areas ahead of time
ShapefileDataReader reader = new ShapefileDataReader(polyfile, NetTopologySuite.Geometries.GeometryFactory.Default);
while (reader.Read())
{
//Reprojection not needed for NHDPLUSV2
CoordinateList cordlist = new CoordinateList();
foreach (Coordinate coord in reader.Geometry.Coordinates)
{
double[] newCoord = { coord.X, coord.Y };
newCoord = inverseTransformTo.Transform(newCoord);
Coordinate newpt = new Coordinate(newCoord[0], newCoord[1]);
cordlist.Add(newpt);
}
Coordinate[] listofpts = cordlist.ToCoordinateArray();
IGeometryFactory geoFactory = new NetTopologySuite.Geometries.GeometryFactory();
NetTopologySuite.Geometries.LinearRing linear = (NetTopologySuite.Geometries.LinearRing) new GeometryFactory().CreateLinearRing(listofpts);
Polygon projPoly = new Polygon(linear, null, geoFactory);
polys.Add(projPoly);
polygonArea += projPoly.Area;
}
reader.Dispose();
List <Object> huc8Table = new List <Object>();
huc8Table.Add(new KeyValuePair <string, string>("HUC 8 ID: ", catchmentID));
foreach (Polygon p in polys)
{
List <Object> huc12Table = new List <Object>();
huc12Table.Add(new KeyValuePair <string, string>("HUC 12 ID: ", null));
catchmentID = null;//result[i].Attributes["OBJECTID"].ToString();
huc12Table.Add(new KeyValuePair <string, string>("Catchment ID: ", catchmentID));
foreach (GeoAPI.Geometries.IGeometry s in squares)
{
double interArea = 0.0;
squareArea = s.Area;
if (p.Intersects(s))
{
GeoAPI.Geometries.IGeometry intersection = p.Intersection(s);
interArea += intersection.Area;
double percent2 = (interArea / squareArea) * 100;
Dictionary <string, string> catchTable = new Dictionary <string, string>();
//catchTable.Add("catchmentID", catchmentID);
catchTable.Add("latitude", s.Centroid.X.ToString());
catchTable.Add("longitude", s.Centroid.Y.ToString());
catchTable.Add("cellArea", squareArea.ToString());
catchTable.Add("containedArea", interArea.ToString());
catchTable.Add("percentArea", percent2.ToString());
huc12Table.Add(catchTable);
}
}
huc8Table.Add(huc12Table);
}
infoTable.Add(huc8Table);
}
//System.IO.DirectoryInfo del = new DirectoryInfo(directory);
/*
* foreach (FileInfo file in del.GetFiles())
* {
* file.Delete();
* }
* foreach (DirectoryInfo dir in del.GetDirectories())
* {
* dir.Delete(true);
* }*/
//del.Delete(true);
/////
//infoTable.Add(new List<Object>() { elapsedTime, elapsedTime, elapsedTime, elapsedTime });
//////
return(infoTable);
}
private static double[] TestForwardAndBackProjNet(IMathTransform transform, double[] ordinates,
ref int succeed, ref int failed, ref int exception)
{
try
{
var forward = transform.Transform(ordinates);
transform.Invert();
var back = transform.Transform(ordinates);
transform.Invert();
if (Math.Abs(ordinates[0] - back[0]) <= Tolerance &&
Math.Abs(ordinates[1] - back[1]) <= Tolerance)
{
succeed++;
}
else
{
failed++;
}
return forward;
}
catch
{
exception++;
}
return ordinates;
}
static void TestDirectTransform(IMathTransform t, double latitud, double longitud, double x, double y, double sigma)
{
double[] transformado = t.Transform(new double[] { longitud, latitud });
Assert.AreEqual(x, transformado[0], sigma);
Assert.AreEqual(y, transformado[1], sigma);
}
/// <summary>
/// Transforms a <see cref="Point" /> object.
/// </summary>
/// <param name="p"></param>
/// <param name="transform"></param>
/// <returns></returns>
public static IPoint TransformPoint(IPoint p, IMathTransform transform)
{
try
{
double[] point = transform.Transform(ToArray(p.X, p.Y));
return ToNTS(point[0], point[1]);
}
catch { return null; }
}
/// <summary>
///GetFeatureInfo request
/// </summary>
protected override void GetFeatureInfo(NameValueCollection requestParams, Stream responseOutputStream, ref string responseContentType)
{
#region Request processing GetFeatureInfo
int x = 0, y = 0, featureCount = 1;
int row, column;
if (requestParams["FEATURE_COUNT"] != null)
{
int.TryParse(requestParams["FEATURE_COUNT"], out featureCount);
}
if (requestParams["I"] == null)
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Required parameter I undefined.",
responseOutputStream,
ref responseContentType);
return;
}
else if (!int.TryParse(requestParams["I"], out x))
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Parameter I has wrong value.",
responseOutputStream,
ref responseContentType);
return;
}
if (requestParams["J"] == null)
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Required parameter J undefined.",
responseOutputStream,
ref responseContentType);
return;
}
else if (!int.TryParse(requestParams["J"], out y))
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Parameter J has wrong value.",
responseOutputStream,
ref responseContentType);
return;
}
if (requestParams["TILEROW"] == null)
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Required parameter TILEROW undefined.",
responseOutputStream,
ref responseContentType);
return;
}
else if (!int.TryParse(requestParams["TILEROW"], out row))
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Parameter TILEROW has wrong value.",
responseOutputStream,
ref responseContentType);
return;
}
if (requestParams["TILECOL"] == null)
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Required parameter TILECOL undefined.",
responseOutputStream,
ref responseContentType);
return;
}
else if (!int.TryParse(requestParams["TILECOL"], out column))
{
WmtsException(WmtsExceptionCode.NotApplicable,
"Parameter TILECOL has wrong value.",
responseOutputStream,
ref responseContentType);
return;
}
string mimeTypeNeeded = "text/html";
if (requestParams["INFO_FORMAT"] != null)
{
mimeTypeNeeded = requestParams["INFO_FORMAT"];
}
Tile tile = new Tile(_map, (uint)row, (uint)column);
//_description.Tile = tile; //
//tile.PixelSize = _description.GetScaleDenominator(_description.ZoomLevel[tileMatrixName]); //
//tile.ScaleDenominator = _description.GetPixelSize(_description.ZoomLevel[tileMatrixName]); //
BoundingRectangle bbox = tile.BBox;
int width = tile.Width, height = tile.Height;
List <FeatureLayer> queryableLayers = new List <FeatureLayer>();
if (!string.IsNullOrEmpty(requestParams["LAYER"]))
{
string[] layers = requestParams["LAYER"].Split(new[] { ',' });
foreach (string layer in layers)
{
LayerBase l = null;
int i;
for (i = 0; i < _map.Layers.Count; i++)
{
if (string.Equals(_map.Layers[i].Alias, layer,
StringComparison.InvariantCultureIgnoreCase))
{
l = _map.Layers[i];
}
}
if (l == null)
{
WmtsException(WmtsExceptionCode.LayerNotDefined,
"Layer \"" + layer + "\" not found.",
responseOutputStream,
ref responseContentType);
return;
}
else if (!(l is FeatureLayer) || !((FeatureLayer)l).FeaturesSelectable)
{
WmtsException(WmtsExceptionCode.LayerNotQueryable,
"Layer \"" + layer + "\" is not queryable.",
responseOutputStream,
ref responseContentType);
return;
}
else
{
queryableLayers.Add((FeatureLayer)l);
}
}
queryableLayers.Sort(
(FeatureLayer l1, FeatureLayer l2) =>
_map.Layers.IndexOf(l1) > _map.Layers.IndexOf(l2) ? -1 : 1);
List <Feature> selectedFeatures = new List <Feature>();
if (queryableLayers.Count > 0)
{
lock (_syncRoot)
{
// calculate the error of selection of point and line objects
_map.SelectionPointRadius =
_selectionMargin * bbox.Width / width;
double resultX = bbox.Width / width * x + bbox.MinX;
double resultY = bbox.MaxY - bbox.Height / height * y;
ICoordinate point = PlanimetryEnvironment.NewCoordinate(resultX, resultY);
ICoordinate tempPoint = PlanimetryEnvironment.NewCoordinate(x, y);
if (_map.OnTheFlyTransform != null)
{
ICoordinate delta =
PlanimetryEnvironment.NewCoordinate(tempPoint.X + _map.SelectionPointRadius,
tempPoint.Y);
IMathTransform inverseTransform = _map.OnTheFlyTransform.Inverse();
delta =
PlanimetryEnvironment.NewCoordinate(inverseTransform.Transform(delta.Values()));
_map.SelectionPointRadius =
PlanimetryAlgorithms.Distance(
PlanimetryEnvironment.NewCoordinate(
inverseTransform.Transform(tempPoint.Values())), delta);
}
if (queryableLayers[0].Map.OnTheFlyTransform != null)
{
IMathTransform inverseTransform = queryableLayers[0].Map.OnTheFlyTransform.Inverse();
point =
PlanimetryEnvironment.NewCoordinate(inverseTransform.Transform(point.Values()));
}
foreach (LayerBase l in queryableLayers)
{
FeatureLayer fl = l as FeatureLayer;
if (fl != null)
{
Feature feature = null;
fl.SelectObject(point, out feature);
if (feature != null)
{
selectedFeatures.Add(feature);
}
if (selectedFeatures.Count == featureCount)
{
break;
}
}
}
}
}
WmsFeaturesInfoNeededEventArgs args = new WmsFeaturesInfoNeededEventArgs(selectedFeatures,
mimeTypeNeeded,
responseOutputStream,
responseContentType);
OnFeaturesInfoNeeded(args);
responseContentType = args.ResponseContentType;
return;
}
StringBuilder sb = new StringBuilder();
sb.Append("none");
byte[] bytes = Encoding.UTF8.GetBytes(sb.ToString().ToCharArray());
responseOutputStream.Write(bytes, 0, bytes.Length);
responseContentType = "text/html";
#endregion
}